Publish stereo matches numerically

srv · Sep 16, 2016 · cb51a7d · cb51a7d
1 parent e98511a
commit cb51a7d
Show file tree

Hide file tree

Showing 3 changed files with 46 additions and 28 deletions.
diff --git a/include/publisher.h b/include/publisher.h
@@ -8,15 +8,18 @@
 
 #include <ros/ros.h>
 #include <sensor_msgs/Image.h>
+#include <std_msgs/Int32.h>
 #include <cv_bridge/cv_bridge.h>
 
 #include <opencv2/opencv.hpp>
 #include <opencv2/features2d.hpp>
+#include <boost/lexical_cast.hpp>
 
 #include "tracking.h"
 #include "frame.h"
 
 using namespace std;
+using namespace boost;
 
 namespace slam
 {
@@ -57,7 +60,8 @@ class Publisher
 private:
 
   ros::Publisher pub_clustering_; //!> Publisher for the frame clustering
-  ros::Publisher pub_stereo_matches_; //!> Publisher for the frame stereo matches
+  ros::Publisher pub_stereo_matches_img_; //!> Publisher for the frame stereo matches
+  ros::Publisher pub_stereo_matches_num_; //!> Publisher for the frame stereo matches
 
 };
 

diff --git a/src/loop_closing.cpp b/src/loop_closing.cpp
@@ -1,4 +1,4 @@
-#include <std_msgs/String.h>
+#include <std_msgs/Int32.h>
 #include <image_geometry/pinhole_camera_model.h>
 
 #include <numeric>
@@ -14,9 +14,9 @@ namespace slam
   LoopClosing::LoopClosing()
   {
     ros::NodeHandle nhp("~");
-    pub_num_keyframes_ = nhp.advertise<std_msgs::String>("keyframes", 2, true);
-    pub_num_lc_ = nhp.advertise<std_msgs::String>("loop_closings", 2, true);
-    pub_queue_ = nhp.advertise<std_msgs::String>("loop_closing_queue", 2, true);
+    pub_num_keyframes_ = nhp.advertise<std_msgs::Int32>("keyframes", 2, true);
+    pub_num_lc_ = nhp.advertise<std_msgs::Int32>("loop_closings", 2, true);
+    pub_queue_ = nhp.advertise<std_msgs::Int32>("loop_closing_queue", 2, true);
     pub_lc_matchings_ = nhp.advertise<sensor_msgs::Image>("loop_closing_matchings", 2, true);
   }
 
@@ -69,21 +69,21 @@ namespace slam
         // Publish loop closing information
         if (pub_num_keyframes_.getNumSubscribers() > 0)
         {
-          std_msgs::String msg;
-          msg.data = lexical_cast<string>(graph_->getFrameNum());
+          std_msgs::Int32 msg;
+          msg.data = lexical_cast<int>(graph_->getFrameNum());
           pub_num_keyframes_.publish(msg);
         }
         if (pub_num_lc_.getNumSubscribers() > 0)
         {
-          std_msgs::String msg;
-          msg.data = lexical_cast<string>(cluster_lc_found_.size());
+          std_msgs::Int32 msg;
+          msg.data = lexical_cast<int>(cluster_lc_found_.size());
           pub_num_lc_.publish(msg);
         }
         if (pub_queue_.getNumSubscribers() > 0)
         {
           mutex::scoped_lock lock(mutex_cluster_queue_);
-          std_msgs::String msg;
-          msg.data = lexical_cast<string>(cluster_queue_.size());
+          std_msgs::Int32 msg;
+          msg.data = lexical_cast<int>(cluster_queue_.size());
           pub_queue_.publish(msg);
         }
       }

diff --git a/src/publisher.cpp b/src/publisher.cpp
@@ -10,7 +10,8 @@ namespace slam
   {
     ros::NodeHandle nhp("~");
     pub_clustering_ = nhp.advertise<sensor_msgs::Image>("keypoints_clustering", 2, true);
-    pub_stereo_matches_ = nhp.advertise<sensor_msgs::Image>("stereo_matches", 2, true);
+    pub_stereo_matches_img_ = nhp.advertise<sensor_msgs::Image>("stereo_matches_img", 2, true);
+    pub_stereo_matches_num_ = nhp.advertise<std_msgs::Int32>("stereo_matches_num", 2, true);
   }
 
   void Publisher::publishClustering(const Frame frame)
@@ -21,7 +22,8 @@ namespace slam
 
   void Publisher::publishStereoMatches(const Frame frame)
   {
-    if (pub_stereo_matches_.getNumSubscribers() > 0)
+    if (pub_stereo_matches_img_.getNumSubscribers() > 0 ||
+        pub_stereo_matches_num_.getNumSubscribers() > 0)
       drawStereoMatches(frame);
   }
 
@@ -67,24 +69,36 @@ namespace slam
     vector<cv::KeyPoint> l_kp = frame.getNonFilteredLeftKp();
     vector<cv::KeyPoint> r_kp = frame.getNonFilteredRightKp();
     vector<cv::DMatch> matches = frame.getMatches();
-    cv::drawMatches(l_img, l_kp, r_img, r_kp, matches, out);
 
-    // Draw text
-    stringstream s;
-    int baseline = 0;
-    s << " Number of matches: " << matches.size();
-    cv::Size text_size = cv::getTextSize(s.str(), cv::FONT_HERSHEY_PLAIN, 1.5, 1, &baseline);
-    cv::Mat im_text = cv::Mat(out.rows + text_size.height + 20, out.cols, out.type());
-    out.copyTo(im_text.rowRange(0, out.rows).colRange(0, out.cols));
-    im_text.rowRange(out.rows, im_text.rows).setTo(cv::Scalar(255,255,255));
-    cv::putText(im_text, s.str(), cv::Point(5, im_text.rows - 10), cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(0,0,0), 2, 8);
+    if (pub_stereo_matches_img_.getNumSubscribers() > 0)
+    {
+      cv::drawMatches(l_img, l_kp, r_img, r_kp, matches, out);
 
+      // Draw text
+      stringstream s;
+      int baseline = 0;
+      s << " Number of matches: " << matches.size();
+      cv::Size text_size = cv::getTextSize(s.str(), cv::FONT_HERSHEY_PLAIN, 1.5, 1, &baseline);
+      cv::Mat im_text = cv::Mat(out.rows + text_size.height + 20, out.cols, out.type());
+      out.copyTo(im_text.rowRange(0, out.rows).colRange(0, out.cols));
+      im_text.rowRange(out.rows, im_text.rows).setTo(cv::Scalar(255,255,255));
+      cv::putText(im_text, s.str(), cv::Point(5, im_text.rows - 10), cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(0,0,0), 2, 8);
+
+      // Publish
+      cv_bridge::CvImage ros_image;
+      ros_image.image = im_text.clone();
+      ros_image.header.stamp = ros::Time::now();
+      ros_image.encoding = "bgr8";
+      pub_stereo_matches_img_.publish(ros_image.toImageMsg());
+    }
+
+    if (pub_stereo_matches_num_.getNumSubscribers() > 0)
+    {
+      std_msgs::Int32 msg;
+      msg.data = lexical_cast<int>(matches.size());
+      pub_stereo_matches_num_.publish(msg);
+    }
 
-    cv_bridge::CvImage ros_image;
-    ros_image.image = im_text.clone();
-    ros_image.header.stamp = ros::Time::now();
-    ros_image.encoding = "bgr8";
-    pub_stereo_matches_.publish(ros_image.toImageMsg());
   }
 
 } //namespace slam