ROS-相机话题-获取图像-颜色目标识别与定位-目标跟随-人脸检测

文章目录

相机话题
获取图像
颜色目标识别与定位
目标跟随
人脸检测

相机话题

启动仿真

roslaunch wpr_simulation wpb_stage_robocup.launch 

 rostopic hz /kinect2/qhd/image_color_rect

/camera/image_raw：原始的、未经处理的图像数据。
/camera/image_rect：经过畸变校正的图像数据。
/camera/image_rect_color：彩色的、经过畸变校正的图像数据。
/camera/camera_info：相机的内参信息，如焦距、主点坐标和畸变系数等。

image_rect_color话题的消息格式就是sensor_msgs/Image

Header header          # 图像的头部信息
uint32 height           # 图像的高度（行数）
uint32 width            # 图像的宽度（列数）
string encoding         # 像素编码格式
uint8 is_bigendian      # 数据是否为大端序
uint32 step             # 每行的字节长度
uint8[] data            # 图像的原始数据

在这里插入图片描述

光线入射
入射光线：光线从物体反射后进入相机镜头，最终到达相机的感光芯片（图像传感器）。
光线包含红（R）、绿（G）、蓝（B）三种颜色的成分。
栅格滤镜
Bayer 栅格滤镜：感光芯片上方覆盖了一层滤镜，称为 Bayer 栅格滤镜。它由红、绿、蓝三种颜色的滤光片以特定的排列方式组成。
滤光片的作用：每个滤光片只允许特定颜色的光线通过：
红色滤光片：只允许红色光线通过。
绿色滤光片：只允许绿色光线通过。
蓝色滤光片：只允许蓝色光线通过。
感光芯片
感光芯片：光线通过滤光片后，照射到感光芯片上。感光芯片由许多光敏单元（像素）组成，每个像素只能检测到一种颜色的光强信息。
光强信息：每个像素根据接收到的光强生成电信号，电信号的强度与光强成正比。
像素阵列
像素阵列：感光芯片上的像素按照一定的排列方式组成一个二维阵列。每个像素存储的是经过滤光片过滤后的光强信息。
颜色分离：由于每个像素只能检测到一种颜色，因此整个图像被分解为红、绿、蓝三个独立的像素阵列：
红色像素阵列：存储红色光强信息。
绿色像素阵列：存储绿色光强信息。
蓝色像素阵列：存储蓝色光强信息。
图像重建
插值算法：为了生成完整的彩色图像，需要对每个像素的颜色信息进行插值。插值算法会根据相邻像素的颜色信息，估计出每个像素的完整 RGB 值。
最终图像：经过插值处理后，每个像素都包含完整的 RGB 信息，从而形成完整的彩色图像。

获取图像

在这里插入图片描述

catkin_create_pkg cv_pkg roscpp rospy cv_bridge

 roslaunch wpr_simulation wpb_balls.launch 
 rosrun cv_pkg cv_image_node
rosrun wpr_simulation ball_random_move

