Point Cloud Library (PCL)  1.3.1
ppf_registration.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Alexandru-Eugen Ichim
00005  *                      Willow Garage, Inc
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  * $Id: ppf_registration.hpp 3042 2011-11-01 04:44:47Z svn $
00036  */
00037 
00038 
00039 #ifndef PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
00040 #define PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
00041 
00042 //#include "pcl/registration/ppf_registration.h"
00043 #include <pcl/features/ppf.h>
00044 #include <pcl/common/transforms.h>
00045 
00046 #include <pcl/features/pfh.h>
00048 void
00049 pcl::PPFHashMapSearch::setInputFeatureCloud (PointCloud<PPFSignature>::ConstPtr feature_cloud)
00050 {
00051   // Discretize the feature cloud and insert it in the hash map
00052   feature_hash_map_->clear ();
00053   unsigned int n = sqrt ((float)feature_cloud->points.size ());
00054   int d1, d2, d3, d4;
00055   max_dist_ = -1.0;
00056   alpha_m_.resize (n);
00057   for (size_t i = 0; i < n; ++i)
00058   {
00059     std::vector <float> alpha_m_row (n);
00060     for (size_t j = 0; j < n; ++j)
00061     {
00062       d1 = floor (feature_cloud->points[i*n+j].f1 / angle_discretization_step_);
00063       d2 = floor (feature_cloud->points[i*n+j].f2 / angle_discretization_step_);
00064       d3 = floor (feature_cloud->points[i*n+j].f3 / angle_discretization_step_);
00065       d4 = floor (feature_cloud->points[i*n+j].f4 / distance_discretization_step_);
00066       feature_hash_map_->insert (std::pair<HashKeyStruct, std::pair<size_t, size_t> > (HashKeyStruct (d1, d2, d3, d4), std::pair<size_t, size_t> (i, j)));
00067       alpha_m_row [j] = feature_cloud->points[i*n + j].alpha_m;
00068 
00069       if (max_dist_ < feature_cloud->points[i*n + j].f4)
00070         max_dist_ = feature_cloud->points[i*n + j].f4;
00071     }
00072     alpha_m_[i] = alpha_m_row;
00073   }
00074 
00075   internals_initialized_ = true;
00076 }
00077 
00078 
00080 void
00081 pcl::PPFHashMapSearch::nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4,
00082                                               std::vector<std::pair<size_t, size_t> > &indices)
00083 {
00084   if (!internals_initialized_)
00085   {
00086     PCL_ERROR("[pcl::PPFRegistration::nearestNeighborSearch]: input feature cloud has not been set - skipping search!\n");
00087     return;
00088   }
00089 
00090   int d1 = floor (f1 / angle_discretization_step_),
00091       d2 = floor (f2 / angle_discretization_step_),
00092       d3 = floor (f3 / angle_discretization_step_),
00093       d4 = floor (f4 / distance_discretization_step_);
00094 
00095   indices.clear ();
00096   HashKeyStruct key = HashKeyStruct (d1, d2, d3, d4);
00097   std::pair <FeatureHashMapType::iterator, FeatureHashMapType::iterator> map_iterator_pair = feature_hash_map_->equal_range (key);
00098   for (; map_iterator_pair.first != map_iterator_pair.second; ++ map_iterator_pair.first)
00099     indices.push_back (std::pair<size_t, size_t> (map_iterator_pair.first->second.first,
00100                                                   map_iterator_pair.first->second.second));
00101 }
00102 
00103 
00105 template <typename PointSource, typename PointTarget> void
00106 pcl::PPFRegistration<PointSource, PointTarget>::setInputTarget (const PointCloudTargetConstPtr &cloud)
00107 {
00108   Registration<PointSource, PointTarget>::setInputTarget (cloud);
00109 
00110   scene_search_tree_ = typename pcl::KdTreeFLANN<PointTarget>::Ptr (new pcl::KdTreeFLANN<PointTarget>);
00111   scene_search_tree_->setInputCloud (target_);
00112 }
00113 
00114 
00116 template <typename PointSource, typename PointTarget> void
00117 pcl::PPFRegistration<PointSource, PointTarget>::computeTransformation (PointCloudSource &output)
00118 {
00119   if (!search_method_)
00120   {
00121     PCL_ERROR("[pcl::PPFRegistration::computeTransformation] Search method not set - skipping computeTransformation!\n");
00122     return;
00123   }
00124 
00125   PoseWithVotesList voted_poses;
00126   std::vector <std::vector <unsigned int> > accumulator_array;
00127   accumulator_array.resize (input_->points.size ());
00128   for (size_t i = 0; i < input_->points.size (); ++i)
00129   {
00130     std::vector <unsigned int> aux ((size_t)floor(2*M_PI / search_method_->getAngleDiscretizationStep ()), 0);
00131     accumulator_array[i] = aux;
00132   }
00133   PCL_INFO ("Accumulator array size: %u x %u.\n", accumulator_array.size (), accumulator_array.back ().size ());
00134 
00135   // Consider every <scene_reference_point_sampling_rate>-th point as the reference point => fix s_r
00136   float f1, f2, f3, f4;
00137   for (size_t scene_reference_index = 0; scene_reference_index < target_->points.size (); scene_reference_index += scene_reference_point_sampling_rate_)
00138   {
00139     Eigen::Vector3f scene_reference_point = target_->points[scene_reference_index].getVector3fMap (),
00140         scene_reference_normal = target_->points[scene_reference_index].getNormalVector3fMap ();
00141 
00142     Eigen::AngleAxisf rotation_sg (acos (scene_reference_normal.dot (Eigen::Vector3f::UnitX ())),
00143                                    scene_reference_normal.cross (Eigen::Vector3f::UnitX ()). normalized());
00144     Eigen::Affine3f transform_sg = Eigen::Translation3f ( rotation_sg* ((-1)*scene_reference_point)) * rotation_sg;
00145 
00146     // For every other point in the scene => now have pair (s_r, s_i) fixed
00147     std::vector<int> indices;
00148     std::vector<float> distances;
00149     scene_search_tree_->radiusSearch (target_->points[scene_reference_index],
00150                                      search_method_->getModelDiameter () /2,
00151                                      indices,
00152                                      distances);
00153     for(size_t i = 0; i < indices.size (); ++i)
00154 //    for(size_t i = 0; i < target_->points.size (); ++i)
00155     {
00156       //size_t scene_point_index = i;
00157       size_t scene_point_index = indices[i];
00158       if (scene_reference_index != scene_point_index)
00159       {
00160         if (/*pcl::computePPFPairFeature*/pcl::computePairFeatures (target_->points[scene_reference_index].getVector4fMap (),
00161                                         target_->points[scene_reference_index].getNormalVector4fMap (),
00162                                         target_->points[scene_point_index].getVector4fMap (),
00163                                         target_->points[scene_point_index].getNormalVector4fMap (),
00164                                         f1, f2, f3, f4))
00165         {
00166           std::vector<std::pair<size_t, size_t> > nearest_indices;
00167           search_method_->nearestNeighborSearch (f1, f2, f3, f4, nearest_indices);
00168 
00169           // Compute alpha_s angle
00170           Eigen::Vector3f scene_point = target_->points[scene_point_index].getVector3fMap ();
00171           Eigen::AngleAxisf rotation_sg (acos (scene_reference_normal.dot (Eigen::Vector3f::UnitX ())),
00172                                          scene_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ());
00173           Eigen::Affine3f transform_sg = Eigen::Translation3f ( rotation_sg * ((-1) * scene_reference_point)) * rotation_sg;
00174 //          float alpha_s = acos (Eigen::Vector3f::UnitY ().dot ((transform_sg * scene_point).normalized ()));
00175 
00176           Eigen::Vector3f scene_point_transformed = transform_sg * scene_point;
00177           float alpha_s = atan2f ( -scene_point_transformed(2), scene_point_transformed(1));
00178           if ( alpha_s != alpha_s)
00179           {
00180             PCL_ERROR ("alpha_s is nan\n");
00181             continue;
00182           }
00183           if (sin (alpha_s) * scene_point_transformed(2) < 0.0f)
00184             alpha_s *= (-1);
00185           alpha_s *= (-1);
00186 
00187           // Go through point pairs in the model with the same discretized feature
00188           for (std::vector<std::pair<size_t, size_t> >::iterator v_it = nearest_indices.begin (); v_it != nearest_indices.end (); ++ v_it)
00189           {
00190             size_t model_reference_index = v_it->first,
00191                 model_point_index = v_it->second;
00192             // Calculate angle alpha = alpha_m - alpha_s
00193             float alpha = search_method_->alpha_m_[model_reference_index][model_point_index] - alpha_s;
00194             unsigned int alpha_discretized = floor(alpha) + floor(M_PI / search_method_->getAngleDiscretizationStep ());
00195             accumulator_array[model_reference_index][alpha_discretized] ++;
00196           }
00197         }
00198         else PCL_ERROR ("[pcl::PPFRegistration::computeTransformation] Computing pair feature vector between points %lu and %lu went wrong.\n", (unsigned long) scene_reference_index, (unsigned long) scene_point_index);
00199       }
00200     }
00201 
00202     size_t max_votes_i = 0, max_votes_j = 0;
00203     unsigned int max_votes = 0;
00204 
00205     for (size_t i = 0; i < accumulator_array.size (); ++i)
00206       for (size_t j = 0; j < accumulator_array.back ().size (); ++j)
00207       {
00208         if (accumulator_array[i][j] > max_votes)
00209         {
00210           max_votes = accumulator_array[i][j];
00211           max_votes_i = i;
00212           max_votes_j = j;
00213         }
00214         // Reset accumulator_array for the next set of iterations with a new scene reference point
00215         accumulator_array[i][j] = 0;
00216       }
00217 
00218     Eigen::Vector3f model_reference_point = input_->points[max_votes_i].getVector3fMap (),
00219         model_reference_normal = input_->points[max_votes_i].getNormalVector3fMap ();
00220     Eigen::AngleAxisf rotation_mg (acos (model_reference_normal.dot (Eigen::Vector3f::UnitX ())), model_reference_normal.cross (Eigen::Vector3f::UnitX ()).normalized ());
00221     Eigen::Affine3f transform_mg = Eigen::Translation3f ( rotation_mg * ((-1) * model_reference_point)) * rotation_mg;
00222     Eigen::Affine3f max_transform = transform_sg.inverse () * Eigen::AngleAxisf ( (max_votes_j - floor(M_PI / search_method_->getAngleDiscretizationStep ())) * search_method_->getAngleDiscretizationStep (), Eigen::Vector3f::UnitX ()) * transform_mg;
00223 
00224     voted_poses.push_back (PoseWithVotes (max_transform, max_votes));
00225   }
00226   PCL_INFO ("Done with the Hough Transform ...\n");
00227 
00228   // Cluster poses for filtering out outliers and obtaining more precise results
00229   PoseWithVotesList results;
00230   clusterPoses (voted_poses, results);
00231 
00232   pcl::transformPointCloud (*input_, output, results.front ().pose);
00233 
00234   transformation_ = final_transformation_ = results.front ().pose.matrix ();
00235   converged_ = true;
00236 }
00237 
00238 
00240 template <typename PointSource, typename PointTarget> void
00241 pcl::PPFRegistration<PointSource, PointTarget>::clusterPoses (typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotesList &poses,
00242                                                               typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotesList &result)
00243 {
00244   PCL_INFO ("Clustering poses ...\n");
00245   // Start off by sorting the poses by the number of votes
00246   sort(poses.begin (), poses.end (), poseWithVotesCompareFunction);
00247 
00248   std::vector<PoseWithVotesList> clusters;
00249   std::vector<std::pair<size_t, unsigned int> > cluster_votes;
00250   for (size_t poses_i = 0; poses_i < poses.size(); ++ poses_i)
00251   {
00252     bool found_cluster = false;
00253     for (size_t clusters_i = 0; clusters_i < clusters.size(); ++ clusters_i)
00254     {
00255       if (posesWithinErrorBounds (poses[poses_i].pose, clusters[clusters_i].front ().pose))
00256       {
00257         found_cluster = true;
00258         clusters[clusters_i].push_back (poses[poses_i]);
00259         cluster_votes[clusters_i].second += poses[poses_i].votes;
00260         break;
00261       }
00262     }
00263 
00264     if (found_cluster == false)
00265     {
00266       // Create a new cluster with the current pose
00267       PoseWithVotesList new_cluster;
00268       new_cluster.push_back (poses[poses_i]);
00269       clusters.push_back (new_cluster);
00270       cluster_votes.push_back (std::pair<size_t, unsigned int> (clusters.size () - 1, poses[poses_i].votes));
00271     }
00272  }
00273 
00274   // Sort clusters by total number of votes
00275   std::sort (cluster_votes.begin (), cluster_votes.end (), clusterVotesCompareFunction);
00276   // Compute pose average and put them in result vector
00279   result.clear ();
00280   size_t max_clusters = (clusters.size () < 3) ? clusters.size () : 3;
00281   for (size_t cluster_i = 0; cluster_i < max_clusters; ++ cluster_i)
00282   {
00283     PCL_INFO ("Winning cluster has #votes: %d and #poses voted: %d.\n", cluster_votes[cluster_i].second, clusters[cluster_votes[cluster_i].first].size ());
00284     Eigen::Vector3f translation_average (0.0, 0.0, 0.0);
00285     Eigen::Vector4f rotation_average (0.0, 0.0, 0.0, 0.0);
00286     for (typename PoseWithVotesList::iterator v_it = clusters[cluster_votes[cluster_i].first].begin (); v_it != clusters[cluster_votes[cluster_i].first].end (); ++ v_it)
00287     {
00288       translation_average += v_it->pose.translation ();
00290       rotation_average += Eigen::Quaternionf (v_it->pose.rotation ()).coeffs ();
00291     }
00292 
00293     translation_average /= clusters[cluster_votes[cluster_i].first].size ();
00294     rotation_average /= clusters[cluster_votes[cluster_i].first].size ();
00295 
00296     Eigen::Affine3f transform_average;
00297     transform_average.translation () = translation_average;
00298     transform_average.linear () = Eigen::Quaternionf (rotation_average).normalized().toRotationMatrix ();
00299 
00300     result.push_back (PoseWithVotes (transform_average, cluster_votes[cluster_i].second));
00301   }
00302 }
00303 
00304 
00306 template <typename PointSource, typename PointTarget> bool
00307 pcl::PPFRegistration<PointSource, PointTarget>::posesWithinErrorBounds (Eigen::Affine3f &pose1,
00308                                                                         Eigen::Affine3f &pose2)
00309 {
00310   float position_diff = (pose1.translation () - pose2.translation ()).norm ();
00311   Eigen::AngleAxisf rotation_diff_mat (pose1.rotation ().inverse () * pose2.rotation ());
00312 
00313   float rotation_diff_angle = fabs (rotation_diff_mat.angle ());
00314 
00315   if (position_diff < clustering_position_diff_threshold_ && rotation_diff_angle < clustering_rotation_diff_threshold_)
00316     return true;
00317   else return false;
00318 }
00319 
00320 
00322 template <typename PointSource, typename PointTarget> bool
00323 pcl::PPFRegistration<PointSource, PointTarget>::poseWithVotesCompareFunction (const typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotes &a,
00324                                                                               const typename pcl::PPFRegistration<PointSource, PointTarget>::PoseWithVotes &b )
00325 {
00326   return (a.votes > b.votes);
00327 }
00328 
00329 
00331 template <typename PointSource, typename PointTarget> bool
00332 pcl::PPFRegistration<PointSource, PointTarget>::clusterVotesCompareFunction (const std::pair<size_t, unsigned int> &a,
00333                                                                              const std::pair<size_t, unsigned int> &b)
00334 {
00335   return (a.second > b.second);
00336 }
00337 
00338 //#define PCL_INSTANTIATE_PPFRegistration(PointSource,PointTarget) template class PCL_EXPORTS pcl::PPFRegistration<PointSource, PointTarget>;
00339 
00340 #endif // PCL_REGISTRATION_IMPL_PPF_REGISTRATION_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines