rgb_feature_matcher.cpp
Go to the documentation of this file.
00001 /*
00002  * rgb_feature_matcher.cpp
00003  *
00004  *  Created on: Sep 26, 2012
00005  *      Author: kidson
00006  */
00007 
00008 #include "rgbd_registration/rgb_feature_matcher.h"
00009 #include "rgbd_registration/rgb_feature_detection.h"
00010 #include "rgbd_registration/ransac_transformation.h"
00011 #include "rgbd_registration/parameter_server.h"
00012 
00013 #include "opencv2/highgui/highgui.hpp"
00014 
00015 // converts rgb point clouds to images
00016 
00017 RGBFeatureMatcher::RGBFeatureMatcher (PointCloudPtr source_cloud_ptr,
00018     PointCloudPtr target_cloud_ptr) :
00019   source_cloud_ptr_ (source_cloud_ptr), target_cloud_ptr_ (target_cloud_ptr)
00020 {
00021   source_image_ = this->restoreCVMatFromPointCloud (source_cloud_ptr_);
00022   target_image_ = this->restoreCVMatFromPointCloud (target_cloud_ptr_);
00023 }
00024 
00025 // if the point cloud and images are separated
00026 
00027 RGBFeatureMatcher::RGBFeatureMatcher (PointCloudPtr source_cloud_ptr,
00028     PointCloudPtr target_cloud_ptr, const cv::Mat& source_image, const cv::Mat& target_image) :
00029   source_cloud_ptr_ (source_cloud_ptr), target_cloud_ptr_ (target_cloud_ptr), source_image_ (
00030       source_image), target_image_ (target_image)
00031 {
00032 }
00033 
00034 RGBFeatureMatcher::RGBFeatureMatcher ()
00035 {
00036   // TODO Auto-generated destructor stub
00037 }
00038 
00039 RGBFeatureMatcher::~RGBFeatureMatcher ()
00040 {
00041   // TODO Auto-generated destructor stub
00042 }
00043 
00044 void RGBFeatureMatcher::setSourceCloud (const PointCloudPtr source_cloud)
00045 {
00046   source_cloud_ptr_ = source_cloud;
00047 }
00048 
00049 PointCloudConstPtr RGBFeatureMatcher::getSourceCloud ()
00050 {
00051   return source_cloud_ptr_;
00052 }
00053 
00054 void RGBFeatureMatcher::setTargetCloud (const PointCloudPtr target_cloud)
00055 {
00056   target_cloud_ptr_ = target_cloud;
00057 }
00058 
00059 PointCloudConstPtr RGBFeatureMatcher::getTargetCloud ()
00060 {
00061   return target_cloud_ptr_;
00062 }
00063 
00064 void RGBFeatureMatcher::setSourceImage (const cv::Mat& source_image)
00065 {
00066   source_image_ = source_image;
00067 }
00068 cv::Mat RGBFeatureMatcher::getSourceImage ()
00069 {
00070   return source_image_;
00071 }
00072 void RGBFeatureMatcher::setTargetImage (const cv::Mat& target_image)
00073 {
00074   target_image_ = target_image;
00075 }
00076 cv::Mat RGBFeatureMatcher::getTargetImage ()
00077 {
00078   return target_image_;
00079 }
00080 
00081 cv::Mat RGBFeatureMatcher::restoreCVMatFromPointCloud (PointCloudConstPtr cloud_in)
00082 {
00083   cv::Mat restored_image = cv::Mat (cloud_in->height, cloud_in->width, CV_8UC3);
00084   for (uint rows = 0; rows < cloud_in->height; rows++)
00085   {
00086     for (uint cols = 0; cols < cloud_in->width; ++cols)
00087     {
00088       //      restored_image.at<uint8_t>(rows, cols) = cloud_in->at(cols, rows).r;
00089       restored_image.at<cv::Vec3b> (rows, cols)[0] = cloud_in->at (cols, rows).b;
00090       restored_image.at<cv::Vec3b> (rows, cols)[1] = cloud_in->at (cols, rows).g;
00091       restored_image.at<cv::Vec3b> (rows, cols)[2] = cloud_in->at (cols, rows).r;
00092     }
00093   }
00094   return restored_image;
00095 }
00096 
00097 bool RGBFeatureMatcher::getMatches (std::vector<Eigen::Vector4f, Eigen::aligned_allocator<
00098     Eigen::Vector4f> >& source_inlier_3d_locations, std::vector<Eigen::Vector4f,
00099     Eigen::aligned_allocator<Eigen::Vector4f> >& target_inlier_3d_locations,
00100     Eigen::Matrix4f& ransac_trafo)
00101 {
00102   if (source_image_.empty () || target_image_.empty ())
00103   {
00104     ROS_ERROR("Image not set for RGBFeatureMatcher");
00105     return 0;
00106   }
00107   // Extract RGB features and project into 3d
00108   RGBFeatureDetection RGB_feature_detector;
00109   std::vector<cv::KeyPoint> source_keypoints, target_keypoints;
00110   cv::Mat source_descriptors, target_descriptors;
00111   std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> >
00112       source_feature_3d_locations, target_feature_3d_locations;
00113 
00114   RGB_feature_detector.detectFeatures (source_image_, source_keypoints);
00115   RGB_feature_detector.detectFeatures (target_image_, target_keypoints);
00116 
00117   RGB_feature_detector.projectFeaturesTo3D (source_keypoints, source_feature_3d_locations,
00118       source_cloud_ptr_);
00119   ROS_INFO_STREAM("[RGBFeatureMatcher] Found " << source_keypoints.size() << " valid keypoints in source frame");
00120   RGB_feature_detector.projectFeaturesTo3D (target_keypoints, target_feature_3d_locations,
00121       target_cloud_ptr_);
00122   ROS_INFO_STREAM("[RGBFeatureMatcher] Found " << target_keypoints.size() << " valid keypoints in target frame");
00123 
00124   RGB_feature_detector.extractFeatures (source_image_, source_keypoints, source_descriptors);
00125   RGB_feature_detector.extractFeatures (target_image_, target_keypoints, target_descriptors);
00126 
00127   // Match features using opencv (doesn't consider depth info)
00128   std::vector<cv::DMatch> matches, good_matches;
00129   this->findMatches (source_descriptors, target_descriptors, matches);
00130 
00131   // Run Ransac to remove outliers and obtain a transformation between clouds
00132   RansacTransformation ransac_transformer;
00133   float rmse = 0.0;
00134   if (!ransac_transformer.getRelativeTransformationTo (source_feature_3d_locations,
00135       target_feature_3d_locations, &matches, ransac_trafo, rmse, good_matches,
00136       ParameterServer::instance ()->get<int> ("minimum_inliers")))
00137   {
00138     ROS_ERROR( "Not enough feature matches between frames.  Adjust 'minimum inliers parameter'");
00139     return false;
00140   }
00141 
00142   // Copy just the inliers to the output vectors
00143   for (std::vector<cv::DMatch>::iterator itr = good_matches.begin (); itr != good_matches.end (); ++itr)
00144   {
00145     source_inlier_3d_locations.push_back (source_feature_3d_locations.at (itr->queryIdx));
00146     target_inlier_3d_locations.push_back (target_feature_3d_locations.at (itr->trainIdx));
00147   }
00148 
00149   if (ParameterServer::instance ()->get<bool> ("show_feature_matching"))
00150   {
00151     cv::Mat img_matches;
00152     cv::drawMatches (source_image_, source_keypoints, target_image_, target_keypoints, matches,
00153         img_matches, cv::Scalar::all (-1), cv::Scalar::all (-1), std::vector<char> (),
00154         cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
00155     cv::imshow ("Matches", img_matches);
00156     cv::moveWindow ("Matches", -10, 0);
00157 
00158     cv::drawMatches (source_image_, source_keypoints, target_image_, target_keypoints,
00159         good_matches, img_matches, cv::Scalar::all (-1), cv::Scalar::all (-1),
00160         std::vector<char> (), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
00161     cv::imshow ("Good Matches", img_matches);
00162     cv::moveWindow ("Good Matches", -10, 500);
00163     ROS_WARN("[RGBFeatureMatcher] Displaying feature matches.  Please press any key on the image to continue........");
00164     cv::waitKey (0);
00165     cv::destroyAllWindows ();
00166   }
00167   return true;
00168 }
00169 
00170 void RGBFeatureMatcher::findMatches (const cv::Mat& source_descriptors,
00171     const cv::Mat& target_descriptors, std::vector<cv::DMatch>& matches)
00172 {
00173   cv::DescriptorMatcher* matcher;
00174   if (ParameterServer::instance ()->get<std::string> ("descriptor_matcher") == "FLANN")
00175     matcher = new cv::FlannBasedMatcher;
00176   else if (ParameterServer::instance ()->get<std::string> ("descriptor_matcher") == "Bruteforce")
00177     matcher = new cv::BFMatcher (cv::NORM_L1, false);
00178   else
00179   {
00180     ROS_WARN ("descriptor_matcher parameter not correctly set, defaulting to FLANN");
00181     matcher = new cv::FlannBasedMatcher;
00182   }
00183   matcher->match (source_descriptors, target_descriptors, matches);
00184 }
00185 
00186 // crude outlier removal implementation.  RANSAC is preferred to find outliers
00187 
00188 void RGBFeatureMatcher::OutlierRemoval (const std::vector<cv::DMatch>& matches, std::vector<
00189     cv::DMatch>& good_matches)
00190 {
00191   // Outlier detection
00192   double max_dist = 0;
00193   double min_dist = 100;
00194 
00195   //-- Quick calculation of max and min distances between keypoints
00196   for (uint i = 0; i < matches.size (); i++)
00197   {
00198     double dist = matches[i].distance;
00199     if (dist < min_dist)
00200       min_dist = dist;
00201     if (dist > max_dist)
00202       max_dist = dist;
00203   }
00204 
00205   printf ("-- Max dist : %f \n", max_dist);
00206   printf ("-- Min dist : %f \n", min_dist);
00207 
00208   //-- Find only "good" matches (i.e. whose distance is less than 2*min_dist )
00209   //-- PS.- radiusMatch can also be used here.
00210   for (uint i = 0; i < matches.size (); i++)
00211   {
00212     if (matches[i].distance < 4 * min_dist)
00213       good_matches.push_back (matches[i]);
00214   }
00215   for (uint i = 0; i < good_matches.size (); i++)
00216   {
00217     printf ("-- Good Match [%d] Keypoint 1: %d  -- Keypoint 2: %d  \n", i,
00218         good_matches[i].queryIdx, good_matches[i].trainIdx);
00219   }
00220 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


rgbd_registration
Author(s): Ross Kidson
autogenerated on Thu May 23 2013 15:36:53