目录

前言

一、背景知识

Opencv轮廓检测

ROS相关知识

二、环境依赖

三、具体实现

Step1：初始化ROS，订阅话题

Step2：接收话题，进入回调

1. 帧处理 

2. 膨胀腐蚀处理

Step3：红绿特征处理

1. 提取绘制轮廓

2. 转换矩形、排序

3. 显示检测结果

四、完整代码

五、使用方法

CMakeLists.txt

 package.xml

detect.launch

六、后续改进思路 

---

前言

根据需求需要使用Opencv实现红绿灯检测的功能，于是在猿力猪大佬的【OpenCV】红绿灯识别 轮廓识别 C++ OpenCV 案例实现 文章的基础上，将Opencv 3中的写法改成了Opencv 4，在具体图片处理的部分也按照我自己的逻辑进行了一些改动，并写成ROS工作空间包含了完整的话题读取，图片处理及检测结果显示。

一、背景知识

Opencv轮廓检测

这个部分主要用到Opencv中的findContours函数，具体介绍可以参考：findContours函数参数详解，关于轮廓检测的基本概念参考上面提到的猿力猪大佬的博文即可。

ROS相关知识

ROS编译方式：[详细教程]使用ros编译运行自己写的程序

ROS多节点运行：ROS中的roslaunch命令和launch文件（ROS入门学习笔记四）

ROS话题订阅：ROS消息发布（publish）与订阅(subscribe)（C++代码详解）

二、环境依赖

• OpenCV 4
• cv_bridge

三、具体实现

Step1：初始化ROS，订阅话题

int main(int argc, char **argv)
{
    ros::init(argc, argv, "tld_cv_node");
    ros::NodeHandle nh;

    std::string image_topic;
    nh.param<std::string>("sub_topic", image_topic, "/src_rgb/compressed");
    std::cout << "image_topic: " << image_topic << std::endl;

    ros::Subscriber camera_sub =
        nh.subscribe(image_topic, 2, camera_callback);
    ros::spin();
    ros::waitForShutdown();
    return 0;
}

Step2：接收话题，进入回调

1. 帧处理 

• 从图像话题中读取图像并转换为BGR格式，调整亮度，而后转为YCrCb格式，提取ROI，根据红绿阈值拆分红色和绿色分量

cv_bridge::CvImagePtr cv_ptr =
            cv_bridge::toCvCopy(msg_pic, sensor_msgs::image_encodings::BGR8);
        if (rotated)
        {
            cv::flip(cv_ptr->image, src_image, -1);
        }
        else
        {
            src_image = cv_ptr->image;
        }
        // std::cout << "src_image" << src_image.size() << std::endl;

        // 亮度参数
        double a = 0.3;
        double b = (1 - a) * 125;

        // 统计检测用时
        clock_t start, end;

        start = clock();

        src_image.copyTo(frame);
        // 调整亮度
        src_image.convertTo(img, img.type(), a, b);
        // cv::imshow("img",img);

        // 使用ROI（感兴趣区域）方式截取图像
        cv::Rect roi(0, 0, 2048, 768); // 定义roi，图片尺寸2048*1536
        // std::cout << "img size:" << img.size() << std::endl;
        cv::Mat roi_image = img(roi);

        // 转换为YCrCb颜色空间
        cvtColor(roi_image, imgYCrCb, cv::COLOR_BGR2YCrCb);
        // cvtColor(img, imgYCrCb, cv::COLOR_BGR2YCrCb);
        imgRed.create(imgYCrCb.rows, imgYCrCb.cols, CV_8UC1);
        imgGreen.create(imgYCrCb.rows, imgYCrCb.cols, CV_8UC1);

        // 分解YCrCb的三个成分
        std::vector<cv::Mat> planes;
        split(imgYCrCb, planes);

        // 遍历以根据Cr分量拆分红色和绿色
        cv::MatIterator_<uchar> it_Cr = planes[1].begin<uchar>(),
                                it_Cr_end = planes[1].end<uchar>();
        cv::MatIterator_<uchar> it_Red = imgRed.begin<uchar>();
        cv::MatIterator_<uchar> it_Green = imgGreen.begin<uchar>();

        for (; it_Cr != it_Cr_end; ++it_Cr, ++it_Red, ++it_Green)
        {
            // RED, 145<Cr<470 红色
            // if (*it_Cr > 145 && *it_Cr < 470)
            if (*it_Cr > 140 && *it_Cr < 470)
                *it_Red = 255;
            else
                *it_Red = 0;

            // GREEN 95<Cr<110 绿色
            if (*it_Cr > 95 && *it_Cr < 110)
                *it_Green = 255;
            else
                *it_Green = 0;
            // YELLOW 黄色
        }

