feature_viewer.cpp
Go to the documentation of this file.
00001 
00024 #include "ccny_rgbd/apps/feature_viewer.h"
00025 
00026 namespace ccny_rgbd {
00027   
00028 FeatureViewer::FeatureViewer(
00029   const ros::NodeHandle& nh, 
00030   const ros::NodeHandle& nh_private):
00031   nh_(nh), 
00032   nh_private_(nh_private),
00033   frame_count_(0)
00034 {
00035   ROS_INFO("Starting RGBD Feature Viewer");
00036   
00037   // dynamic reconfigure
00038   FeatureDetectorConfigServer::CallbackType f = boost::bind(
00039     &FeatureViewer::reconfigCallback, this, _1, _2);
00040   config_server_.setCallback(f);
00041   
00042   // **** initialize ROS parameters
00043   
00044   initParams();
00045   
00046   mutex_.lock();
00047   resetDetector();
00048   mutex_.unlock();
00049 
00050   // **** publishers
00051   
00052   cloud_publisher_ = nh_.advertise<PointCloudFeature>(
00053     "feature/cloud", 1);
00054   covariances_publisher_ = nh_.advertise<visualization_msgs::Marker>(
00055     "feature/covariances", 1);
00056   
00057   // **** subscribers
00058   
00059   ImageTransport rgb_it(nh_);
00060   ImageTransport depth_it(nh_);
00061 
00062   sub_rgb_.subscribe(rgb_it,     "/rgbd/rgb",   queue_size_);
00063   sub_depth_.subscribe(depth_it, "/rgbd/depth", queue_size_);
00064   sub_info_.subscribe(nh_,       "/rgbd/info",  queue_size_);
00065 
00066   // Synchronize inputs.
00067   sync_.reset(new RGBDSynchronizer3(
00068                 RGBDSyncPolicy3(queue_size_), sub_rgb_, sub_depth_, sub_info_));
00069   
00070   sync_->registerCallback(boost::bind(&FeatureViewer::RGBDCallback, this, _1, _2, _3));  
00071 }
00072 
00073 FeatureViewer::~FeatureViewer()
00074 {
00075   ROS_INFO("Destroying RGBD Feature Viewer"); 
00076 
00077   //delete feature_detector_;
00078 }
00079 
00080 void FeatureViewer::initParams()
00081 { 
00082   if (!nh_private_.getParam ("queue_size", queue_size_))
00083     queue_size_ = 5;
00084   if (!nh_private_.getParam ("feature/detector_type", detector_type_))
00085     detector_type_ = "GFT";
00086   if (!nh_private_.getParam ("feature/show_keypoints", show_keypoints_))
00087     show_keypoints_ = false;
00088   if (!nh_private_.getParam ("feature/publish_cloud", publish_cloud_))
00089     publish_cloud_ = false;
00090   if (!nh_private_.getParam ("feature/publish_covariances", publish_covariances_))
00091     publish_covariances_ = false;
00092 }
00093 
00094 void FeatureViewer::resetDetector()
00095 {  
00096   gft_config_server_.reset();
00097   star_config_server_.reset();
00098   orb_config_server_.reset();
00099   surf_config_server_.reset();
00100   
00101   if (detector_type_ == "ORB")
00102   { 
00103     ROS_INFO("Creating ORB detector");
00104     feature_detector_.reset(new rgbdtools::OrbDetector());
00105     orb_config_server_.reset(new 
00106       OrbDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/ORB")));
00107     
00108     // dynamic reconfigure
00109     OrbDetectorConfigServer::CallbackType f = boost::bind(
00110       &FeatureViewer::orbReconfigCallback, this, _1, _2);
00111     orb_config_server_->setCallback(f);
00112   }
00113   else if (detector_type_ == "SURF")
00114   {
00115     ROS_INFO("Creating SURF detector");
00116     feature_detector_.reset(new rgbdtools::SurfDetector());
00117     surf_config_server_.reset(new 
00118       SurfDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/SURF")));
00119     
00120     // dynamic reconfigure
00121     SurfDetectorConfigServer::CallbackType f = boost::bind(
00122       &FeatureViewer::surfReconfigCallback, this, _1, _2);
00123     surf_config_server_->setCallback(f);
00124   }
00125   else if (detector_type_ == "GFT")
00126   {
00127     ROS_INFO("Creating GFT detector");
00128     feature_detector_.reset(new rgbdtools::GftDetector());
00129     gft_config_server_.reset(new 
00130       GftDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/GFT")));
00131     
00132     // dynamic reconfigure
00133     GftDetectorConfigServer::CallbackType f = boost::bind(
00134       &FeatureViewer::gftReconfigCallback, this, _1, _2);
00135     gft_config_server_->setCallback(f);
00136   }
00137   else if (detector_type_ == "STAR")
00138   {
00139     ROS_INFO("Creating STAR detector");
00140     feature_detector_.reset(new rgbdtools::StarDetector());
00141     star_config_server_.reset(new 
00142       StarDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/STAR")));
00143     
00144     // dynamic reconfigure
00145     StarDetectorConfigServer::CallbackType f = boost::bind(
00146       &FeatureViewer::starReconfigCallback, this, _1, _2);
00147     star_config_server_->setCallback(f);
00148   }
00149   else
00150   {
00151     ROS_FATAL("%s is not a valid detector type! Using GFT", detector_type_.c_str());
00152     feature_detector_.reset(new rgbdtools::GftDetector());
00153     gft_config_server_.reset(new 
00154       GftDetectorConfigServer(ros::NodeHandle(nh_private_, "feature/GFT")));
00155     
00156     // dynamic reconfigure
00157     GftDetectorConfigServer::CallbackType f = boost::bind(
00158       &FeatureViewer::gftReconfigCallback, this, _1, _2);
00159     gft_config_server_->setCallback(f);
00160   }
00161 }
00162 
00163 void FeatureViewer::RGBDCallback(
00164   const ImageMsg::ConstPtr& rgb_msg,
00165   const ImageMsg::ConstPtr& depth_msg,
00166   const CameraInfoMsg::ConstPtr& info_msg)
00167 {
00168   mutex_.lock();
00169   
00170   ros::WallTime start = ros::WallTime::now();
00171 
00172   // create frame
00173   rgbdtools::RGBDFrame frame;
00174   createRGBDFrameFromROSMessages(rgb_msg, depth_msg, info_msg, frame); 
00175 
00176   // find features
00177   feature_detector_->findFeatures(frame);
00178  
00179   ros::WallTime end = ros::WallTime::now();
00180   
00181   // visualize 
00182   
00183   if (show_keypoints_) showKeypointImage(frame);
00184   if (publish_cloud_) publishFeatureCloud(frame);
00185   if (publish_covariances_) publishFeatureCovariances(frame);
00186   
00187   // print diagnostics
00188 
00189   int n_features = frame.keypoints.size();
00190   int n_valid_features = frame.n_valid_keypoints;
00191 
00192   double d_total = 1000.0 * (end - start).toSec();
00193 
00194   printf("[FV %d] %s[%d][%d]: TOTAL %3.1f\n",
00195     frame_count_, detector_type_.c_str(), n_features, n_valid_features, d_total);
00196 
00197   frame_count_++;
00198   
00199   mutex_.unlock();
00200 }
00201 
00202 void FeatureViewer::showKeypointImage(rgbdtools::RGBDFrame& frame)
00203 {
00204   cv::namedWindow("Keypoints", CV_WINDOW_NORMAL);
00205   cv::Mat kp_img(frame.rgb_img.size(), CV_8UC1);
00206   cv::drawKeypoints(frame.rgb_img, frame.keypoints, kp_img);
00207   cv::imshow("Keypoints", kp_img);
00208   cv::waitKey(1);
00209 }
00210 
00211 void FeatureViewer::publishFeatureCloud(rgbdtools::RGBDFrame& frame)
00212 {
00213   PointCloudFeature cloud;
00214   frame.constructFeaturePointCloud(cloud);   
00215   cloud_publisher_.publish(cloud);
00216 }
00217 
00218 void FeatureViewer::publishFeatureCovariances(rgbdtools::RGBDFrame& frame)
00219 {
00220   // create markers
00221   visualization_msgs::Marker marker;
00222   
00223   marker.header.stamp.sec  = frame.header.stamp.sec;
00224   marker.header.stamp.nsec = frame.header.stamp.nsec;
00225   marker.header.seq        = frame.header.seq;
00226   marker.header.frame_id   = frame.header.frame_id;
00227   
00228   marker.type = visualization_msgs::Marker::LINE_LIST;
00229   marker.color.r = 1.0;
00230   marker.color.g = 1.0;
00231   marker.color.b = 1.0;
00232   marker.color.a = 1.0;
00233   marker.scale.x = 0.0025;
00234   marker.action = visualization_msgs::Marker::ADD;
00235   marker.ns = "covariances";
00236   marker.id = 0;
00237   marker.lifetime = ros::Duration();
00238 
00239   for (unsigned int kp_idx = 0; kp_idx < frame.keypoints.size(); ++kp_idx)
00240   {
00241     if (!frame.kp_valid[kp_idx]) continue;
00242     
00243     const Vector3f& kp_mean = frame.kp_means[kp_idx];
00244     const Matrix3f& kp_cov  = frame.kp_covariances[kp_idx];
00245 
00246     // transform Eigen to OpenCV matrices
00247     cv::Mat m(3, 1, CV_64F);
00248     for (int j = 0; j < 3; ++j)
00249       m.at<double>(j, 0) = kp_mean(j, 0);
00250 
00251     cv::Mat cov(3, 3, CV_64F);
00252     for (int j = 0; j < 3; ++j)
00253     for (int i = 0; i < 3; ++i)
00254       cov.at<double>(j, i) = kp_cov(j, i);
00255 
00256     // compute eigenvectors
00257     cv::Mat evl(1, 3, CV_64F);
00258     cv::Mat evt(3, 3, CV_64F);
00259     cv::eigen(cov, evl, evt);
00260 
00261     double mx = m.at<double>(0,0);
00262     double my = m.at<double>(1,0);
00263     double mz = m.at<double>(2,0);
00264 
00265     for (int e = 0; e < 3; ++e)
00266     {
00267       geometry_msgs::Point a;
00268       geometry_msgs::Point b;
00269 
00270       double sigma = sqrt(evl.at<double>(0,e));
00271       double scale = sigma * 3.0;
00272       tf::Vector3 evt_tf(evt.at<double>(e,0), 
00273                          evt.at<double>(e,1), 
00274                          evt.at<double>(e,2));
00275     
00276       a.x = mx + evt_tf.getX() * scale;
00277       a.y = my + evt_tf.getY() * scale;
00278       a.z = mz + evt_tf.getZ() * scale;
00279    
00280       b.x = mx - evt_tf.getX() * scale;
00281       b.y = my - evt_tf.getY() * scale;
00282       b.z = mz - evt_tf.getZ() * scale;
00283 
00284       marker.points.push_back(a);
00285       marker.points.push_back(b);
00286     }
00287   }
00288 
00289   covariances_publisher_.publish(marker);
00290 }
00291 
00292 void FeatureViewer::reconfigCallback(FeatureDetectorConfig& config, uint32_t level)
00293 { 
00294   mutex_.lock();
00295   std::string old_detector_type = detector_type_;
00296   detector_type_ = config.detector_type;
00297   
00298   if(old_detector_type != detector_type_)
00299     resetDetector();   
00300   
00301   feature_detector_->setSmooth(config.smooth);
00302   feature_detector_->setMaxRange(config.max_range);
00303   feature_detector_->setMaxStDev(config.max_stdev);
00304   
00305   publish_cloud_ = config.publish_cloud;
00306   publish_covariances_ = config.publish_covariances;
00307   show_keypoints_ = config.show_keypoints;
00308   
00309   mutex_.unlock();
00310 }
00311 
00312 void FeatureViewer::gftReconfigCallback(GftDetectorConfig& config, uint32_t level)
00313 {
00314   rgbdtools::GftDetectorPtr gft_detector = 
00315     boost::static_pointer_cast<rgbdtools::GftDetector>(feature_detector_);
00316     
00317   gft_detector->setNFeatures(config.n_features);
00318   gft_detector->setMinDistance(config.min_distance); 
00319 }
00320 
00321 void FeatureViewer::starReconfigCallback(StarDetectorConfig& config, uint32_t level)
00322 {
00323   rgbdtools::StarDetectorPtr star_detector = 
00324     boost::static_pointer_cast<rgbdtools::StarDetector>(feature_detector_);
00325     
00326   star_detector->setThreshold(config.threshold);
00327   star_detector->setMinDistance(config.min_distance); 
00328 }
00329 
00330 void FeatureViewer::surfReconfigCallback(SurfDetectorConfig& config, uint32_t level)
00331 {
00332   rgbdtools::SurfDetectorPtr surf_detector = 
00333     boost::static_pointer_cast<rgbdtools::SurfDetector>(feature_detector_);
00334     
00335   surf_detector->setThreshold(config.threshold);
00336 }
00337     
00338 void FeatureViewer::orbReconfigCallback(OrbDetectorConfig& config, uint32_t level)
00339 {
00340   rgbdtools::OrbDetectorPtr orb_detector = 
00341     boost::static_pointer_cast<rgbdtools::OrbDetector>(feature_detector_);
00342     
00343   orb_detector->setThreshold(config.threshold);
00344   orb_detector->setNFeatures(config.n_features);
00345 }
00346 
00347 } //namespace ccny_rgbd
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


ccny_rgbd
Author(s): Ivan Dryanovski
autogenerated on Tue Aug 27 2013 10:34:01