#include<ros/ros.h>
#include<cv_bridge/cv_bridge.h>
//包含 cv_bridge 头文件，用于在 ROS 图像消息（sensor_msgs/Image）和 OpenCV 图像格式（cv::Mat）之间进行转换。
#include<sensor_msgs/image_encodings.h>
//包含 sensor_msgs 中的图像编码格式定义，例如 BGR8、RGB8 等
#include<opencv2/imgproc/imgproc.hpp>
//包含 OpenCV 的图像处理模块头文件，提供图像处理功能，如图像滤波、边缘检测等
#include<opencv2/highgui/highgui.hpp>
//包含 OpenCV 的高级用户界面模块头文件，提供图像显示和窗口管理功能。
using namespace cv;
//使用 OpenCV 的命名空间 cv，避免在代码中多次使用 cv:: 前缀。
void Cam_RGB_Callback(const sensor_msgs::Image msg)
{
    cv_bridge::CvImagePtr cv_ptr;
    try
    {   //将 ROS 的图像消息（sensor_msgs::Image）转换为 OpenCV 的图像格式（cv::Mat）。
        //BGR8 是一种图像编码格式，表示每个像素由 3 个字节组成，分别对应蓝（Blue）、绿（Green）、红（Red）三个颜色通道，每个通道 8 位（1 字节）
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch(cv_bridge::Exception& e)
    {
        ROS_ERROR("Could not convert from '%s' to 'bgr8'.", e.what());
        return;
    }
    //cv_bridge::CvImagePtr 对象中获取 OpenCV 的 cv::Mat 图像数据
    Mat imageOriginal=cv_ptr->image;
    //作用：在窗口中显示图像。
    // "RGB_Image"：窗口名称。
    // imageOriginal：要显示的图像数据。
    imshow("RGB_Image", imageOriginal);
    //作用：确保图像窗口能够有足够时间显示。
    waitKey(3);
}
int main(int argc, char** argv)
{
    ros::init(argc, argv, "cv_image_node");
    ros::NodeHandle nh;
    //kinect2含义：表示这是 Kinect 2 相机的数据话题。
    //qhd含义：表示 Quarter HD（四分之一高清）分辨率。
    ros::Subscriber rgb_sub = nh.subscribe("/kinect2/qhd/image_color_rect", 1, Cam_RGB_Callback);
    namedWindow("RGB_Image");

    ros::spin();
}

颜色目标识别与定位

在这里插入图片描述

RGB（Red, Green, Blue）颜色空间是一种基于光的加色模型，通过红、绿、蓝三种颜色光的不同强度组合来表示颜色。

立方体结构：
Red（红色）：表示红色光的强度，从 0 到 255。
Green（绿色）：表示绿色光的强度，从 0 到 255。
Blue（蓝色）：表示蓝色光的强度，从 0 到 255。
颜色表示：
每个颜色由三个分量组成，例如 (R, G, B)。
例如，纯红色为 (255, 0, 0)，纯绿色为 (0, 255, 0)，纯蓝色为 (0, 0, 255)。

在这里插入图片描述
HSV（Hue, Saturation, Value）颜色空间是一种基于人类视觉感知的颜色模型，
圆锥结构：

Hue（色调）：表示颜色的类型，通常用角度表示，范围为 0° 到 360°。
0° 或 360°：红色
120°：绿色
240°：蓝色
Saturation（饱和度）：表示颜色的纯度，范围为 0 到 100%。
0%：灰色（没有颜色）
100%：最纯的颜色
Value（亮度）：表示颜色的明暗程度，范围为 0 到 100%。
0%：黑色
100%：最亮的颜色

在这里插入图片描述

#include<ros/ros.h>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/image_encodings.h>
#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2/highgui/highgui.hpp>

using namespace cv;
using namespace std;

static int iLowH = 10;
static int iHighH = 40;

static int iLowS = 90; 
static int iHighS = 255;

static int iLowV = 1;
static int iHighV = 255;

void Cam_RGB_Callback(const sensor_msgs::Image msg)
{
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr= cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch(cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    Mat imgOriginal = cv_ptr->image;


    //创建一个 cv::Mat 对象 imgHSV，用于存储转换后的 HSV 图像
    Mat imgHSV;
    //将原始图像 imgOriginal 从 BGR 颜色空间转换为 HSV 颜色空间，并存储到 imgHSV 中。
    cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV);
    //创建一个 std::vector<cv::Mat> 对象 hsvSplit，用于存储 HSV 图像的三个通道（H、S、V）。
    vector<Mat> hsvSplit;
    //将 imgHSV 图像的三个通道（H、S、V）分离，并存储到 hsvSplit 中
    split(imgHSV, hsvSplit);
    //equalizeHist 是 OpenCV 中用于对单通道图像进行直方图均衡化（Histogram Equalization）的函数。
    //对 hsvSplit[2]（V 通道，即亮度通道）进行直方图均衡化，并将结果存储回 hsvSplit[2] 
    //将输入图像的直方图拉伸为均匀分布，从而增强图像的对比度。
    equalizeHist(hsvSplit[2], hsvSplit[2]);
    //将多个单通道图像合并为一个多通道图像的函数。
    //将处理后的 HSV 通道（H、S、V）重新合并为一个三通道图像，并存储到 imgHSV 中。
    merge(hsvSplit, imgHSV);

    Mat imgThresholded;
    inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded);
    //创建一个 5x5 的矩形结构元素（kernel）。
    // MORPH_RECT：表示矩形形状。
    // Size(5, 5)：表示结构元素的大小为 5x5
    Mat element=getStructuringElement(MORPH_RECT, Size(5, 5));
    // 对 imgThresholded 图像进行 开运算。
    // 开运算：先腐蚀后膨胀。开运算：白色噪声被去除，图像的亮区域变得更加平滑。
    // 腐蚀：去除图像中的小亮斑（白色噪声）。
    // 膨胀：恢复图像中的主要亮区域。
    morphologyEx(imgThresholded, imgThresholded, MORPH_OPEN, element);
    // 作用：对 imgThresholded 图像进行 闭运算。
    // 闭运算：先膨胀后腐蚀。闭运算：黑色噪声被填补，图像的暗区域变得更加平滑。
    // 膨胀：填补图像中的小暗斑（黑色噪声）。
    // 腐蚀：恢复图像中的主要暗区域。
    morphologyEx(imgThresholded, imgThresholded, MORPH_CLOSE, element);

    int nTargetX = 0;
    int nTargetY = 0;
    int nPixCount = 0;
    int nImgWidth = imgThresholded.cols;
    int nImgHeight = imgThresholded.rows;
    int nImgChannels = imgThresholded.channels();
    for(int i=0; i<nImgHeight; i++)
    {
        for(int j=0; j<nImgWidth; j++)
        {
            if(imgThresholded.data[i*nImgWidth+j] == 255)
            {
                nTargetX += j;
                nTargetY += i;
                nPixCount++;
            }
        }
    }

    if(nPixCount > 0)
    {
        nTargetX /= nPixCount; 
        nTargetY /= nPixCount;
        printf("颜色质心坐标（ %d , %d）点数 = %d \n", nTargetX, nTargetY,nPixCount);

        //line_begin：定义了第一条线段的起点，坐标为 (nTargetX-10, nTargetY)。
        //line_end：定义了第一条线段的终点，坐标为 (nTargetX+10, nTargetY)。
        Point line_begin = Point(nTargetX-10, nTargetY);
        Point line_end = Point(nTargetX+10, nTargetY);
        //imgOriginal 图像上绘制一条从 line_begin 到 line_end 的红色线段。
        line(imgOriginal, line_begin, line_end, Scalar(0, 0, 255));
        //两点定义了一条垂直线段，长度为 20 个像素
        line_begin.x = nTargetX;
        line_begin.y = nTargetY-10;
        line_end.x = nTargetX;
        line_end.y = nTargetY+10;
        line(imgOriginal, line_begin, line_end, Scalar(0, 0, 255));
    }
    else
    {
        printf("未检测到目标！\n");
    }
    imshow("RGB", imgOriginal);
    imshow("HSV", imgHSV);
    imshow("Result", imgThresholded);
    cv::waitKey(5);
}

