ros_hydro_Opencv_with_Kinect_camshiftdemo.cpp
2015-12-21 17:50
423 查看
ros下的camshiftdemo.cpp
//Includes all the headers necessary to use the most common public pieces of the ROS system. #include <ros/ros.h> //Use image_transport for publishing and subscribing to images in ROS #include <image_transport/image_transport.h> //Use cv_bridge to convert between ROS and OpenCV Image formats #include <cv_bridge/cv_bridge.h> //Include some useful constants for image encoding. Refer to: http://www.ros.org/doc/api/sensor_msgs/html/namespacesensor__msgs_1_1image__encodings.html for more info. #include <sensor_msgs/image_encodings.h> //Include headers for OpenCV Image processing #include <opencv2/imgproc/imgproc.hpp> //Include headers for OpenCV GUI handling #include <opencv2/highgui/highgui.hpp> #include "opencv2/video/tracking.hpp" #include <geometry_msgs/Twist.h> #include <std_msgs/Float32.h> #include <std_msgs/UInt8.h> using namespace cv; using namespace std; //Store all constants for image encodings in the enc namespace to be used later. namespace enc = sensor_msgs::image_encodings; //Use method of ImageTransport to create image publisher //image_transport::Publisher pub; bool backprojMode = false; bool selectObject = false; int trackObject = 0; bool showHist = true; static Point origin; static Rect selection; int vmin = 30, vmax = 256, smin = 60; Mat image; static int imgWidth, imgHeight; static int initDestArea = 1; //Initialized to 1 to avoid DIV by 0 errors //static int prevDestArea = 0; //static float destAreaDot = 0; static ros::Publisher robotAngleVar; /* State is published according to the following enum * 0: Waiting for destination * 1: Destination tracking enabled * 2: Destination Lost * 3..255: TBD */ static ros::Publisher imagePrStatePub; static ros::Publisher destAreaPub; void camShift(Mat inImg); static void onMouse( int event, int x, int y, int, void* ) { ROS_INFO("Mouse detected"); if( selectObject ) { selection.x = MIN(x, origin.x); selection.y = MIN(y, origin.y); selection.width = std::abs(x - origin.x); selection.height = std::abs(y - origin.y); selection &= Rect(0, 0, image.cols, image.rows); initDestArea = selection.area();//duo } switch( event ) { case CV_EVENT_LBUTTONDOWN: origin = Point(x,y); selection = Rect(x,y,0,0); selectObject = true; break; case CV_EVENT_LBUTTONUP: selectObject = false; if( selection.width > 0 && selection.height > 0 ) trackObject = -1; break; } } //This function is called everytime a new image_info message is published void camInfoCallback(const sensor_msgs::CameraInfo & camInfoMsg) { //Store the image width for calculation of angle imgWidth = camInfoMsg.width; imgHeight = camInfoMsg.height; } /*void destCoordCallback(const sensor_msgs::RegionOfInterest& destROI) { //Copy the ROI to the local buffer selection.x = destROI.x_offset; selection.y = destROI.y_offset; selection.height = destROI.height; selection.width = destROI.width; trackObject = -1; initDestArea = selection.area(); }*/ //This function is called everytime a new image is published void imageCallback(const sensor_msgs::ImageConstPtr& original_image) { //Convert from the ROS image message to a CvImage suitable for working with OpenCV for processing cv_bridge::CvImagePtr cv_ptr; try { //Always copy, returning a mutable CvImage //OpenCV expects color images to use BGR channel order. cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8); } catch (cv_bridge::Exception& e) { //if there is an error during conversion, display it ROS_ERROR("tutorialROSOpenCV::main.cpp::cv_bridge exception: %s", e.what()); return; } camShift(cv_ptr->image); } void trackArea(Rect window) { //TBD //Code should track the area of the target. If the area grows very fast then most probably the destination has a shift //And it has lost the destination std_msgs::Float32 destArea; //destArea.data = ((float)curDestArea)/initDestArea; destArea.data = (float)window.area()/(imgHeight*imgWidth); destAreaPub.publish(destArea); } void calcAngle(Point2f destCentre) { std_msgs::Float32 normAngle; //If we have started tracking the object if(trackObject != 0) { normAngle.data = (destCentre.x - ((float)imgWidth/2))/((float)imgWidth/2); robotAngleVar.publish(normAngle); } } void camShift(Mat inImg) { static Rect trackWindow; static int hsize = 16; static float hranges[] = {0,180}; static const float* phranges = hranges; static Mat frame, hsv, hue, mask, hist, histimg = Mat::zeros(200, 320, CV_8UC3), backproj; static bool paused = false; RotatedRect trackBox; std_msgs::UInt8 state; //If the image processing is not paused if( !paused ) { //cap >> frame; if( inImg.empty() ) { ROS_INFO("Camera image empty"); return;//break; } } //Use the input image as the reference //Only a shallow copy, so relatively fast image = inImg; if(!paused) { //Convert the colour space to HSV cvtColor(image, hsv, CV_BGR2HSV); //If the destination coordinates have been received, then start the tracking //trackObject is set when the destination coordinates have been received if( trackObject ) { int _vmin = vmin, _vmax = vmax; inRange(hsv, Scalar(0, smin, MIN(_vmin,_vmax)), Scalar(180, 256, MAX(_vmin, _vmax)), mask); int ch[] = {0, 0}; hue.create(hsv.size(), hsv.depth()); mixChannels(&hsv, 1, &hue, 1, ch, 1); //Do the following steps only for the first time if( trackObject < 0 ) { //Publish that we have started tracking std_msgs::UInt8 state;// state.data = 1;// imagePrStatePub.publish(state);// //Set the Region of interest and the mask for it Mat roi(hue, selection), maskroi(mask, selection); //Calculate the histogram of this calcHist(&roi, 1, 0, maskroi, hist, 1, &hsize, &phranges); normalize(hist, hist, 0, 255, CV_MINMAX); trackWindow = selection; trackObject = 1; histimg = Scalar::all(0); int binW = histimg.cols / hsize; Mat buf(1, hsize, CV_8UC3); for( int i = 0; i < hsize; i++ ) buf.at<Vec3b>(i) = Vec3b(saturate_cast<uchar>(i*180./hsize), 255, 255); cvtColor(buf, buf, CV_HSV2BGR); for( int i = 0; i < hsize; i++ ) { int val = saturate_cast<int>(hist.at<float>(i)*histimg.rows/255); rectangle( histimg, Point(i*binW,histimg.rows), Point((i+1)*binW,histimg.rows - val), Scalar(buf.at<Vec3b>(i)), -1, 8 ); } } calcBackProject(&hue, 1, 0, hist, backproj, &phranges); backproj &= mask; trackBox = CamShift(backproj, trackWindow, TermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 )); if( trackWindow.area() <= 1 ) { //Notify that the destination has been lost std_msgs::UInt8 state;// state.data = 2;// imagePrStatePub.publish(state);// ROS_INFO("*********DESTINATION LOST in CAMSHIFT************"); ROS_INFO("track height %d width %d", trackWindow.height, trackWindow.width); trackObject = 0; // //Disable tracking to avoid termination of node due to negative heights TBD int cols = backproj.cols, rows = backproj.rows, r = (MIN(cols, rows) + 5)/6; trackWindow = Rect(trackWindow.x - r, trackWindow.y - r, trackWindow.x + r, trackWindow.y + r) & Rect(0, 0, cols, rows); } if( backprojMode ) cvtColor( backproj, image, CV_GRAY2BGR ); ellipse( image, trackBox, Scalar(0,0,255), 3, CV_AA ); } } else if( trackObject < 0 ) { //If a new destination has been selected stop pausing paused = false; } //Code to display an inverted image of the selected region //Remove this in the fall validation expt TBD if( selectObject && selection.width > 0 && selection.height > 0 ) { Mat roi(image, selection); bitwise_not(roi, roi); } imshow( "CamShift Demo", image ); imshow( "Histogram", histimg ); char c = (char)waitKey(1); if( c == 27 ) ROS_INFO("Exit boss");//break; switch(c) { case 'b': backprojMode = !backprojMode; //黑白图像 break; case 'c': //刷新,从新取图 trackObject = 0; histimg = Scalar::all(0); break; case 'h': //显示直方图 showHist = !showHist; if( !showHist ) destroyWindow( "Histogram" ); else namedWindow( "Histogram", 1 ); break; case 'p': //暂停检测或继续 paused = !paused; break; default: break; } setMouseCallback( "CamShift Demo", onMouse, 0 );// createTrackbar( "Vmin", "CamShift Demo", &vmin, 256, 0 ); createTrackbar( "Vmax", "CamShift Demo", &vmax, 256, 0 ); createTrackbar( "Smin", "CamShift Demo", &smin, 256, 0 ); //Find the area of the destination and publish it trackArea(trackWindow); //Find the angle of the destination wrt to the robot and publish that calcAngle(trackBox.center); } /** * This is ROS node to track the destination image */ int main(int argc, char **argv) { /** * The ros::init() function needs to see argc and argv so that it can perform * any ROS arguments and name remapping that were provided at the command line. For programmatic * remappings you can use a different version of init() which takes remappings * directly, but for most command-line programs, passing argc and argv is the easiest * way to do it. The third argument to init() is the name of the node. Node names must be unique in a running system. * The name used here must be a base name, ie. it cannot have a / in it. * You must call one of the versions of ros::init() before using any other * part of the ROS system. */ ros::init(argc, argv, "image_processor"); ROS_INFO("-----------------"); /** * NodeHandle is the main access point to communications with the ROS system. * The first NodeHandle constructed will fully initialize this node, and the last * NodeHandle destructed will close down the node. */ ros::NodeHandle nh; //Create an ImageTransport instance, initializing it with our NodeHandle. image_transport::ImageTransport it(nh); std_msgs::UInt8 state; //OpenCV HighGUI call to create a display window on start-up. namedWindow( "Histogram", 0 ); namedWindow( "CamShift Demo", 0 ); /** * Subscribe to the "camera/image_raw" base topic. The actual ROS topic subscribed to depends on which transport is used. * In the default case, "raw" transport, the topic is in fact "camera/image_raw" with type sensor_msgs/Image. ROS will call * the "imageCallback" function whenever a new image arrives. The 2nd argument is the queue size. * subscribe() returns an image_transport::Subscriber object, that you must hold on to until you want to unsubscribe. * When the Subscriber object is destructed, it will automaticaInfoCallbacklly unsubscribe from the "camera/image_raw" base topic. */ image_transport::Subscriber sub = it.subscribe("camera/rgb/image_raw", 1, imageCallback); //style="background-color: rgb(255, 255, 0);"//Kinect Topic</span> ros::Subscriber camInfo = nh.subscribe("camera/rgb/camera_info", 1, camInfoCallback);// style="background-color: rgb(255, 255, 0);"//Kinect Topic</span> // ros::Subscriber destCoord = nh.subscribe("dest_coord", 1, destCoordCallback); robotAngleVar = nh.advertise<std_msgs::Float32>("robot_angle", 100); imagePrStatePub = nh.advertise<std_msgs::UInt8>("improc_state", 10); destAreaPub = nh.advertise<std_msgs::Float32>("dest_area", 10); state.data = 0; imagePrStatePub.publish(state); //OpenCV HighGUI call to destroy a display window on shut-down. //destroyWindow(WINDOW); destroyWindow("Histogram"); destroyWindow("CamShift Demo"); /** * In this application all user callbacks will be called from within the ros::spin() call. * ros::spin() will not return until the node has been shutdown, either through a call * to ros::shutdown() or a Ctrl-C. */ ros::spin(); //ROS_INFO is the replacement for printf/cout. ROS_INFO("tutorialROSOpenCV::main.cpp::No error."); }
与windows下的camshift.cpp对比
#include <opencv2/core/utility.hpp> #include "opencv2/video/tracking.hpp" #include "opencv2/imgproc.hpp" #include "opencv2/videoio.hpp" #include "opencv2/highgui.hpp" #include <iostream> #include <ctype.h> using namespace cv; using namespace std; Mat image; bool backprojMode = false; bool selectObject = false; int trackObject = 0; bool showHist = true; Point origin; Rect selection; int vmin = 10, vmax = 256, smin = 30; static void onMouse( int event, int x, int y, int, void* ) { if( selectObject ) { selection.x = MIN(x, origin.x); selection.y = MIN(y, origin.y); selection.width = std::abs(x - origin.x); selection.height = std::abs(y - origin.y); selection &= Rect(0, 0, image.cols, image.rows); } switch( event ) { case EVENT_LBUTTONDOWN: origin = Point(x,y); selection = Rect(x,y,0,0); selectObject = true; break; case EVENT_LBUTTONUP: selectObject = false; if( selection.width > 0 && selection.height > 0 ) trackObject = -1; break; } } static void help() { cout << "\nThis is a demo that shows mean-shift based tracking\n" "You select a color objects such as your face and it tracks it.\n" "This reads from video camera (0 by default, or the camera number the user enters\n" "Usage: \n" " ./camshiftdemo [camera number]\n"; cout << "\n\nHot keys: \n" "\tESC - quit the program\n" "\tc - stop the tracking\n" "\tb - switch to/from backprojection view\n" "\th - show/hide object histogram\n" "\tp - pause video\n" "To initialize tracking, select the object with mouse\n"; } const char* keys = { "{@camera_number| 0 | camera number}" }; int main( int argc, const char** argv ) { help(); VideoCapture cap; Rect trackWindow; int hsize = 16; float hranges[] = {0,180}; const float* phranges = hranges; CommandLineParser parser(argc, argv, keys); int camNum = parser.get<int>(0); cap.open(camNum); if( !cap.isOpened() ) { help(); cout << "***Could not initialize capturing...***\n"; cout << "Current parameter's value: \n"; parser.printMessage(); return -1; } namedWindow( "Histogram", 0 ); namedWindow( "CamShift Demo", 0 ); setMouseCallback( "CamShift Demo", onMouse, 0 ); createTrackbar( "Vmin", "CamShift Demo", &vmin, 256, 0 ); createTrackbar( "Vmax", "CamShift Demo", &vmax, 256, 0 ); createTrackbar( "Smin", "CamShift Demo", &smin, 256, 0 ); Mat frame, hsv, hue, mask, hist, histimg = Mat::zeros(200, 320, CV_8UC3), backproj; bool paused = false; for(;;) { if( !paused ) { cap >> frame; if( frame.empty() ) break; } frame.copyTo(image); if( !paused ) { cvtColor(image, hsv, COLOR_BGR2HSV); if( trackObject ) { int _vmin = vmin, _vmax = vmax; inRange(hsv, Scalar(0, smin, MIN(_vmin,_vmax)), Scalar(180, 256, MAX(_vmin, _vmax)), mask); int ch[] = {0, 0}; hue.create(hsv.size(), hsv.depth()); mixChannels(&hsv, 1, &hue, 1, ch, 1); if( trackObject < 0 ) { Mat roi(hue, selection), maskroi(mask, selection); calcHist(&roi, 1, 0, maskroi, hist, 1, &hsize, &phranges); normalize(hist, hist, 0, 255, NORM_MINMAX); trackWindow = selection; trackObject = 1; histimg = Scalar::all(0); int binW = histimg.cols / hsize; Mat buf(1, hsize, CV_8UC3); for( int i = 0; i < hsize; i++ ) buf.at<Vec3b>(i) = Vec3b(saturate_cast<uchar>(i*180./hsize), 255, 255); cvtColor(buf, buf, COLOR_HSV2BGR); for( int i = 0; i < hsize; i++ ) { int val = saturate_cast<int>(hist.at<float>(i)*histimg.rows/255); rectangle( histimg, Point(i*binW,histimg.rows), Point((i+1)*binW,histimg.rows - val), Scalar(buf.at<Vec3b>(i)), -1, 8 ); } } calcBackProject(&hue, 1, 0, hist, backproj, &phranges); backproj &= mask; RotatedRect trackBox = CamShift(backproj, trackWindow, TermCriteria( TermCriteria::EPS | TermCriteria::COUNT, 10, 1 )); if( trackWindow.area() <= 1 ) { int cols = backproj.cols, rows = backproj.rows, r = (MIN(cols, rows) + 5)/6; trackWindow = Rect(trackWindow.x - r, trackWindow.y - r, trackWindow.x + r, trackWindow.y + r) & Rect(0, 0, cols, rows); } if( backprojMode ) cvtColor( backproj, image, COLOR_GRAY2BGR ); ellipse( image, trackBox, Scalar(0,0,255), 3, LINE_AA ); } } else if( trackObject < 0 ) paused = false; if( selectObject && selection.width > 0 && selection.height > 0 ) { Mat roi(image, selection); bitwise_not(roi, roi); } imshow( "CamShift Demo", image ); imshow( "Histogram", histimg ); char c = (char)waitKey(10); if( c == 27 ) break; switch(c) { case 'b': backprojMode = !backprojMode; break; case 'c': trackObject = 0; histimg = Scalar::all(0); break; case 'h': showHist = !showHist; if( !showHist ) destroyWindow( "Histogram" ); else namedWindow( "Histogram", 1 ); break; case 'p': paused = !paused; break; default: ; } } return 0; }
实现方式:
roscorerosrun openni_camera openni_node////可不要
roslaunch openni_launch openni.launch//运行kinect
cd catkin_ws//前提是已经建好了程序包,参考:http://blog.csdn.net/hanshuning/article/details/50168847
catkin_make
source devel/setup.bash
rosun 程序包名称 camshiftdemo
用鼠标选中目标物体,就会自动跟踪。
参考:github中ecp
相关文章推荐
- C++中对象生存周期
- 【C++探索之旅】第一部分第四课:内存,变量和引用
- 【C++探索之旅】第一部分第四课:内存,变量和引用
- C/C++ 笔试、面试题目大汇总
- C++ CCF真题----消除类游戏
- Leetcode 3 - Longest Substring Without Repeating Characters
- C/C++面试题大集锦
- 编写字符小游戏——贪吃蛇程序(c语言)
- C++ using关键字作用总结
- 环缓冲托管实现
- 提取字符串中的数字-非指针版-(C语言描述)
- C++中类所占内存空间大小
- C++ Primer 学习笔记15 容器的选择
- C++设计模式新解四 观察者模式+事件订阅
- 插入排序的简单解释(C++)
- c++函数传参的一些指导原则
- C++关联容器-map[ ]=与insert操作不同
- Jni 学习之Java调用C++
- C++ CCF真题----画图
- 策略模式--c++实现