correspondence_viewer.cpp
Go to the documentation of this file.
00001 #include "typedefs.h"
00002 
00003 #include <vector>
00004 #include <string>
00005 #include <sstream>
00006 #include <pcl/console/parse.h>
00007 #include <pcl/io/pcd_io.h>
00008 #include <pcl/kdtree/kdtree_flann.h>
00009 #include <pcl/registration/transforms.h>
00010 #include <pcl/visualization/pcl_visualizer.h>
00011 
00012 /* A few functions to help load up the points */
00013 PointCloudPtr
00014 loadPoints (std::string filename)
00015 {
00016   PointCloudPtr output (new PointCloud);
00017   filename.append (".pcd");
00018   pcl::io::loadPCDFile (filename, *output);
00019   pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ());
00020   return (output);
00021 }
00022 
00023 PointCloudPtr
00024 loadKeypoints (std::string filename)
00025 {
00026   PointCloudPtr output (new PointCloud);
00027   filename.append ("_keypoints.pcd");
00028   pcl::io::loadPCDFile (filename, *output);
00029   pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ());
00030   return (output);
00031 }
00032 
00033 LocalDescriptorsPtr
00034 loadLocalDescriptors (std::string filename)
00035 {
00036   LocalDescriptorsPtr output (new LocalDescriptors);
00037   filename.append ("_localdesc.pcd");
00038   pcl::io::loadPCDFile (filename, *output);
00039   pcl::console::print_info ("Loaded %s (%zu points)\n", filename.c_str (), output->size ());
00040   return (output);
00041 }
00042 
00043 
00044 void
00045 find_feature_correspondences (const LocalDescriptorsPtr & source_descriptors, 
00046                               const LocalDescriptorsPtr & target_descriptors,
00047                               std::vector<int> & correspondences_out, std::vector<float> & correspondence_scores_out)
00048 {
00049   // Resize the output vector
00050   correspondences_out.resize (source_descriptors->size ());
00051   correspondence_scores_out.resize (source_descriptors->size ());
00052 
00053   // Use a KdTree to search for the nearest matches in feature space
00054   pcl::KdTreeFLANN<LocalDescriptorT> descriptor_kdtree;
00055   descriptor_kdtree.setInputCloud (target_descriptors);
00056 
00057   // Find the index of the best match for each keypoint, and store it in "correspondences_out"
00058   const int k = 1;
00059   std::vector<int> k_indices (k);
00060   std::vector<float> k_squared_distances (k);
00061   for (size_t i = 0; i < source_descriptors->size (); ++i)
00062   {
00063     descriptor_kdtree.nearestKSearch (*source_descriptors, i, k, k_indices, k_squared_distances);
00064     correspondences_out[i] = k_indices[0];
00065     correspondence_scores_out[i] = k_squared_distances[0];
00066   }
00067 }
00068 
00069 
00070 void 
00071 visualize_correspondences (const PointCloudPtr points1, const PointCloudPtr keypoints1,
00072                            const PointCloudPtr points2, const PointCloudPtr keypoints2,
00073                            const std::vector<int> &correspondences,
00074                            const std::vector<float> &correspondence_scores, int max_to_display)
00075 { 
00076   // We want to visualize two clouds side-by-side, so do to this, we'll make copies of the clouds and transform them
00077   // by shifting one to the left and the other to the right.  Then we'll draw lines between the corresponding points
00078 
00079   // Create some new point clouds to hold our transformed data
00080   PointCloudPtr points_left (new PointCloud);
00081   PointCloudPtr keypoints_left (new PointCloud);
00082   PointCloudPtr points_right (new PointCloud);
00083   PointCloudPtr keypoints_right (new PointCloud);
00084 
00085   // Shift the first clouds' points to the left
00086   //const Eigen::Vector3f translate (0.0, 0.0, 0.3);
00087   const Eigen::Vector3f translate (0.4, 0.0, 0.0);
00088   const Eigen::Quaternionf no_rotation (0, 0, 0, 0);
00089   pcl::transformPointCloud (*points1, *points_left, -translate, no_rotation);
00090   pcl::transformPointCloud (*keypoints1, *keypoints_left, -translate, no_rotation);
00091 
00092   // Shift the second clouds' points to the right
00093   pcl::transformPointCloud (*points2, *points_right, translate, no_rotation);
00094   pcl::transformPointCloud (*keypoints2, *keypoints_right, translate, no_rotation);
00095 
00096   // Add the clouds to the visualizer
00097   pcl::visualization::PCLVisualizer vis;
00098   vis.addPointCloud (points_left, "points_left");
00099   vis.addPointCloud (points_right, "points_right");
00100 
00101   // Compute the weakest correspondence score to display
00102   std::vector<float> temp (correspondence_scores);
00103   std::sort (temp.begin (), temp.end ());
00104   if (max_to_display >= temp.size ())
00105     max_to_display = temp.size () - 1;
00106   float threshold = temp[max_to_display];
00107 
00108   std::cout << max_to_display << std::endl;
00109 
00110   // Draw lines between the best corresponding points
00111   for (size_t i = 0; i < keypoints_left->size (); ++i)
00112   {
00113     if (correspondence_scores[i] > threshold)
00114     {
00115       continue; // Don't draw weak correspondences
00116     }
00117 
00118     // Get the pair of points
00119     const PointT & p_left = keypoints_left->points[i];
00120     const PointT & p_right = keypoints_right->points[correspondences[i]];
00121 
00122     // Generate a random (bright) color
00123     double r = (rand() % 100);
00124     double g = (rand() % 100);
00125     double b = (rand() % 100);
00126     double max_channel = std::max (r, std::max (g, b));
00127     r /= max_channel;
00128     g /= max_channel;
00129     b /= max_channel;
00130 
00131     // Generate a unique string for each line
00132     std::stringstream ss ("line");
00133     ss << i;
00134 
00135     // Draw the line
00136     vis.addLine (p_left, p_right, r, g, b, ss.str ());
00137   }
00138 
00139   vis.resetCamera ();
00140   vis.spin ();
00141 }
00142 
00143 int 
00144 main (int argc, char ** argv)
00145 {
00146   if (argc < 3) 
00147   {
00148     pcl::console::print_info ("Syntax is: %s source target <options>\n", argv[0]);
00149     pcl::console::print_info ("  where options are:\n");
00150     pcl::console::print_info ("    -n nr_correspondences ... The number of correspondence to draw (Default = all)\n");
00151     pcl::console::print_info ("Note: The inputs (source and target) must be specified without the .pcd extension\n");
00152 
00153     return (1);
00154   }
00155 
00156   PointCloudPtr source_points = loadPoints (argv[1]);
00157   PointCloudPtr source_keypoints = loadKeypoints (argv[1]);
00158   LocalDescriptorsPtr source_descriptors = loadLocalDescriptors (argv[1]);
00159   PointCloudPtr target_points = loadPoints (argv[2]);
00160   PointCloudPtr target_keypoints = loadKeypoints (argv[2]);
00161   LocalDescriptorsPtr target_descriptors = loadLocalDescriptors (argv[2]);
00162 
00163   int nr_correspondences = source_keypoints->size ();
00164   pcl::console::parse_argument (argc, argv, "-n", nr_correspondences);
00165 
00166   std::vector<int> correspondences;
00167   std::vector<float> correspondence_scores;
00168   find_feature_correspondences (source_descriptors, target_descriptors, correspondences, correspondence_scores);
00169   
00170   visualize_correspondences (source_points, source_keypoints, target_points, target_keypoints, 
00171                              correspondences, correspondence_scores, nr_correspondences);
00172 
00173   return (0);
00174 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:23:13