PS：ROI选取这里只是随意截取了图片的上半部分。

2. 膨胀腐蚀处理

• 膨胀的第三个参数：膨胀操作的内核，我根据实际场景的检测效果进行了修改

// 膨胀和腐蚀
        dilate(imgRed, imgRed, cv::Mat(8, 8, CV_8UC1), cv::Point(-1, -1));
        erode(imgRed, imgRed, cv::Mat(1, 1, CV_8UC1), cv::Point(-1, -1));
        dilate(imgGreen, imgGreen, cv::Mat(12, 12, CV_8UC1), cv::Point(-1, -1));
        erode(imgGreen, imgGreen, cv::Mat(1, 1, CV_8UC1), cv::Point(-1, -1));

Step3：红绿特征处理

• 这是我改动最大的一个函数，只保留了原作者提取轮廓转换为矩形的思路。先提取、绘制轮廓、显示检测结果，然后对得到的矩形进行位置排序，再对轮廓依次进行显示。

1. 提取绘制轮廓

// 提取轮廓
    findContours(tmp_Red, contours_Red, hierarchy_Red, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
    findContours(tmp_Green, contours_Green, hierarchy_Green, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);

    // 绘制轮廓
    drawContours(frame, contours_Red, -1, cv::Scalar(0, 0, 255), cv::FILLED); // Red
    std::cout << "提取到的红色轮廓数量：" << contours_Red.size() << std::endl;
    drawContours(frame, contours_Green, -1, cv::Scalar(0, 255, 0), cv::FILLED); // Green
    std::cout << "提取到的绿色轮廓数量：" << contours_Green.size() << std::endl;

    // 显示轮廓
    //  imshow("contours", frame);

    trackBox_Red = new cv::Rect[contours_Red.size()];
    trackBox_Green = new cv::Rect[contours_Green.size()];

2. 转换矩形、排序

• 此处需特别注意trackBox指针的清空
• 对结构体的排序方式参考了用sort对结构体某个元素排序的方法

// 确定要跟踪的区域
    for (int i = 0; i < contours_Red.size(); i++)
    {
        // Yi opencv4 不支持 CvSeq
        trackBox_Red[i] = cv::boundingRect(contours_Red[i]);
    }

    for (int i = 0; i < contours_Green.size(); i++)
    {
        // Yi opencv4 不支持 CvSeq
        trackBox_Green[i] = cv::boundingRect(contours_Green[i]);
    }

    // imshow("contours", tmp);

    // Rect.tl() 返回矩形左上顶点的坐标
    for (int i = 0; i < contours_Red.size(); i++)
    {
        Store_x_color a;
        a.x = trackBox_Red[i].tl().x;
        a.y = trackBox_Red[i].tl().y;
        a.color = 0;
        x_color.push_back(a);
    }

    for (int i = 0; i < contours_Green.size(); i++)
    {
        Store_x_color a;
        a.x = trackBox_Green[i].tl().x;
        a.y = trackBox_Green[i].tl().y;
        a.color = 1;
        x_color.push_back(a);
    }

    // 清空指针
    delete[] trackBox_Red;
    delete[] trackBox_Green;

    // 对左上顶点横坐标进行排序
    sort(x_color.begin(), x_color.end(), CompareByX);

3. 显示检测结果

// 显示结果
    for (int i = 0; i < x_color.size(); i++)
    {
        if (0 == x_color[i].color)
            cv::putText(frame, "Red", cv::Point(x_color[i].x, x_color[i].y - 25), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 2, 8, 0);
        else if (1 == x_color[i].color)
            cv::putText(frame, "Green", cv::Point(x_color[i].x, x_color[i].y - 25), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2, 8, 0);
        else if (2 == x_color[i].color)
            cv::putText(frame, "Yellow", cv::Point(x_color[i].x, x_color[i].y - 25), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 255), 2, 8, 0);
        else
            cv::putText(frame, "Lights off", cv::Point(x_color[i].x, x_color[i].y - 25), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), 2, 8, 0);
    }

    cv::namedWindow("tld_result", 0);
    cv::resizeWindow("tld_result", 1920, 1080);
    cv::imshow("tld_result", frame);
    cv::waitKey(1);

