機器人視覺專案：視覺檢測識別+機器人跟隨（24）--surf--flann
阿新 • • 發佈：2018-12-14
行人跟隨中的extract+match feature
在小強機器人行人跟隨功能包中加入影象特徵提取和特徵匹配，提取surf特徵，匹配方法是快速最近鄰近似搜尋flann。
直接上程式碼：
/****************************************************************************** * * The MIT License (MIT) * * Copyright (c) 2018 Bluewhale Robot * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in all * copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE * SOFTWARE. * * Author: Randoms *******************************************************************************/ #include "tracking_node.h" using namespace cv; using namespace std; using namespace XiaoqiangTrack; sensor_msgs::Image last_frame; XiaoqiangTrack::Tracker *tracker = NULL; Rect2d body_rect; ros::Publisher image_pub; ros::Publisher target_pub; std::mutex update_track_mutex; int track_ok_flag = 0; cv::Rect2d previous_body_rect; cv::Rect2d body_track_rect; sensor_msgs::Image get_one_frame() { return last_frame; } void update_frame(const sensor_msgs::ImageConstPtr &new_frame) //更新幀 { last_frame = *new_frame; cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(new_frame, "bgr8"); //影象格式轉換 cv::Mat cv_image = cv_ptr->image; if (tracker == NULL) return; unique_lock<mutex> lock(update_track_mutex); previous_body_rect = body_rect;//將檢測到的當前rect儲存為previous_body_rect track_ok_flag = tracker->updateFrame(cv_image, body_rect); cv::rectangle(cv_image, body_rect, cv::Scalar(0, 255, 0)); image_pub.publish(cv_ptr->toImageMsg()); xiaoqiang_track::TrackTarget target; target.x = body_rect.x + body_rect.width / 2; target.y = body_rect.y + body_rect.height / 2; target.width = body_track_rect.width; target.height = body_track_rect.height; if (track_ok_flag == 0) { // send stop target.x = 0; target.y = 0; } target_pub.publish(target);//target.x,target.y是跟蹤的點的座標，kalman... } int main(int argc, char **argv) { ros::init(argc, argv, "xiaoqiang_track_node"); // ros節點初始化 //在一個節點中開闢多個執行緒，構造時可以指定執行緒數如(4)，AsyncSpinner有start()和stop()函式 ros::AsyncSpinner spinner(4); spinner.start(); ros::NodeHandle private_nh("~"); ros::Publisher talk_pub = private_nh.advertise<std_msgs::String>("text", 10); image_pub = private_nh.advertise<sensor_msgs::Image>("processed_image", 10); target_pub = private_nh.advertise<xiaoqiang_track::TrackTarget>("target", 10); int watch_dog; private_nh.param("watch_dog", watch_dog, 2); ros::Subscriber image_sub = private_nh.subscribe("image", 10, update_frame); PoseTracker *client; std::string pose_tracker_type; ros::param::param<std::string>("~pose_tracker", pose_tracker_type, ""); if (pose_tracker_type == "baidu") //判斷跟蹤型別：baidu track or body track { client = (PoseTracker *)new BaiduTrack(private_nh); } else if (pose_tracker_type == "body_pose") { client = (PoseTracker *)new BodyTrack(private_nh); } else { ROS_FATAL_STREAM("unknown pose tracker type " << pose_tracker_type); ROS_FATAL_STREAM("supported pose trakers are body_pose and baidu"); exit(1); } std::string tracker_main_type; //定義主跟蹤型別 std::string tracker_aided_type; //輔跟蹤 ros::param::param<std::string>("~tracker_main", tracker_main_type, ""); ros::param::param<std::string>("~tracker_aided", tracker_aided_type, ""); tracker = new XiaoqiangTrack::Tracker(tracker_main_type,tracker_aided_type); //設定跟蹤器 // 告訴使用者站在前面 std_msgs::String words; words.data = "請站在我前面"; talk_pub.publish(words); // 提醒使用者調整好距離 sensor_msgs::Image frame = get_one_frame(); //得到一幀影象 body_rect.x = -1; body_rect.y = -1; while (!ros::isShuttingDown()) { if (frame.data.size() != 0) { cv::Rect2d rect = client->getBodyRect(frame); //通過frame得到人體影象區域 if (rect.x <= 1 || rect.y <= 1) { words.data = "我沒有看到人,請站到我前面"; talk_pub.publish(words); } else if (rect.x + rect.width / 2 > 440 || rect.x + rect.width / 2 < 200) { body_rect = rect; words.data = "請站到鏡頭中間來"; talk_pub.publish(words); } else { body_rect = rect; words.data = "我看到人了,開始追蹤"; talk_pub.publish(words); break; } } sleep(1); frame = get_one_frame(); } /* ￥經過分析程式碼，初步的想法是在這個位置加上特徵提取方法和Opencv的特徵匹配，思路是： - 特徵提取是從一幀影象中提取特徵，想要提取的特徵可以是ORB，FAST，SIFT，SURF等，上面的 - frame = get_one_frame()是獲取最新的一幀影象，return last_frame - 那麼對於這一幀影象抽取想要的特徵資訊，得到特徵點，儲存檢測到的目標特徵，之後用來與再次檢測時的影象來做匹配 - 當然這裡是做特徵提取，匹配是在跟蹤丟失後，再次啟動檢測識別時，識別到多個目標，進行匹配 */ /*fuck!sorry,i don't wanna say that,but i just lost everything i did the whole day because of clicking the close table without save it! so let me start at the begining... */ //begining extract feature int minHessian = 2000; SurfFeatureDetector detector(minHessian); vector<KeyPoint>keypoint1, keypoint2; //image1 = resize(frame, rect.x:(rect.x+rect.width), rect.y:(rect.y+rect.higth)) IplImage *img1; CvRect rectInImage1; rectInImage1 = cvRect(rect.x, rect.y,rect.width, rect.height); CvSize size1; size1.width = rectInImage.width; size1.height = rectInImage.height; img1 = CvCreatImage(size1, frame->depth, frame->nChannels); CvSetImageROI(frame, rectInImage1); cvCopy(img1, frame);//img1是從frame上提取的目標框區域 //檢測特徵點 detector.detect(img1, keypoint1) //ending // 告訴使用者可以開始走了 sensor_msgs::Image tracking_frame = get_one_frame(); cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(tracking_frame, "bgr8"); cv::Mat cv_image = cv_ptr->image; tracker->initTracker(cv_image, body_rect); // init int repeat_count = 0; int watch_dog_count = 0; while (ros::ok()) { usleep(1000 * 100); // 100ms // 如果位置不變，則認為可能丟失 if (previous_body_rect == body_rect) { repeat_count += 100; if (repeat_count == 5 * 1000) //rect檢測到的資料不變，且超過一定時間，判Lost { ROS_WARN_STREAM("Target not move, may lost"); repeat_count = 0; track_ok_flag = 0; // heihei,flag=0 -> reset } } //這裡判斷跟丟 else { repeat_count = 0; } if (body_rect.width < 300 && body_rect.height < 300 && track_ok_flag == 2 && body_rect.height > body_rect.width) //確認檢測到的rect符合這些要求 { watch_dog_count += 100; if (watch_dog_count <= watch_dog * 1000) // watch_dog=2 { continue; } } //這裡判斷是否正確的給出rect watch_dog_count = 0; tracking_frame = get_one_frame(); //再次獲得newframe body_track_rect = client->getBodyRect(tracking_frame); // track //usr code begining IplImage *img2; CvRect rectInImage2; ectInImage2 = cvRect(body_track_rect.x, body_track_rect.y, ody_track_rect.width, body_track_rect.height); CvSize size2; size2.width = rectInImage2.width; size2.height = rectInImage2.height; img2 = CvCreatImage(size2, tracking_frame->depth, tracking_frame->nChannels); CvSetImageROI(tracking_frame, rectInImage2); cvCopy(img2, tracking_frame);//img2是從tracking_frame上提取的目標框區域 //檢測特徵點 detector.detect(img2, keypoint2); //計算特徵點描述子 SurfDescriptorExtractor extractor; Mat descriptor1, descriptor2; extractor.compute(img1, keypoint1, descriptor1); extractor.compute(img2, keypoint2, descriptor2); //使用flann匹配 FlannBasedMatcher matcher; vector<DMatch>matches; matcher.match(descriptor1, descriptor2, matches);//匹配結束 double max_dist = 0; double min_dist = 100; for(int i = 0; i < descriptor1.rows; i++) { double dist = matches[i].distance; if(dist < min_dist) min_dist = dist; if(dist > max_dist) max_dist = dist; }//得到匹配結果中的最小距離和最大距離 //處理匹配結果：判斷當前匹配的物件是否為目標，僅根據最大最小匹配距離，能否進行判斷？ if(min_dist < 1.0)// get it~ { tracking_frame = tracking_frame; body_track_rect = body_track_rect; } elseif(max_dist > 5.0)//abandon { continue;//繼續下一幀匹配額 } elseif((max_dist - min_dist) < 2.0) { tracking_frame = tracking_frame; body_track_rect = body_track_rect; } else { continue; } //usr code ending if (body_track_rect.x <= 1 || body_track_rect.y <= 1) //識別到的rect.x / .y小於1後跟蹤停止 { tracker->stop(); } else { { unique_lock<mutex> lock(update_track_mutex); body_rect = body_track_rect; cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(tracking_frame, "bgr8"); cv::Mat cv_image = cv_ptr->image; if (track_ok_flag == 0) //跟蹤標誌為0時跟蹤復位 { tracker->reset(cv_image, body_rect, true); // watch out the reset praram } else tracker->reset(cv_image, body_rect); /// reset } } } }
說下重新編譯時遇到的bug供參考：
bug1
In file included from /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:28:0: /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/include/tracking_node.h:46:33: fatal error: opencv2\core\core.hpp: No such file or directory compilation terminated. xiaoqiang_track/CMakeFiles/tracking_node.dir/build.make:62: recipe for target 'xiaoqiang_track/CMakeFiles/tracking_node.dir/src/tracking_node.cpp.o' failed make[2]: *** [xiaoqiang_track/CMakeFiles/tracking_node.dir/src/tracking_node.cpp.o] Error 1 CMakeFiles/Makefile2:2241: recipe for target 'xiaoqiang_track/CMakeFiles/tracking_node.dir/all' failed make[1]: *** [xiaoqiang_track/CMakeFiles/tracking_node.dir/all] Error 2 Makefile:138: recipe for target 'all' failed make: *** [all] Error 2
bug2
/home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp: In function \u2018int main(int, char**)\u2019: /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:149:3: error: \u2018SurfFeatureDetector\u2019 was not declared in this scope SurfFeatureDetector detector(minHessian); ^ /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:157:25: error: \u2018rect\u2019 was not declared in this scope rectInImage1 = cvRect(rect.x, rect.y,rect.width, rect.height); ^ /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:159:17: error: \u2018rectInImage\u2019 was not declared in this scope size1.width = rectInImage.width; ^ /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:161:35: error: base operand of \u2018->\u2019 has non-pointer type \u2018sensor_msgs::Image {aka sensor_msgs::Image_<std::allocator<void> >}\u2019 img1 = CvCreatImage(size1, frame->depth, frame->nChannels); ^ /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:161:49: error: base operand of \u2018->\u2019 has non-pointer type \u2018sensor_msgs::Image {aka sensor_msgs::Image_<std::allocator<void> >}\u2019 img1 = CvCreatImage(size1, frame->depth, frame->nChannels); ^ /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:161:60: error: \u2018CvCreatImage\u2019 was not declared in this scope img1 = CvCreatImage(size1, frame->depth, frame->nChannels); ^ /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:162:36: error: \u2018CvSetImageROI\u2019 was not declared in this scope CvSetImageROI(frame, rectInImage1); ^ /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:163:21: error: cannot convert \u2018sensor_msgs::Image {aka sensor_msgs::Image_<std::allocator<void> >}\u2019 to \u2018CvArr* {aka void*}\u2019 for argument \u20182\u2019 to \u2018void cvCopy(const CvArr*, CvArr*, const CvArr*)\u2019 cvCopy(img1, frame);//img1\u662f\u4eceframe\u4e0a\u63d0\u53d6\u7684\u76ee\u6807\u6846\u533a\u57df ^ /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:165:3: error: \u2018detector\u2019 was not declared in this scope detector.detect(img1, keypoint1) ^ /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:171:54: error: \u2018tracking_frame\u2019 was not declared in this scope cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(tracking_frame, "bgr8"); ^ /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:211:3: error: \u2018ectInImage2\u2019 was not declared in this scope ectInImage2 = cvRect(body_track_rect.x, body_track_rect.y, ^ /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:212:24: error: \u2018ody_track_rect\u2019 was not declared in this scope ody_track_rect.width, body_track_rect.height); ^ /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:223:3: error: \u2018SurfDescriptorExtractor\u2019 was not declared in this scope SurfDescriptorExtractor extractor; ^ /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:226:3: error: \u2018extractor\u2019 was not declared in this scope extractor.compute(img1, keypoint1, descriptor1); ^ /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:251:24: error: \u2018elseif\u2019 was not declared in this scope elseif(max_dist > 5.0)//abandon ^ /home/xiaoqiang/catkin_ww/src/xiaoqiang_track/src/tracking_node.cpp:260:3: error: \u2018else\u2019 without a previous \u2018if\u2019 else ^ xiaoqiang_track/CMakeFiles/tracking_node.dir/build.make:62: recipe for target 'xiaoqiang_track/CMakeFiles/tracking_node.dir/src/tracking_node.cpp.o' failed make[2]: *** [xiaoqiang_track/CMakeFiles/tracking_node.dir/src/tracking_node.cpp.o] Error 1 CMakeFiles/Makefile2:2241: recipe for target 'xiaoqiang_track/CMakeFiles/tracking_node.dir/all' failed make[1]: *** [xiaoqiang_track/CMakeFiles/tracking_node.dir/all] Error 2 Makefile:138: recipe for target 'all' failed make: *** [all] Error 2
這個包在之前沒有修改的時候測試正常，加上特徵的提取和匹配後遇到問題，嘗試解決bug。
注意需要加上標頭檔案：
#include <iostream>
#include <opencv2\core.hpp>
#include <opencv2\highgui.hpp>
#include <opencv2\imgproc.hpp>
#include <opencv2\features2d.hpp>
#include <opencv2\xfeatures2d\nonfree.hpp>這些標頭檔案由於opencv版本號的不同可能會產生一些變動，這裡的是3.3.1。