ROS_OpenCV2_MatchTemplate模板匹配

给一张图片，在视野中找到。获取物体的二维坐标。语音相关的话题注释掉了。

//给一个物体，在视野中查找#include <ros/ros.h>  #include <image_transport/image_transport.h>  #include <cv_bridge/cv_bridge.h>  #include <sensor_msgs/image_encodings.h>  #include <opencv2/imgproc/imgproc.hpp>      #include <opencv2/core/core.hpp>                    #include <opencv2/opencv.hpp>             #include <iostream>          #include<string>          #include <sstream>    #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;  stringstream sss;      static string ImgInfo="/home/hsn/catkin_ws/src/rosopencv/1.jpg";//如果用语音把1.jpg去掉void MTemplate(Mat src);/////////////////////////////////////////////////////从语音获取图像的名字。/*void imgInfoCallback(const std_msgs::String::ConstPtr & msg)  {  ROS_INFO("From SPEECH, I heard: [%s]",msg->data.c_str());  sss.clear();      sss<<ImgInfo;      sss<<msg->data.c_str();      sss<<".jpg";      sss>>ImgInfo;   } *///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;  }  MTemplate(cv_ptr->image);  }  void MTemplate(Mat src){    Mat templ;        Mat result;      int match_method=0;      templ=imread(ImgInfo,1);         int result_cols =  src.cols - templ.cols + 1;          int result_rows = src.rows - templ.rows + 1;            result.create( result_cols, result_rows, CV_32FC1 );            matchTemplate( src, templ, result, match_method );          normalize( result, result, 0, 1, NORM_MINMAX, -1, Mat() );            /// 通过函数 minMaxLoc 定位最匹配的位置          double minVal; double maxVal; Point minLoc; Point maxLoc;          Point matchLoc;            minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );            /// 对于方法 SQDIFF 和 SQDIFF_NORMED, 越小的数值代表更高的匹配结果. 而对于其他方法, 数值越大匹配越好          if( match_method  == CV_TM_SQDIFF || match_method == CV_TM_SQDIFF_NORMED )          {             matchLoc = minLoc;         }          else          {           matchLoc = maxLoc;         }            /// 最终结果          rectangle( src, matchLoc, Point( matchLoc.x + templ.cols , matchLoc.y + templ.rows ), Scalar::all(0), 2, 8, 0 );          rectangle( result, matchLoc, Point( matchLoc.x + templ.cols , matchLoc.y + templ.rows ), Scalar::all(0), 2, 8, 0 );          cout<<"目标的中心坐标 ( "<<matchLoc.x + templ.cols/2<<" , ("<<matchLoc.y + templ.rows<<" )"<<endl;          imshow("当前视频",src);         imshow("视频",result);          waitKey(1);}/** * This is ROS node to track the destination image */  int main(int argc, char **argv)  {  ros::init(argc, argv, "image_processor");  ROS_INFO("-----------------");  ros::NodeHandle nh;  //Create an ImageTransport instance, initializing it with our NodeHandle.  image_transport::ImageTransport it(nh);  //OpenCV HighGUI call to create a display window on start-up.  namedWindow( "CamShift Demo", 0 );  image_transport::Subscriber sub = it.subscribe("camera/rgb/image_raw", 1, imageCallback);/////////////////////////////////////////////////////////////////////////////////ros::Subscriber imgInfo         = nh.subscribe("final_recognizer_begin", 1, imgInfoCallback);//接收来自语音部分的话题   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.");  }