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 IA_RANSAC_HPP_ 00042 #define IA_RANSAC_HPP_ 00043 00044 #include <pcl/common/distances.h> 00045 00046 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00047 template <typename PointSource, typename PointTarget, typename FeatureT> void 00048 pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::setSourceFeatures (const FeatureCloudConstPtr &features) 00049 { 00050 if (features == NULL || features->empty ()) 00051 { 00052 PCL_ERROR ("[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); 00053 return; 00054 } 00055 input_features_ = features; 00056 } 00057 00058 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00059 template <typename PointSource, typename PointTarget, typename FeatureT> void 00060 pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::setTargetFeatures (const FeatureCloudConstPtr &features) 00061 { 00062 if (features == NULL || features->empty ()) 00063 { 00064 PCL_ERROR ("[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); 00065 return; 00066 } 00067 target_features_ = features; 00068 feature_tree_->setInputCloud (target_features_); 00069 } 00070 00071 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00072 template <typename PointSource, typename PointTarget, typename FeatureT> void 00073 pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::selectSamples ( 00074 const PointCloudSource &cloud, int nr_samples, float min_sample_distance, 00075 std::vector<int> &sample_indices) 00076 { 00077 if (nr_samples > static_cast<int> (cloud.points.size ())) 00078 { 00079 PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ()); 00080 PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%zu)!\n", 00081 nr_samples, cloud.points.size ()); 00082 return; 00083 } 00084 00085 // Iteratively draw random samples until nr_samples is reached 00086 int iterations_without_a_sample = 0; 00087 int max_iterations_without_a_sample = static_cast<int> (3 * cloud.points.size ()); 00088 sample_indices.clear (); 00089 while (static_cast<int> (sample_indices.size ()) < nr_samples) 00090 { 00091 // Choose a sample at random 00092 int sample_index = getRandomIndex (static_cast<int> (cloud.points.size ())); 00093 00094 // Check to see if the sample is 1) unique and 2) far away from the other samples 00095 bool valid_sample = true; 00096 for (size_t i = 0; i < sample_indices.size (); ++i) 00097 { 00098 float distance_between_samples = euclideanDistance (cloud.points[sample_index], cloud.points[sample_indices[i]]); 00099 00100 if (sample_index == sample_indices[i] || distance_between_samples < min_sample_distance) 00101 { 00102 valid_sample = false; 00103 break; 00104 } 00105 } 00106 00107 // If the sample is valid, add it to the output 00108 if (valid_sample) 00109 { 00110 sample_indices.push_back (sample_index); 00111 iterations_without_a_sample = 0; 00112 } 00113 else 00114 ++iterations_without_a_sample; 00115 00116 // If no valid samples can be found, relax the inter-sample distance requirements 00117 if (iterations_without_a_sample >= max_iterations_without_a_sample) 00118 { 00119 PCL_WARN ("[pcl::%s::selectSamples] ", getClassName ().c_str ()); 00120 PCL_WARN ("No valid sample found after %d iterations. Relaxing min_sample_distance_ to %f\n", 00121 iterations_without_a_sample, 0.5*min_sample_distance); 00122 00123 min_sample_distance_ *= 0.5f; 00124 min_sample_distance = min_sample_distance_; 00125 iterations_without_a_sample = 0; 00126 } 00127 } 00128 } 00129 00130 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00131 template <typename PointSource, typename PointTarget, typename FeatureT> void 00132 pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::findSimilarFeatures ( 00133 const FeatureCloud &input_features, const std::vector<int> &sample_indices, 00134 std::vector<int> &corresponding_indices) 00135 { 00136 std::vector<int> nn_indices (k_correspondences_); 00137 std::vector<float> nn_distances (k_correspondences_); 00138 00139 corresponding_indices.resize (sample_indices.size ()); 00140 for (size_t i = 0; i < sample_indices.size (); ++i) 00141 { 00142 // Find the k features nearest to input_features.points[sample_indices[i]] 00143 feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances); 00144 00145 // Select one at random and add it to corresponding_indices 00146 int random_correspondence = getRandomIndex (k_correspondences_); 00147 corresponding_indices[i] = nn_indices[random_correspondence]; 00148 } 00149 } 00150 00151 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00152 template <typename PointSource, typename PointTarget, typename FeatureT> float 00153 pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeErrorMetric ( 00154 const PointCloudSource &cloud, float) 00155 { 00156 std::vector<int> nn_index (1); 00157 std::vector<float> nn_distance (1); 00158 00159 const ErrorFunctor & compute_error = *error_functor_; 00160 float error = 0; 00161 00162 for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i) 00163 { 00164 // Find the distance between cloud.points[i] and its nearest neighbor in the target point cloud 00165 tree_->nearestKSearch (cloud, i, 1, nn_index, nn_distance); 00166 00167 // Compute the error 00168 error += compute_error (nn_distance[0]); 00169 } 00170 return (error); 00171 } 00172 00173 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00174 template <typename PointSource, typename PointTarget, typename FeatureT> void 00175 pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) 00176 { 00177 // Some sanity checks first 00178 if (!input_features_) 00179 { 00180 PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); 00181 PCL_ERROR ("No source features were given! Call setSourceFeatures before aligning.\n"); 00182 return; 00183 } 00184 if (!target_features_) 00185 { 00186 PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); 00187 PCL_ERROR ("No target features were given! Call setTargetFeatures before aligning.\n"); 00188 return; 00189 } 00190 00191 if (input_->size () != input_features_->size ()) 00192 { 00193 PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); 00194 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", 00195 input_->size (), input_features_->size ()); 00196 return; 00197 } 00198 00199 if (target_->size () != target_features_->size ()) 00200 { 00201 PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ()); 00202 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", 00203 target_->size (), target_features_->size ()); 00204 return; 00205 } 00206 00207 if (!error_functor_) 00208 error_functor_.reset (new TruncatedError (static_cast<float> (corr_dist_threshold_))); 00209 00210 00211 std::vector<int> sample_indices (nr_samples_); 00212 std::vector<int> corresponding_indices (nr_samples_); 00213 PointCloudSource input_transformed; 00214 float error, lowest_error (0); 00215 00216 final_transformation_ = guess; 00217 int i_iter = 0; 00218 if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f)) 00219 { 00220 // If guess is not the Identity matrix we check it. 00221 transformPointCloud (*input_, input_transformed, final_transformation_); 00222 lowest_error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_)); 00223 i_iter = 1; 00224 } 00225 00226 for (; i_iter < max_iterations_; ++i_iter) 00227 { 00228 // Draw nr_samples_ random samples 00229 selectSamples (*input_, nr_samples_, min_sample_distance_, sample_indices); 00230 00231 // Find corresponding features in the target cloud 00232 findSimilarFeatures (*input_features_, sample_indices, corresponding_indices); 00233 00234 // Estimate the transform from the samples to their corresponding points 00235 transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_); 00236 00237 // Tranform the data and compute the error 00238 transformPointCloud (*input_, input_transformed, transformation_); 00239 error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_)); 00240 00241 // If the new error is lower, update the final transformation 00242 if (i_iter == 0 || error < lowest_error) 00243 { 00244 lowest_error = error; 00245 final_transformation_ = transformation_; 00246 } 00247 } 00248 00249 // Apply the final transformation 00250 transformPointCloud (*input_, output, final_transformation_); 00251 } 00252 00253 #endif //#ifndef IA_RANSAC_HPP_ 00254