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