publisher.cpp
Go to the documentation of this file.
00001 #include <opencv2/core/core.hpp>
00002 #include <opencv2/highgui/highgui.hpp>
00003 
00004 #include "publisher.h"
00005 
00006 namespace slam
00007 {
00008 
00009   Publisher::Publisher()
00010   {
00011     ros::NodeHandle nhp("~");
00012     pub_clustering_ = nhp.advertise<sensor_msgs::Image>("keypoints_clustering", 2, true);
00013   }
00014 
00015   void Publisher::publishClustering(const Frame frame)
00016   {
00017     if (pub_clustering_.getNumSubscribers() > 0)
00018       drawKeypointsClustering(frame);
00019   }
00020 
00021   void Publisher::drawKeypointsClustering(const Frame frame)
00022   {
00023     vector< vector<int> > clusters = frame.getClusters();
00024     if (clusters.size() == 0) return;
00025 
00026     cv::Mat img;
00027     frame.getLeftImg().copyTo(img);
00028     vector<cv::KeyPoint> kp = frame.getLeftKp();
00029     cv::RNG rng(12345);
00030     for (uint i=0; i<clusters.size(); i++)
00031     {
00032       cv::Scalar color = cv::Scalar(rng.uniform(0,255), rng.uniform(0, 255), rng.uniform(0, 255));
00033       for (uint j=0; j<clusters[i].size(); j++)
00034         cv::circle(img, kp[clusters[i][j]].pt, 5, color, -1);
00035     }
00036 
00037     // Draw text
00038     stringstream s;
00039     int baseline = 0;
00040     s << " Number of clusters: " << clusters.size();
00041     cv::Size text_size = cv::getTextSize(s.str(), cv::FONT_HERSHEY_PLAIN, 1.5, 1, &baseline);
00042     cv::Mat im_text = cv::Mat(img.rows + text_size.height + 20, img.cols, img.type());
00043     img.copyTo(im_text.rowRange(0, img.rows).colRange(0, img.cols));
00044     im_text.rowRange(img.rows, im_text.rows).setTo(cv::Scalar(255,255,255));
00045     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);
00046 
00047     // Publish
00048     cv_bridge::CvImage ros_image;
00049     ros_image.image = im_text.clone();
00050     ros_image.header.stamp = ros::Time::now();
00051     ros_image.encoding = "bgr8";
00052     pub_clustering_.publish(ros_image.toImageMsg());
00053   }
00054 
00055 } //namespace slam


stereo_slam
Author(s): Pep Lluis Negre
autogenerated on Thu Jul 14 2016 04:09:13