int main (int argc, char** argv)
{
    ros::init(argc, argv, "cv_hsv_node");
    ros::NodeHandle nh;
    ros::Subscriber rgb_sub = nh.subscribe("kinect2/qhd/image_color_rect", 1, Cam_RGB_Callback);
    namedWindow("Threshold", WINDOW_AUTOSIZE);
    createTrackbar("LowH", "Threshold", &iLowH, 179); //Hue (0 - 179) 缩小一半
    createTrackbar("HighH", "Threshold", &iHighH, 179);
    createTrackbar("LowS", "Threshold", &iLowS, 255);//Saturation (0 - 255)
    createTrackbar("HighS", "Threshold", &iHighS, 255);
    createTrackbar("LowV", "Threshold", &iLowV, 255);//Value (0 - 255)
    createTrackbar("HighV", "Threshold", &iHighV, 255);

    namedWindow("RGB");
    namedWindow("HSV");
    namedWindow("Result");

    ros::Rate loop_rate(30);
    while(ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();
    }
}

目标跟随

识别的物体的质心与中点的距离作为移动速度和旋转速度

#include<ros/ros.h>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/image_encodings.h>
#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<geometry_msgs/Twist.h>

using namespace cv;
using namespace std;


static int iLowH = 10;
static int iHighH = 40;

