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) 2010-2012, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 00041 #ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_ 00042 #define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_ 00043 00044 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00045 template <typename PointSource, typename PointTarget, typename FeatureT> void 00046 pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::setSourceFeatures (const FeatureCloudConstPtr &features) 00047 { 00048 if (features == NULL || features->empty ()) 00049 { 00050 PCL_ERROR ("[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); 00051 return; 00052 } 00053 input_features_ = features; 00054 } 00055 00056 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00057 template <typename PointSource, typename PointTarget, typename FeatureT> void 00058 pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::setTargetFeatures (const FeatureCloudConstPtr &features) 00059 { 00060 if (features == NULL || features->empty ()) 00061 { 00062 PCL_ERROR ("[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); 00063 return; 00064 } 00065 target_features_ = features; 00066 feature_tree_->setInputCloud (target_features_); 00067 } 00068 00069 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00070 template <typename PointSource, typename PointTarget, typename FeatureT> void 00071 pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::selectSamples ( 00072 const PointCloudSource &cloud, int nr_samples, 00073 std::vector<int> &sample_indices) 00074 { 00075 if (nr_samples > static_cast<int> (cloud.points.size ())) 00076 { 00077 PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ()); 00078 PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%zu)!\n", 00079 nr_samples, cloud.points.size ()); 00080 return; 00081 } 00082 00083 // Iteratively draw random samples until nr_samples is reached 00084 sample_indices.clear (); 00085 std::vector<bool> sampled_indices (cloud.points.size (), false); 00086 while (static_cast<int> (sample_indices.size ()) < nr_samples) 00087 { 00088 // Choose a unique sample at random 00089 int sample_index; 00090 do 00091 { 00092 sample_index = getRandomIndex (static_cast<int> (cloud.points.size ())); 00093 } 00094 while (sampled_indices[sample_index]); 00095 00096 // Mark index as sampled 00097 sampled_indices[sample_index] = true; 00098 00099 // Store 00100 sample_indices.push_back (sample_index); 00101 } 00102 } 00103 00104 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00105 template <typename PointSource, typename PointTarget, typename FeatureT> void 00106 pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::findSimilarFeatures ( 00107 const FeatureCloud &input_features, const std::vector<int> &sample_indices, 00108 std::vector<int> &corresponding_indices) 00109 { 00110 std::vector<int> nn_indices (k_correspondences_); 00111 std::vector<float> nn_distances (k_correspondences_); 00112 00113 corresponding_indices.resize (sample_indices.size ()); 00114 for (size_t i = 0; i < sample_indices.size (); ++i) 00115 { 00116 // Find the k features nearest to input_features.points[sample_indices[i]] 00117 feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances); 00118 00119 // Select one at random and add it to corresponding_indices 00120 if (k_correspondences_ == 1) 00121 { 00122 corresponding_indices[i] = nn_indices[0]; 00123 } 00124 else 00125 { 00126 int random_correspondence = getRandomIndex (k_correspondences_); 00127 corresponding_indices[i] = nn_indices[random_correspondence]; 00128 } 00129 } 00130 } 00131 00132 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00133 template <typename PointSource, typename PointTarget, typename FeatureT> void 00134 pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) 00135 { 00136 // Some sanity checks first 00137 if (!input_features_) 00138 { 00139 PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); 00140 PCL_ERROR ("No source features were given! Call setSourceFeatures before aligning.\n"); 00141 return; 00142 } 00143 if (!target_features_) 00144 { 00145 PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); 00146 PCL_ERROR ("No target features were given! Call setTargetFeatures before aligning.\n"); 00147 return; 00148 } 00149 00150 if (input_->size () != input_features_->size ()) 00151 { 00152 PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); 00153 PCL_ERROR ("The source points and source feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n", 00154 input_->size (), input_features_->size ()); 00155 return; 00156 } 00157 00158 if (target_->size () != target_features_->size ()) 00159 { 00160 PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); 00161 PCL_ERROR ("The target points and target feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n", 00162 target_->size (), target_features_->size ()); 00163 return; 00164 } 00165 00166 if (inlier_fraction_ < 0.0f || inlier_fraction_ > 1.0f) 00167 { 00168 PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); 00169 PCL_ERROR ("Illegal inlier fraction %f, must be in [0,1]!\n", 00170 inlier_fraction_); 00171 return; 00172 } 00173 00174 const float similarity_threshold = correspondence_rejector_poly_->getSimilarityThreshold (); 00175 if (similarity_threshold < 0.0f || similarity_threshold >= 1.0f) 00176 { 00177 PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); 00178 PCL_ERROR ("Illegal prerejection similarity threshold %f, must be in [0,1[!\n", 00179 similarity_threshold); 00180 return; 00181 } 00182 00183 // Initialize prerejector (similarity threshold already set to default value in constructor) 00184 correspondence_rejector_poly_->setInputSource (input_); 00185 correspondence_rejector_poly_->setInputTarget (target_); 00186 correspondence_rejector_poly_->setCardinality (nr_samples_); 00187 int num_rejections = 0; // For debugging 00188 00189 // Initialize results 00190 final_transformation_ = guess; 00191 inliers_.clear (); 00192 float highest_inlier_fraction = inlier_fraction_; 00193 converged_ = false; 00194 00195 // Temporaries 00196 std::vector<int> inliers; 00197 float inlier_fraction; 00198 float error; 00199 00200 // If guess is not the Identity matrix we check it 00201 if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f)) 00202 { 00203 getFitness (inliers, error); 00204 inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ()); 00205 00206 if (inlier_fraction > highest_inlier_fraction) 00207 { 00208 inliers_ = inliers; 00209 highest_inlier_fraction = inlier_fraction; 00210 converged_ = true; 00211 } 00212 } 00213 00214 // Start 00215 for (int i = 0; i < max_iterations_; ++i) 00216 { 00217 // Temporary containers 00218 std::vector<int> sample_indices (nr_samples_); 00219 std::vector<int> corresponding_indices (nr_samples_); 00220 00221 // Draw nr_samples_ random samples 00222 selectSamples (*input_, nr_samples_, sample_indices); 00223 00224 // Find corresponding features in the target cloud 00225 findSimilarFeatures (*input_features_, sample_indices, corresponding_indices); 00226 00227 // Apply prerejection 00228 if (!correspondence_rejector_poly_->thresholdPolygon (sample_indices, corresponding_indices)){ 00229 ++num_rejections; 00230 continue; 00231 } 00232 00233 // Estimate the transform from the correspondences, write to transformation_ 00234 transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_); 00235 00236 // Take a backup of previous result 00237 const Matrix4 final_transformation_prev = final_transformation_; 00238 00239 // Set final result to current transformation 00240 final_transformation_ = transformation_; 00241 00242 // Transform the input and compute the error (uses input_ and final_transformation_) 00243 getFitness (inliers, error); 00244 00245 // If the new fit is better, update results 00246 const float inlier_fraction = static_cast<float> (inliers.size ()) / static_cast<float> (input_->size ()); 00247 if (inlier_fraction > highest_inlier_fraction) 00248 { 00249 inliers_ = inliers; 00250 highest_inlier_fraction = inlier_fraction; 00251 converged_ = true; 00252 } 00253 else 00254 { 00255 // Restore previous result 00256 final_transformation_ = final_transformation_prev; 00257 } 00258 } 00259 00260 // Apply the final transformation 00261 if (converged_) 00262 transformPointCloud (*input_, output, final_transformation_); 00263 00264 // Debug output 00265 PCL_DEBUG("[pcl::%s::computeTransformation] Rejected %i out of %i generated pose hypotheses.\n", 00266 getClassName ().c_str (), num_rejections, max_iterations_); 00267 } 00268 00269 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00270 template <typename PointSource, typename PointTarget, typename FeatureT> void 00271 pcl::SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::getFitness (std::vector<int>& inliers, float& fitness_score) 00272 { 00273 // Initialize variables 00274 inliers.clear (); 00275 inliers.reserve (input_->size ()); 00276 fitness_score = 0.0f; 00277 00278 // Use squared distance for comparison with NN search results 00279 const float max_range = corr_dist_threshold_ * corr_dist_threshold_; 00280 00281 // Transform the input dataset using the final transformation 00282 PointCloudSource input_transformed; 00283 input_transformed.resize (input_->size ()); 00284 transformPointCloud (*input_, input_transformed, final_transformation_); 00285 00286 // For each point in the source dataset 00287 for (size_t i = 0; i < input_transformed.points.size (); ++i) 00288 { 00289 // Find its nearest neighbor in the target 00290 std::vector<int> nn_indices (1); 00291 std::vector<float> nn_dists (1); 00292 tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists); 00293 00294 // Check if point is an inlier 00295 if (nn_dists[0] < max_range) 00296 { 00297 // Errors 00298 const float dx = input_transformed.points[i].x - target_->points[nn_indices[0]].x; 00299 const float dy = input_transformed.points[i].y - target_->points[nn_indices[0]].y; 00300 const float dz = input_transformed.points[i].z - target_->points[nn_indices[0]].z; 00301 00302 // Update inliers 00303 inliers.push_back (static_cast<int> (i)); 00304 00305 // Update fitness score 00306 fitness_score += dx*dx + dy*dy + dz*dz; 00307 } 00308 } 00309 00310 // Calculate MSE 00311 if (inliers.size () > 0) 00312 fitness_score /= static_cast<float> (inliers.size ()); 00313 else 00314 fitness_score = std::numeric_limits<float>::max (); 00315 } 00316 00317 #endif 00318