ROSOpenTLD技术文档.docx

ROS-OpenTLD 技术文档李明龙 2014 .11.19 1、简介TLD算法全称为tracking-learning-detecting算法，旨在实现长期的单目标跟踪，具有实时性和鲁棒性。ROS-OpenTLD是由大牛Ronan Chauvin所编写，创建日期为2012年6月8日，为George Nebehay所编写的C+版本TLD算法实现ROS封装并提供接口（TLD算法的原作者为捷克籍博士Zdenek Kalal，用MATLAB和C编写）。需要补充的是，无论是原作者Zdenek的MATLAB代码，George的C+代码，还是由Ronan所实现的ROS封装，都遵循GPL开源协议，允许复制，保存，修改，重新发布甚至做产品，但是必须附上原作者的名字，具体参考GPL开源协议文档（ROS中使用BSD开源协议，限制更少，比GPL更宽松）。2、实现 2.1 TLD算法思想概述图1 TLD算法总体框架TLD算法的总体实现如图1所示，包括四大模块：跟踪，学习，检测以及集成模块。视频帧输入之后，通过学习模块不断对在线目标模型进行实时更新，同时学习模块也担任着另一个任务，那就是实时更新跟踪模块和检测模块中所需要的一些阈值（阈值初始由yml文件读入，是人为设置的经验参数，后期根据所跟踪目标的不同进行实时的训练，最终收敛。）跟踪模块通过中值流（median flow）跟踪法会预测出一个下一帧的目标位置。检测模块通过级联分类器（cascade classfier，由阈值分类器，随机森林分类器，最近邻分类器三个分类器组成），会检测出一定数量（不一定是一个，可能是多个）的下一帧的目标位置。综合模块就对这两类目标位置进行综合，从而得出一个最合适的目标位置，通过图像框（bounding box）的方式显示出来，并把这个最优图像框反馈给学习模块，来丰富他的训练集。就这样一帧一帧的循环迭代，实现了实时而又鲁棒的单目标跟踪。 2.2 ROS中的封装2.2.1消息格式（message format） 在阐述封装实现细节之前，首先列举ROS 包所定义的消息格式： 1、BoundingBox，表示图像框boundingbox。 Header headerint32 xint32 yint32 widthint32 heightfloat32 confidencex, y, width, height分别表示目标图像框的位置坐标以及宽和高，confidence这个浮点变量表示的是所跟踪到的目标的置信度（与目标模型的符合程度）。 2、Target，表示目标 BoundingBox bbsensor_msgs/Image img target格式的message中包含一个BoundingBox类型的变脸bb，同时包含一个img类，img类定义在sensor_msgs包中，ROS用sensor_msgs/Image来传输图像，但是image格式是ROS所能识别和处理的，要让openCV也能识别和处理，必须借助于CvBridge来相互转化（见附录一）。2.2.2 参量、发布者和订阅者 首先列举作者所定义的一些参量：ros:NodeHandle np();np.param(showOutput, showOutput, true);np.param(loadModel, loadModel, false);np.param(autoFaceDetection, autoFaceDetection, false);np.param(exportModelAfterRun, exportModelAfterRun, false);np.param(modelImportFile, modelImportFile, std:string(model);np.param(modelExportFile, modelExportFile, std:string(model);np.param(cascadePath, face_cascade_path, std:string(haarcascade_frontalface_alt.xml);np.param(x, target_bb.x, 100);np.param(y, target_bb.y, 100);np.param(width, target_bb.width, 100);np.param(height, target_bb.height, 100);np.param(correctBB, correctBB, false); 作者对于参量的命名十分具体，例如loadModel表示载入模型，在这里不做叙述。下面列举作者所定义的两个发布者和三个订阅者：pub1 = n.advertise(tld_tracked_object, 1000, true);pub2 = n.advertise(tld_fps, 1000, true);sub1 = n.subscribe(image, 1000, &Main:imageReceivedCB, this);sub2 = n.subscribe(bounding_box, 1000, &Main:targetReceivedCB, this);sub3 = n.subscribe(cmds, 1000, &Main:cmdReceivedCB, this); pub1在tld_tracked_object主题上发布消息，发布的消息格式是boundingbox，用图像框代表被跟踪的目标；pub2在tld_fps主题上发布消息，表示tld中的帧速率；sub1在image主题上接收消息，调用回调函数imageReceivedCB，表示接收了待检测的图像片时的处理过程；sub2在bounding box主题上接收消息，调用回调函数targetReceivedCB，表示接收了被检测到的目标图像框时的处理过程；sub3在cmds主题上接收消息，调用回调函数cmdReceivedCB，表示接收了外部命令时的处理过程。2.2.3 有限状态机作者在ROS中的封装，把一些George所编写的C+版本中实现的既有函数当做黑箱，只考虑其功能和输入输出，不考虑实现细节，直接调用。同时运用了有限状态机的思想，在while (ros:ok()循环体中实现这四个状态：1、INIT 2、TRACKER_INIT 3、TRACKING4、STOPPED下面通过一一阐述这四个状态及其相互转换，来叙述算法的实现过程。1、INIT状态： case INIT:if(newImageReceived() if(showOutput) sendTrackedObject(0, 0, 0, 0, 0.0); getLastImageFromBuffer(); tld-detectorCascade-imgWidth = gray.cols; tld-detectorCascade-imgHeight = gray.rows; tld-detectorCascade-imgWidthStep = gray.step; state = TRACKER_INIT; break; 在INIT状态中，如果新图像片被接收，那么sendTrackedObject函数调用pub1，把被跟踪的图像片发布出来，同时getLastImageFromBuffer，从缓冲池中调取最新一阵的图像片位置，把它的位置和扫描步长都赋给级联检测器detectorCascade，这一系列工作都完成后，转到TRACKER_INIT状态。2、TRACKER_INIT状态case TRACKER_INIT: if(loadModel & !modelImportFile.empty() ROS_INFO(Loading model %s, modelImportFile.c_str(); tld-readFromFile(modelImportFile.c_str(); tld-learningEnabled = false; state = TRACKING; else if(autoFaceDetection | correctBB) if(autoFaceDetection) target_image = gray; target_bb = faceDetection(); sendTrackedObject(target_bb.x,target_bb.y,target_bb.width,target_bb.height,1.0); ROS_INFO(Starting at %d %d %d %dn, target_bb.x, target_bb.y, target_bb.width, target_bb.height); tld-selectObject(target_image, &target_bb); tld-learningEnabled = true; state = TRACKING; else ros:Duration(1.0).sleep(); ROS_INFO(Waiting for a BB); break;TRACKER_INIT状态主要完成的功能是：如果模型已被载入，并且输入文件非空，那么就标志一些参数，同时转到TRACKING状态；如果模型未被载入，但是已检测出来，那么就把检测的读入，最后也是把检测的目标发布出来，同时标志一些位，转到TRACKING状态。3、TRACKING 状态if(newImageReceived() ros:Time tic = ros:Time:now(); getLastImageFromBuffer(); tld-processImage(img); ros:Duration toc = (ros:Time:now() - tic); float fps = 1.0/toc.toSec(); std_msgs:Float32 msg_fps; msg_fps.data = fps; pub2.publish(msg_fps); if(showOutput) if(tld-currBB != NULL) sendTrackedObject(tld-currBB-x,tld-currBB-y,tld-currBB-width,tld-currBB-height,tld-currConf); else sendTrackedObject(1, 1, 1, 1, 0.0); 在这个状态里，通过getLastImageFromBuffer()把图像片取出来，通过processImage这个核心函数实现跟踪的过程（具体过程是在George的C+代码中封装好的，我们调用就行） ，同时有一些即使函数，实时地发布出视频的帧速率（调用pub1），最后通过sendTrackedObject函数检测到的模型发布出来（调用pub2）。 4、STOPPED状态case STOPPED: ros:Duration(1.0).sleep(); ROS_INFO(Tracker stopped); break;这个状态表示跟踪器停止。作者ROS封装的核心思想就是这样，至于QtGui实现的分析和相关实验是下一步的工作。附录一 利用CvBridge实现ROS与OpenCV的接口ConceptsROS passes around images in its ownsensor_msgs/Imagemessage format, but many users will want to use images in conjunction with OpenCV.CvBridgeis a ROS library that provides an interface between ROS and OpenCV.CvBridgecan be found in thecv_bridgepackage in thevision_opencvstack.In this tutorial, you will learn how to write a node that usesCvBridgeto convert ROS images into OpenCVcv:Matformat.You will also learn how to convert OpenCV images to ROS format to be published over ROS.Migration from codes written in C-Turtle or earlierThere was a major api changeROS DiamondbackregardingOpenCV, of which backward compatibility has been maintained for a while but removed in newer distro (eg.hydro), e.g.sensor_msgs/CvBridge. Seethe design decision. Alsothis QAis helpful.Converting ROS image messages to OpenCV imagesCvBridgedefines aCvImagetype containing an OpenCV image, its encoding and a ROS header.CvImagecontains exactly the informationsensor_msgs/Imagedoes, so we can convert either representation to the other.CvImageclass format:Toggle line numbers 1 namespace cv_bridge 2 3 class CvImage 4 5 public: 6 std_msgs:Header header; 7 std:string encoding; 8 cv:Mat image; 9 ; 10 11 typedef boost:shared_ptr CvImagePtr; 12 typedef boost:shared_ptr CvImageConstPtr; 13 14 When converting a ROSsensor_msgs/Imagemessage into aCvImage,CvBridgerecognizes two distinct use cases:1. We want to modify the data in-place. We have to make a copy of the ROS message data.2. We wont modify the data. We can safely share the data owned by the ROS message instead of copying.CvBridgeprovides the following functions for converting toCvImage:Toggle line numbers 1 / Case 1: Always copy, returning a mutable CvImage 2 CvImagePtr toCvCopy(const sensor_msgs:ImageConstPtr& source, 3 const std:string& encoding = std:string(); 4 CvImagePtr toCvCopy(const sensor_msgs:Image& source, 5 const std:string& encoding = std:string(); 6 7 / Case 2: Share if possible, returning a const CvImage 8 CvImageConstPtr toCvShare(const sensor_msgs:ImageConstPtr& source, 9 const std:string& encoding = std:string(); 10 CvImageConstPtr toCvShare(const sensor_msgs:Image& source, 11 const boost:shared_ptr& tracked_object, 12 const std:string& encoding = std:string();The input is the image message pointer, as well as an optionalencodingargument. The encoding refers to the destinationCvImage.toCvCopycreates a copy of the image data from the ROS message, even when the source and destination encodings match. However, you are free to modify the returnedCvImage.toCvSharewill point the returnedcv:Matat the ROS message data, avoiding a copy, if the source and destination encodings match. As long as you hold a copy of the returnedCvImage, the ROS message data will not be freed. If the encodings do not match, it will allocate a new buffer and perform the conversion. You are not permitted to modify the returnedCvImage, as it may share data with the ROS image message, which in turn may be shared with other callbacks. Note: the second overload oftoCvShareis more convenient when you have a pointer to some other message type (e.g.stereo_msgs/DisparityImage) that contains asensor_msgs/Imageyou want to convert.If no encoding (or rather, the empty string) is given, the destination image encoding will be the same as the image message encoding. In this casetoCvShareis guaranteed to not copy the image data. Image encodings can be any one of the following OpenCV image encodings: 8UC1-4 8SC1-4 16UC1-4 16SC1-4 32SC1-4 32FC1-4 64FC1-4For popular image encodings,CvBridgewill optionallydo color or pixel depth conversions as necessary. To use this feature, specify the encoding to be one of the following strings: mono8:CV_8UC1, grayscale image mono16:CV_16UC1, 16-bit grayscale image bgr8:CV_8UC3, color image with blue-green-red color order rgb8:CV_8UC3, color image with red-green-blue color order bgra8:CV_8UC4, BGR color image with an alpha channel rgba8:CV_8UC4, RGB color image with an alpha channelNote thatmono8andbgr8are the two image encodings expected by most OpenCV functions.Finally,CvBridgewill recognize Bayer pattern encodings as having OpenCV type8UC1(8-bit unsigned, one channel). It will not perform conversions to or from Bayer pattern; in a typical ROS system, this is done instead byimage_proc.CvBridgerecognizes the following Bayer encodings: bayer_rggb8 bayer_bggr8 bayer_gbrg8 bayer_grbg8Converting OpenCV images to ROS image messagesTo convert aCvImageinto a ROS image message, use one thetoImageMsg()member function:Toggle line numbers 1 class CvImage 2 3 sensor_msgs:ImagePtr toImageMsg() const; 4 5 / Overload mainly intended for aggregate messages that contain 6 / a sensor_msgs:Image as a member. 7 void toImageMsg(sensor_msgs:Image& ros_image) const; 8 ;If theCvImageis one you have allocated yourself, dont forget to fill in theheaderandencodingfields.An example ROS nodeHere is a node that listens to a ROS image message topic, converts the image into acv:Mat, draws a circle on it and displays the image using OpenCV. The image is then republished over ROS.In your package.xml and CMakeLists.xml (or when you usecatkin_create_pkg), add the following dependencies:sensor_msgscv_bridgeroscppstd_msgsimage_transportCreate a image_converter.cpp file in your /src folder and add the following:Toggle line numbers 1 #include 2 #include 3 #include 4 #include 5 #include 6 #include 7 8 static const std:string OPENCV_WINDOW = Image window; 9 10 class ImageConverter 11 12 ros:NodeHandle nh_; 13 image_transport:ImageTransport it_; 14 image_transport:Subscriber image_sub_; 15 image_transport:Publisher image_pub_; 16 17 public: 18 ImageConverter() 19 : it_(nh_) 20 21 / Subscrive to input video feed and publish output video feed 22 image_sub_ = it_.subscribe(/camera/image_raw, 1, 23 &ImageConverter:imageCb, this); 24 image_pub_ = it_.advertise(/image_converter/output_video, 1); 25 26 cv:namedWindow(OPENCV_WINDOW); 27 28 29 ImageConverter() 30 31 cv:destroyWindow(OPENCV_WINDOW); 32 33 34 void imageCb(const sensor_msgs:ImageConstPtr& msg) 35 36 cv_bridge:CvImagePtr cv_ptr; 37 try 38 39 cv_ptr = cv_bridge:toCvCopy(msg, sensor_msgs:image_encodings:BGR8); 40 41 catch (cv_bridge:Exception& e) 42 43 ROS_ERROR(cv_bridge exception: %s, e.what(); 44 return; 45 46 47 / Draw an example circle on the video stream 48 if (cv_ptr-image.rows 60 & cv_ptr-image.cols 60) 49 cv:circle(cv_ptr-image, cv:Point(50, 50), 10, CV_RGB(255,0,0); 50 51 / Update GUI Window 52 cv:imshow(OPENCV_WINDOW, cv_ptr-image); 53 cv:waitKey(3); 54 55 / Output modified video stream 56 image_pub_.publish(cv_ptr-toImageMsg(); 57 58 ; 59 60 int main(int argc, char* argv) 61 62 ros:init(argc, argv, image_converter); 63 ImageConverter ic; 64 ros:spin(); 65 return 0; 66 Lets break down the above node:Toggle line numbers 2 #include 3 Usingimage_transportfor publishing and subscribing to images in ROS allows you to subscribe to compressed image streams. Remember to includeimage_transportin your package.xml.Toggle line numbers 3 #include 4 #include 5 Includes the header forCvBridgeas well as some useful constants and functions related toimage encodings. Remember to includecv_bridgein your package.xml.Toggle line numbers 5 #include 6 #include 7 Includes the headers for OpenCVs image processing and GUI modules. Remember to includeopencv2in your package.xml.Toggle line numbers 12 ros:NodeHandle nh_; 13 image_transport:ImageTransport it_; 14 image_transport:Subscriber image_sub_; 15 image_transport:Publisher image_pub_; 16 17 public: 18 ImageConverter() 19 : it_(nh_) 20 21 / Subscrive to input video feed and publish output video feed 22 image_sub_ = it_.subscribe(/camera/image_raw, 1, 23 &ImageConverter:imageCb, this); 24 image_pub_ = it_.advertise(/image_converter/output_video, 1);Subscribe to an image topic in and advertise an image topic out usingimage_transport.Toggle line numbers 26 cv:namedWindow(OPENCV_WINDOW); 27 28 29 ImageConverter() 30 31 cv:destroyWindow(OPENCV_WINDOW); 32 OpenCV HighGUIcalls to create/destroy a display window on start-up/shutdown.Toggle line numbers 34 void imageCb(const sensor_msgs:ImageConstPtr& msg) 35 36 cv_bridge:CvImagePtr cv_ptr; 37 try 38 39 cv_ptr = cv_bridge:toCvCopy(msg, sensor_msgs:image_encodings:BGR8); 40 41 catch (cv_bridge:Exception& e) 42 43 ROS_ERROR(cv_bridge exception: %s, e.what(); 44 return; 45 In our subscriber callback, we first convert the ROS image message to aCvImagesuitable for working with OpenCV. Since were going to draw on the image, we need a mutable copy of it, so we usetoCvCopy().sensor_msgs:image_encodings:BGR8is simply a constant for bgr8, but less susceptible to typos.Note that OpenCV expects color images to use BGR channel order.You should always wrap your calls totoCvCopy()/toCvShared()to catch conversion errors as those functions will not check for the validity of your data.Toggle line numbers 47 / Draw an example circle on the video stream 48 if (cv_ptr-image.rows 60 & cv_ptr-image.cols 60) 49 cv:circle(cv_ptr-image, cv:Point(50, 50), 10, CV_RGB(255,0,0); 50 51 / Update GUI Window 52 Draw a red circle on the image, then show it in the display window.Toggle line numbers 53 cv:waitKey(3);Convert theCvImageto a ROS image message and publish it on the out topic.To run the node, you will need an image stream. Run a camera or play a bag file to generate the image stream. Now you can run this node,remappingin to the actual image stream topic.If you have successfully converted images to OpenCV format, you will see a HighGui window with the name Image window and your image+circle displayed.You can see whether your node is correctly publishing images over ROS using eitherrostopicor by viewing the images usingimage_view.Examples of sharing the image dataIn the complete example above, we explicitly copied the image data, but sharing (when po