static int iLowS = 90; 
static int iHighS = 255;

static int iLowV = 1;
static int iHighV = 255;

geometry_msgs::Twist vel_cmd;
ros::Publisher vel_pub;


void Cam_RGB_Callback(const sensor_msgs::Image msg)
{
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr= cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch(cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    Mat imgOriginal = cv_ptr->image;

    Mat imgHSV;
    vector<Mat> hsvSplit;
    cvtColor(imgOriginal, imgHSV, CV_BGR2HSV);

    split(imgHSV, hsvSplit);
    equalizeHist(hsvSplit[2], hsvSplit[2]);
    merge(hsvSplit, imgHSV);

    Mat imgThresholded;
    inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded);

    Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));
    morphologyEx(imgThresholded, imgThresholded, MORPH_OPEN, element); 
    morphologyEx(imgThresholded, imgThresholded, MORPH_CLOSE, element);

    int nTargetX = 0;
    int nTargetY = 0;
    int nPixCount = 0;
    int nImgWidth = imgThresholded.cols;
    int nImgHeight = imgThresholded.rows;
    int nImgChannels = imgThresholded.channels();
    for(int i=0; i<nImgHeight; i++)
    {
        for(int j=0; j<nImgWidth; j++)
        {
            if(imgThresholded.data[i*nImgWidth+j] == 255)
            {
                nTargetX += j;
                nTargetY += i;
                nPixCount++;
            }
        }
    }

    if(nPixCount > 0)
    {
        nTargetX /= nPixCount;
        nTargetY /= nPixCount;
        printf("颜色质心坐标（ %d , %d）点数 = %d \n", nTargetX, nTargetY,nPixCount);nImgWidth;
        Point line_begin = Point(nTargetX-10,nTargetY);
        Point line_end = Point(nTargetX+10,nTargetY);
        line(imgOriginal,line_begin,line_end,Scalar(255,0,0));
        line_begin.x = nTargetX; line_begin.y = nTargetY-10; 
        line_end.x = nTargetX; line_end.y = nTargetY+10; 
        line(imgOriginal,line_begin,line_end,Scalar(255,0,0));

        float fVeFoward=(nImgHeight/2-nTargetY)*0.002; //差值越大，移动速度越大
        float fVelTurn=(nImgWidth/2-nTargetX)*0.002;  //差值越大，旋转速度越大
        vel_cmd.linear.x = fVeFoward;
        vel_cmd.linear.y = 0;
        vel_cmd.linear.z = 0;
        vel_cmd.angular.x = 0;
        vel_cmd.angular.y = 0;
        vel_cmd.angular.z= fVelTurn;
    }
    else
    {
        printf("未检测到颜色\n");
        vel_cmd.linear.x = 0;
        vel_cmd.linear.y = 0;
        vel_cmd.linear.z = 0;
        vel_cmd.angular.x = 0;
        vel_cmd.angular.y = 0;
        vel_cmd.angular.z= 0;
    }
    vel_pub.publish(vel_cmd);
    printf("移动速度：%f,%f,%f 旋转速度：%f,%f,%f\n",vel_cmd.linear.x,vel_cmd.linear.y,vel_cmd.linear.z,vel_cmd.angular.x,vel_cmd.angular.y,vel_cmd.angular.z);
    imshow("RGB", imgOriginal);
    imshow("Result", imgThresholded);
    cv::waitKey(1);
}
int main(int argc, char** argv)
{   
    ros::init(argc, argv, "cv_follow_node");
    ros::NodeHandle nh;
    ros::Subscriber rgb_sub = nh.subscribe("kinect2/qhd/image_color_rect", 1, Cam_RGB_Callback); 
    vel_pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
    namedWindow("Threshold", WINDOW_AUTOSIZE);

    createTrackbar("LowH", "Threshold", &iLowH, 179); //Hue (0 - 179) 缩小一半
    createTrackbar("HighH", "Threshold", &iHighH, 179);
    createTrackbar("LowS", "Threshold", &iLowS, 255);//Saturation (0 - 255)
    createTrackbar("HighS", "Threshold", &iHighS, 255);
    createTrackbar("LowV", "Threshold", &iLowV, 255);//Value (0 - 255)
    createTrackbar("HighV", "Threshold", &iHighV, 255);

    namedWindow("RGB");
    namedWindow("Result");

    ros::spin();


}

