#include <ros/docs.h>                // ROS头文件
#include <cv_bridge/cv_bridge.h>    // 转换图像格式的头文件
#include <sensor_msgs/image_encodings.h> // 图像编码格式的头文件
#include <opencv2/imgproc/imgproc.hpp>   // OpenCV图像处理函数头文件
#include <opencv2/highgui/highgui.hpp>   // OpenCV图形化显示函数头文件

2. 引入命名空间

using namespace cv;

3. 回调函数

void Cam_RGB_Callback(const sensor_msgs::Image msg) {
    cv_bridge::CvImagePtr cv_ptr; // 定义一个opencv图像类型指针
    try {
        // 将ROS的数据格式转换为OpenCv的数据格式 (BGR8)
        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; // 获取图像矩阵
    imshow("RGB", imgOriginal);      // 显示图像
    waitKey(1);                      // 暂停片刻，给imshow留出时间
}

4. 主函数

int main(int argc, char** argv) {
    ros::init(argc, argv, "cv_image_node"); // 初始化ROS节点
    ros::NodeHandle nh;                     // 创建节点句柄
    
    // 订阅话题: /kinect2/qhd/image_color_rect
    ros::Subscriber rgb_sub = nh.subscribe("/kinect2/qhd/image_color_rect", 1, Cam_RGB_Callback);
    
    namedWindow("RGB"); // 创建显示窗口
    ros::spin();        // 循环等待回调
    return 0;
}

四、颜色目标定位方法

定位是在识别目标区域后，获取其在图像坐标系或世界坐标系中的具体位置。

1. 图像坐标系定位

质心定位：计算目标掩码区域的像素重心 $(x, y)$ $(x, y)$ 。
- 公式： $x = \sum x_i / S$ ， $y = \sum y_i / S$ （ $S$ 为目标区域像素总数）。
- 适用：规则或不规则目标。
边界框定位：获取目标区域的最小外接矩形，输出左上角 $(x_1, y_1)$ 和右下角 $(x_2, y_2)$ 坐标。

2. 世界坐标系定位（进阶）

相机标定：通过棋盘格等标定板，获取相机的内参（焦距、主点坐标）和外参。
坐标转换：利用标定参数，将图像坐标系中的目标坐标转换为世界坐标系（米、厘米）的三维位置 $(x, y, z)$ 。

3. 定位精度优化

提高相机分辨率，减少量化误差。
优化目标区域（形态学操作），避免区域残缺导致质心偏移。
固定相机与目标距离，减少景深变化影响。

五、代码实现：颜色识别与质心定位

1. 完整代码示例

#include <ros/docs.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;

// HSV 阈值变量 (可通过 Trackbar 动态调整)
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;
    
    // 1. RGB 转 HSV
    Mat imgHSV;
    vector<Mat> hsvSplit;
    cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV);
    
    // 2. HSV 直方图均衡化 (增强对比度)
    split(imgHSV, hsvSplit);
    equalizeHist(hsvSplit[2], hsvSplit[2]); // 仅对 V 通道均衡化
    merge(hsvSplit, imgHSV);

    // 3. 阈值分割 (二值化)
    Mat imgThresholded;
    inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded);

    // 4. 形态学操作
    Mat element = getStructuringElement(MORPH_RECT, Size(5, 5));
    morphologyEx(imgThresholded, imgThresholded, MORPH_OPEN, element);   // 开运算去噪
    morphologyEx(imgThresholded, imgThresholded, MORPH_CLOSE, element);  // 闭运算连接连通域

    // 5. 计算质心
    int nTargetX = 0;
    int nTargetY = 0;
    int nPixCount = 0;
    int nImgWidth = imgThresholded.cols;
    int nImgHeight = imgThresholded.rows;

    for (int y = 0; y < nImgHeight; y++) {
        for (int x = 0; x < nImgWidth; x++) {
            if (imgThresholded.data[y * nImgWidth + x] == 255) {
                nTargetX += x;
                nTargetY += y;
                nPixCount++;
            }
        }
    }

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

        // 在原始图像上画十字标记质心
        Point line_begin, line_end;
        
        // 水平线
        line_begin = Point(nTargetX - 10, nTargetY);
        line_end = Point(nTargetX + 10, nTargetY);
        line(imgOriginal, line_begin, line_end, Scalar(255, 0, 0)); // 蓝色
        
        // 垂直线
        line_begin = Point(nTargetX, nTargetY - 10);
        line_end = Point(nTargetX, nTargetY + 10);
        line(imgOriginal, line_begin, line_end, Scalar(255, 0, 0)); // 蓝色
    } else {
        printf("目标颜色消失...\n");
    }

    // 6. 显示结果
    imshow("RGB", imgOriginal);
    imshow("HSV", imgHSV);
    imshow("Result", imgThresholded);
    cv::waitKey(5);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "demo_cv_hsv");
    ros::NodeHandle nh;
    
    // 订阅 Kinect2 相机话题
    ros::Subscriber rgb_sub = nh.subscribe("kinect2/qhd/image_color_rect", 1, Cam_RGB_Callback);
    ros::Rate loop_rate(30);

    // 创建 Trackbar 窗口用于动态调整阈值
    namedWindow("Threshold", WINDOW_AUTOSIZE);
    createTrackbar("LowH", "Threshold", &iLowH, 179);
    createTrackbar("HighH", "Threshold", &iHighH, 179);
    createTrackbar("LowS", "Threshold", &iLowS, 255);
    createTrackbar("HighS", "Threshold", &iHighS, 255);
    createTrackbar("LowV", "Threshold", &iLowV, 255);
    createTrackbar("HighV", "Threshold", &iHighV, 255);

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

    while (ros::ok()) {
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}

六、学习重点

1. 核心重点

掌握 HSV 色彩空间的阈值调整技巧，这是颜色识别的关键。
理解图像坐标系与像素计算逻辑，确保定位结果准确。
熟练运用形态学操作（开运算、闭运算）优化目标区域，提升算法稳定性。

2. 注意事项

避免在强光照变化环境下直接使用 RGB 空间识别，优先选择 HSV 空间。
处理动态目标时，需匹配相机帧率与目标运动速度，避免拖影导致定位偏差。
实际应用中需结合场景调整参数，必要时采用自适应阈值算法（如 Otsu 阈值）替代手动阈值。