实际检测结果如下图所示： 

  

四、完整代码

/*
 * @CopyRight: All Rights Reserved by Plusgo
 * @Author: Yi
 * @E-mail: waterwinsor@gmail.com
 * @Date: 2023年 05月 06日 星期六 15:44:35
 * @LastEditTime: 2023年 05月 08日 星期一 10:07:30
 */

// requirements: opencv 4

#include <iostream>
#include <fstream>
#include <time.h>
#include <algorithm>

#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>

#include <opencv2/opencv.hpp>
#include "opencv2/imgproc.hpp"
#include <opencv2/imgproc/types_c.h>

struct Store_x_color
{
    int x;     // 存储左上顶点横坐标
    int y;     // 存储左上顶点纵坐标
    int color; // 存储当前点颜色
};

// Function headers
void processImg(cv::Mat, cv::Mat); // 前红后绿
bool CompareByX(const Store_x_color &, const Store_x_color &);

// Global variables
cv::Mat src_image;
bool rotated = true; // rotate 180

cv::Mat frame;
cv::Mat img;
cv::Mat imgYCrCb;
cv::Mat imgGreen;
cv::Mat imgRed;
cv::Mat imgYellow;
std::vector<Store_x_color> x_color;

void camera_callback(const sensor_msgs::CompressedImageConstPtr &msg_pic)
{
    try
    {
        cv_bridge::CvImagePtr cv_ptr =
            cv_bridge::toCvCopy(msg_pic, sensor_msgs::image_encodings::BGR8);
        if (rotated)
        {
            cv::flip(cv_ptr->image, src_image, -1);
        }
        else
        {
            src_image = cv_ptr->image;
        }
        // std::cout << "src_image" << src_image.size() << std::endl;

        // 亮度参数
        double a = 0.3;
        double b = (1 - a) * 125;

        // 统计检测用时
        clock_t start, end;

        start = clock();

        src_image.copyTo(frame);
        // 调整亮度
        src_image.convertTo(img, img.type(), a, b);
        // cv::imshow("img",img);

        // 使用ROI（感兴趣区域）方式截取图像
        cv::Rect roi(0, 0, 2048, 768); // 定义roi，图片尺寸2048*1536
        // std::cout << "img size:" << img.size() << std::endl;
        cv::Mat roi_image = img(roi);

        // 转换为YCrCb颜色空间
        cvtColor(roi_image, imgYCrCb, cv::COLOR_BGR2YCrCb);
        // cvtColor(img, imgYCrCb, cv::COLOR_BGR2YCrCb);
        imgRed.create(imgYCrCb.rows, imgYCrCb.cols, CV_8UC1);
        imgGreen.create(imgYCrCb.rows, imgYCrCb.cols, CV_8UC1);

        // 分解YCrCb的三个成分
        std::vector<cv::Mat> planes;
        split(imgYCrCb, planes);

        // 遍历以根据Cr分量拆分红色和绿色
        cv::MatIterator_<uchar> it_Cr = planes[1].begin<uchar>(),
                                it_Cr_end = planes[1].end<uchar>();
        cv::MatIterator_<uchar> it_Red = imgRed.begin<uchar>();
        cv::MatIterator_<uchar> it_Green = imgGreen.begin<uchar>();

        for (; it_Cr != it_Cr_end; ++it_Cr, ++it_Red, ++it_Green)
        {
            // RED, 145<Cr<470 红色
            // if (*it_Cr > 145 && *it_Cr < 470)
            if (*it_Cr > 140 && *it_Cr < 470)
                *it_Red = 255;
            else
                *it_Red = 0;

            // GREEN 95<Cr<110 绿色
            if (*it_Cr > 95 && *it_Cr < 110)
                *it_Green = 255;
            else
                *it_Green = 0;
            // YELLOW 黄色
        }

        // 膨胀和腐蚀
        dilate(imgRed, imgRed, cv::Mat(8, 8, CV_8UC1), cv::Point(-1, -1));
        erode(imgRed, imgRed, cv::Mat(1, 1, CV_8UC1), cv::Point(-1, -1));
        dilate(imgGreen, imgGreen, cv::Mat(12, 12, CV_8UC1), cv::Point(-1, -1));
        erode(imgGreen, imgGreen, cv::Mat(1, 1, CV_8UC1), cv::Point(-1, -1));

        // 检测和显示
        processImg(imgRed, imgGreen);

        // 清空x_color
        x_color.clear();

        end = clock();
        std::cout << "检测时间：" << (double)(end - start) / CLOCKS_PER_SEC << std::endl; // 打印检测时间
    }
    catch (cv_bridge::Exception e)
    {
        ROS_ERROR_STREAM("cant't get image");
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "tld_cv_node");
    ros::NodeHandle nh;

    std::string image_topic;
    nh.param<std::string>("sub_topic", image_topic, "/src_rgb/compressed");
    std::cout << "image_topic: " << image_topic << std::endl;

    ros::Subscriber camera_sub =
        nh.subscribe(image_topic, 2, camera_callback);
    ros::spin();
    ros::waitForShutdown();
    return 0;
}