人脸检测

roslaunch wpr_simulation wpr1_single_face.launch 
 rosrun cv_pkg cv_face_detect
rosrun wpr_simulation keyboard_vel_ctrl

#include<ros/ros.h>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/image_encodings.h>
#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/objdetect/objdetect.hpp>

using namespace std;
using namespace cv;


static CascadeClassifier face_cascade;//cv::CascadeClassifier 对象，用于加载 Haar 特征分类器文件。

static Mat frame_gray; //存储黑白图像
static vector<Rect> faces;

static vector<Rect>::const_iterator face_iter;

void CallbackRGB(const sensor_msgs::ImageConstPtr& msg)
{
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
        cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch( cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    Mat imgOriginal = cv_ptr->image;
    //将原始图像转换为灰度图像，因为人脸检测通常在灰度图像上进行
    cvtColor(imgOriginal, frame_gray, CV_BGR2GRAY);
    equalizeHist(frame_gray, frame_gray);
    //detectMultiScale：CascadeClassifier 类的成员函数，用于检测图像中的多尺度特征（如人脸)
    // frame_gray：输入的灰度图像。
    // faces：输出的矩形框数组，表示检测到的人脸区域。
    // 1.1：尺度因子，表示每次图像缩放的比例。
    // 2：最小邻居数，表示每个候选区域的最小邻居数。
    // 0|CASCADE_SCALE_IMAGE：检测选项，包括图像缩放。
    // Size(30, 30)：最小检测窗口大小，表示检测到的人脸的最小尺寸。
    face_cascade.detectMultiScale(frame_gray, faces, 1.1, 2, 0|CASCADE_SCALE_IMAGE, Size(30, 30));

    if(faces.size()>0)
    {   
        //遍历检测到的所有人脸区域。
        for(face_iter = faces.begin(); face_iter != faces.end(); ++face_iter)
        {   
            // imgOriginal：原始图像，用于绘制矩形框。
            // Point(face_iter->x, face_iter->y)：矩形框的左上角点。
            // Point(face_iter->x + face_iter->width, face_iter->y + face_iter->height)：矩形框的右下角点。
            // CV_RGB(255, 0, 255)：矩形框的颜色，表示为 RGB 值，这里是紫色。
            // 2：矩形框的边框厚度。
            rectangle(
                imgOriginal, 
                Point(face_iter->x , face_iter->y), 
                Point(face_iter->x+face_iter->width, face_iter->y+face_iter->height), 
                CV_RGB(255, 0, 255),
                2);
        }
    }
    imshow("face", imgOriginal);
    waitKey(1);
}
int main(int argc, char** argv)
{
    ros::init(argc, argv, "cv_face_detect");
    namedWindow("face");
    
    std::string strLoadFile;
    char const* home = getenv("HOME");
    strLoadFile = home;
    strLoadFile += "/AI_Study_Note/Embodied-AI/ROS/catkin_ws/src";
    //haarcascade_frontalface_alt.xml 文件是一个 XML 文件，包含了用于人脸检测的 Haar 特征分类器的模型参数
    strLoadFile += "/wpr_simulation/config/haarcascade_frontalface_alt.xml";

    bool res=face_cascade.load(strLoadFile);
    if(res==false)
    {
        ROS_ERROR("Load haarcascade_frontalface_alt.xml failed!");
        return 0;
    }
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/kinect2/qhd/image_color_rect", 1, CallbackRGB);
    ros::spin();
    return 0;
}