ROS中进行OpenCV物体识别

ROS驱动摄像头，发布图像消息
将ROS图像消息转换成OpenCV图像数据
OpenCV图像处理
OpenCV图像转换成ROS消息

头文件

#ifndef FACE_DETECTOR_HPP_
#define FACE_DETECTOR_HPP_

#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/objdetect.hpp>
#include "opencv2/highgui/highgui.hpp"

#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>

定义7种颜色，用于标记人脸
cv::Scalar colors[] =
{
    // 红橙黄绿青蓝紫
   CV_RGB(255,0,0),
   CV_RGB(255, 97, 0),
   CV_RGB(255, 255, 0),
   CV_RGB(0, 255, 0),
   CV_RGB(255, 97, 0),
   CV_RGB(0, 0, 255),
   CV_RGB(160, 32, 240),
};

class FaceDetector
{
public:
    FaceDetector(ros::NodeHandle& nh);

    ~FaceDetector();

private:
    void detectFace(const sensor_msgs::ImageConstPtr &msg);
    void drawFace(cv::Mat &image, const std::vector<cv::Rect> &rect);
    void imageCb(const sensor_msgs::ImageConstPtr &msg);

    image_transport::ImageTransport it_;
    image_transport::Subscriber image_sub_;
    image_transport::Publisher image_pub_;
    cv_bridge::CvImagePtr cv_ptr_;
    cv::Mat gray_img_;
    cv::Mat process_img_;
    cv::CascadeClassifier cascade_;
    std::vector<cv::Rect> rect_;
};

#endif /* FACE_DETECTOR_HPP_ */

cpp文件

#include "face_detector.hpp"

FaceDetector::FaceDetector(ros::NodeHandle& nh) : it_(nh)
{
    // 加载人脸分类器
    cascade_.load("/usr/share/opencv/haarcascades/haarcascade_frontalface_alt2.xml");
    image_sub_ = it_.subscribe("/usb_cam/image_raw", 1, &FaceDetector::imageCb, this);
    image_pub_ = it_.advertise("/face_detect", 1);
}

FaceDetector::~FaceDetector()
{
    ROS_INFO("Bye bye\n");
}

void FaceDetector::drawFace(cv::Mat &image, const std::vector<cv::Rect> &rect)
{
    cv::Point center;
    int radius;
    for(int i = 0; i < rect.size(); i++)
    {
        center.x = cvRound((rect[i].x + rect[i].width * 0.5));
        center.y = cvRound((rect[i].y + rect[i].height * 0.5));
        radius = cvRound((rect[i].width + rect[i].height) *0.25);
        cv::circle(image, center, radius, colors[i % 7], 2);
    }
}

void FaceDetector::detectFace(const sensor_msgs::ImageConstPtr &msg)
{
    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;
    }
    cv::Mat &image = cv_ptr_->image;
    process_img_ = image.clone();
    cv::cvtColor(image, gray_img_, CV_BGR2GRAY);
    cascade_.detectMultiScale(gray_img_, rect_, 1.3, 4, 0);
    ROS_INFO("%d face detected\n", static_cast<int>(rect_.size()));
    drawFace(process_img_, rect_);
    cv_bridge::CvImage out_image;
    out_image.encoding = cv_ptr_->encoding;
    out_image.header = cv_ptr_->header;
    out_image.image = process_img_;
    image_pub_.publish(out_image.toImageMsg());
}

void FaceDetector::imageCb(const sensor_msgs::ImageConstPtr &msg)
{
    detectFace(msg);
}

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

    ros::WallDuration(0.1).sleep();

    FaceDetector fd(n_);

    while (ros::ok())
    {
        // Process image callback
        ros::spinOnce();

        ros::WallDuration(0.1).sleep();
    }
    return 0;
}

launch文件

<launch>
    <include file="$(find ur5_vision)/launch/usb_cam.launch" />
    <node name="face_detector" pkg="ur5_vision" type="face_detector" output="screen" />
    <node name="image_viewer" pkg="rqt_image_view" type="rqt_image_view" output="screen" >
        <param name="image" value="/face_detect" />
    </node>
</launch>

启动后将看到标注人脸（如果有）的视频流，终端会打印检测到人脸的数目
在这里插入图片描述