void processImg(cv::Mat src_Red, cv::Mat src_Green)
{
    cv::Mat tmp_Red;
    cv::Mat tmp_Green;

    std::vector<std::vector<cv::Point>> contours_Red;
    std::vector<std::vector<cv::Point>> contours_Green;

    std::vector<cv::Vec4i> hierarchy_Red;
    std::vector<cv::Vec4i> hierarchy_Green;

    cv::Rect *trackBox_Red;
    cv::Rect *trackBox_Green;

    src_Red.copyTo(tmp_Red);
    src_Green.copyTo(tmp_Green);

    // 提取轮廓
    findContours(tmp_Red, contours_Red, hierarchy_Red, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
    findContours(tmp_Green, contours_Green, hierarchy_Green, CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);

    // 绘制轮廓
    drawContours(frame, contours_Red, -1, cv::Scalar(0, 0, 255), cv::FILLED); // Red
    std::cout << "提取到的红色轮廓数量：" << contours_Red.size() << std::endl;
    drawContours(frame, contours_Green, -1, cv::Scalar(0, 255, 0), cv::FILLED); // Green
    std::cout << "提取到的绿色轮廓数量：" << contours_Green.size() << std::endl;

    // 显示轮廓
    //  imshow("contours", frame);

    trackBox_Red = new cv::Rect[contours_Red.size()];
    trackBox_Green = new cv::Rect[contours_Green.size()];

    // 确定要跟踪的区域
    for (int i = 0; i < contours_Red.size(); i++)
    {
        // Yi opencv4 不支持 CvSeq
        trackBox_Red[i] = cv::boundingRect(contours_Red[i]);
    }

    for (int i = 0; i < contours_Green.size(); i++)
    {
        // Yi opencv4 不支持 CvSeq
        trackBox_Green[i] = cv::boundingRect(contours_Green[i]);
    }

    // imshow("contours", tmp);

    // Rect.tl() 返回矩形左上顶点的坐标
    for (int i = 0; i < contours_Red.size(); i++)
    {
        Store_x_color a;
        a.x = trackBox_Red[i].tl().x;
        a.y = trackBox_Red[i].tl().y;
        a.color = 0;
        x_color.push_back(a);
    }

    for (int i = 0; i < contours_Green.size(); i++)
    {
        Store_x_color a;
        a.x = trackBox_Green[i].tl().x;
        a.y = trackBox_Green[i].tl().y;
        a.color = 1;
        x_color.push_back(a);
    }

    // 清空指针
    delete[] trackBox_Red;
    delete[] trackBox_Green;

    // 对左上顶点横坐标进行排序
    sort(x_color.begin(), x_color.end(), CompareByX);

    // 显示结果
    for (int i = 0; i < x_color.size(); i++)
    {
        if (0 == x_color[i].color)
            cv::putText(frame, "Red", cv::Point(x_color[i].x, x_color[i].y - 25), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 2, 8, 0);
        else if (1 == x_color[i].color)
            cv::putText(frame, "Green", cv::Point(x_color[i].x, x_color[i].y - 25), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 0), 2, 8, 0);
        else if (2 == x_color[i].color)
            cv::putText(frame, "Yellow", cv::Point(x_color[i].x, x_color[i].y - 25), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 255, 255), 2, 8, 0);
        else
            cv::putText(frame, "Lights off", cv::Point(x_color[i].x, x_color[i].y - 25), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255), 2, 8, 0);
    }

    cv::namedWindow("tld_result", 0);
    cv::resizeWindow("tld_result", 1920, 1080);
    cv::imshow("tld_result", frame);
    cv::waitKey(1);

    return;
}

