Point Cloud Library (PCL)
1.3.1
|
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_