ROS中使用C++实现基于opencv的目标跟踪

标签: opencv  c++  计算机视觉

参照胡春旭《ROS机器人开发实战》目标跟踪的python代码思路进行。

motion_detector.cpp

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv-3.3.1-dev/opencv2/opencv.hpp>
#include <sensor_msgs/image_encodings.h>
#include <opencv-3.3.1-dev/opencv2/imgproc/imgproc.hpp>
#include <opencv-3.3.1-dev/opencv2/highgui/highgui.hpp>
#include <opencv-3.3.1-dev/opencv2/calib3d/calib3d.hpp>
#include <opencv-3.3.1-dev/opencv2/core.hpp>
#include <opencv-3.3.1-dev/opencv2/opencv.hpp>
using namespace std;
using namespace cv;
static const std::string OPENCV_WINDOW = "Image window";

class ImageConverter
{
    string  face ;
    ros::NodeHandle nh_;
    image_transport::ImageTransport it_;
    image_transport::Subscriber image_sub_;
    image_transport::Publisher image_pub_;
    //bool get_face=nh_.getParam("cascade_1",face);
    cv::Mat image,image_gray,first_frame,frame,thresh;
public:
    Mat element = getStructuringElement( 2,
                       Size(  1, 1 ),
                       Point( 0, 0 ) );

public:
    
  ImageConverter()
    : it_(nh_)
  {
    // Subscrive to input video feed and publish output video feed
    image_sub_ = it_.subscribe("/usb_cam/image_raw", 1,
      &ImageConverter::imageCb, this);
    image_pub_ = it_.advertise("/image_converter/output_video", 1);

    cv::namedWindow(OPENCV_WINDOW);
  }

  ~ImageConverter()
  {
    cv::destroyWindow(OPENCV_WINDOW);
  }

  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    vector<Rect> faceRect;
    vector<Vec4i> hierarchy;
    vector<vector<Point> > contours;
    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;
    }
    image = cv_ptr->image;
    cv::cvtColor(image,image_gray,CV_RGBA2GRAY);
    cv::equalizeHist(image_gray,image_gray);
    // ROS_INFO("%d",first_frame.rows);
    // ROS_INFO("%d",frame.rows);
    if (first_frame.rows == 0)
        {
            // cv::Mat::clone();
            // first_frame = image_gray;
            first_frame = image_gray.clone();
        }
    cv::absdiff(image_gray,first_frame,frame);
    cv::threshold(frame,thresh,25,255,cv::THRESH_BINARY);
    cv::dilate(thresh,thresh,element);
    cv::findContours( thresh,contours , hierarchy, RETR_TREE, CHAIN_APPROX_SIMPLE, Point(0, 0) );
    for (size_t i = 0;i < contours.size(); i++)
    {
        if (cv::contourArea(Mat(contours[i])) >= 800)
        {
            Rect result1 = cv::boundingRect(Mat(contours[i]));
            cv::rectangle(cv_ptr->image,result1,Scalar(0,0,255),1,LINE_MAX);
            //cv::drawContours(image,contours,(int)i,Scalar(0,0,255), 2, 8, hierarchy, 0, Point());
        }
        
        //cv::drawContours(image,contours,(int)i,Scalar(0,0,255), 2, 8, hierarchy, 0, Point());
        //cv::rectangle(thresh,contours[i],Scalar(0,0,255));
    }

        // Update GUI Window
    cv::imshow(OPENCV_WINDOW, cv_ptr->image);
    cv::waitKey(3);
    // Output modified video stream
    image_pub_.publish(cv_ptr->toImageMsg());
    //}
  }

};

int main(int argc, char** argv)
{

  ros::init(argc, argv, "image_converter");
  ImageConverter ic;
  ros::spin();
  return 0;
}

代码存在冗余部分未进行修改,部分参数可以进行修改实现提高目标跟踪的正确率。

motion_detection_for_cpp.launch

<launch>

    <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen">
        <param name="video_device" value="/dev/video0"/>
        <param name="image_width" value="640"/>
        <param name="image_height" value="480"/>
        <param name="pixel_format" value="yuyv"/>
        <param name="camera_frame_id" value="usb_cam"/>
        <param name="io_method" value="mmap"/>
    </node>


    <node pkg="robot_vision" name="motion_detector" type="motion_detector" output="screen">
    </node>
</launch>

CMakeLists.txt文件与上篇相同

实现效果不好。。。。。。。。。。。。。。。。。。。。。。。。
ros学习在路上!!!!!!!!!!!!!!
在这里插入图片描述
上篇链接:
https://blog.csdn.net/weixin_42606990/article/details/104969769

版权声明:本文为weixin_42606990原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接和本声明。
本文链接:https://blog.csdn.net/weixin_42606990/article/details/104979197