bool CompareByX(const Store_x_color &a, const Store_x_color &b)
{
    return a.x < b.x;
}

五、使用方法

编译所需的CMakeLists.txt、package.xml和运行所需roslaunch文件如下

• CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(tld_cv)

set(CMAKE_INCLUDE_CURRENT_DIR ON)
set(CMAKE_BUILD_TYPE "Release")  # Debug Release RelWithDebInfo

add_definitions(-O2 -pthread)
add_compile_options(-std=c++11)

find_package(OpenCV REQUIRED)
find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
  sensor_msgs
  cv_bridge
  image_transport
)

catkin_package(
    CATKIN_DEPENDS
    roscpp
    std_msgs
    sensor_msgs
    cv_bridge
    image_transport
)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
)

add_executable(tld_cv src/main.cpp)
target_link_libraries(tld_cv
        ${catkin_LIBRARIES}
        ${OpenCV_LIBRARIES}
        )

•  package.xml

<?xml version="1.0"?>
<package format="2">
  <name>tld_cv</name>
  <version>0.0.0</version>
  <description>The tld_cv package</description>

  <maintainer email="sunyuzhe@plusgo.com.cn">sunyuzhe</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>cv_bridge</build_depend>
  <build_depend>image_transport</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>cv_bridge</build_export_depend>
  <build_export_depend>image_transport</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>cv_bridge</exec_depend>
  <exec_depend>image_transport</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>std_msgs</exec_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

• detect.launch

<launch>

    <arg name="sub_image_topic" value="/camera/image_color/compressed"/>
    
    <param name="sub_topic" value="$(arg sub_image_topic)"/>

    <node pkg="tld_cv" type="tld_cv" name="tld_cv" output="screen" />

</launch>

六、后续改进思路 

改进可有如下几个方向：

• ROI

根据具体自动驾驶场景，可以通过红绿灯位置、高度、相机安装方式、车辆定位和IMU信息实时计算出一个更为精确的ROI，检测再进行排序即可确定红绿灯的个数和顺序，方便编写后处理中的逻辑。

• 筛选面积

根据检测后的结果筛选较大的几个轮廓，可以排除掉某些较小物体的误检干扰 

本人接触OpenCV时间尚短、经验尚浅，如果有任何疏漏、错误还请大家指出～