Point Cloud Library (PCL)
1.3.1
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * $Id: icp.hpp 2844 2011-10-24 22:00:52Z rusu $ 00035 * 00036 */ 00037 00038 #include <boost/unordered_map.hpp> 00039 00041 template <typename PointSource, typename PointTarget> void 00042 pcl::IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output) 00043 { 00044 pcl::IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (output, Eigen::Matrix4f::Identity()); 00045 } 00046 00048 template <typename PointSource, typename PointTarget> void 00049 pcl::IterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess) 00050 { 00051 // Allocate enough space to hold the results 00052 std::vector<int> nn_indices (1); 00053 std::vector<float> nn_dists (1); 00054 00055 // Point cloud containing the correspondences of each point in <input, indices> 00056 PointCloudTarget input_corresp; 00057 input_corresp.points.resize (indices_->size ()); 00058 00059 nr_iterations_ = 0; 00060 converged_ = false; 00061 double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_; 00062 00063 // If the guessed transformation is non identity 00064 if (guess != Eigen::Matrix4f::Identity ()) 00065 { 00066 // Initialise final transformation to the guessed one 00067 final_transformation_ = guess; 00068 // Apply guessed transformation prior to search for neighbours 00069 transformPointCloud (output, output, guess); 00070 } 00071 00072 // Resize the vector of distances between correspondences 00073 std::vector<float> previous_correspondence_distances (indices_->size ()); 00074 correspondence_distances_.resize (indices_->size ()); 00075 00076 while (!converged_) // repeat until convergence 00077 { 00078 // Save the previously estimated transformation 00079 previous_transformation_ = transformation_; 00080 // And the previous set of distances 00081 previous_correspondence_distances = correspondence_distances_; 00082 00083 int cnt = 0; 00084 std::vector<int> source_indices (indices_->size ()); 00085 std::vector<int> target_indices (indices_->size ()); 00086 00087 // Iterating over the entire index vector and find all correspondences 00088 for (size_t idx = 0; idx < indices_->size (); ++idx) 00089 { 00090 if (!searchForNeighbors (output, (*indices_)[idx], nn_indices, nn_dists)) 00091 { 00092 PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[idx]); 00093 return; 00094 } 00095 00096 // Check if the distance to the nearest neighbor is smaller than the user imposed threshold 00097 if (nn_dists[0] < dist_threshold) 00098 { 00099 source_indices[cnt] = (*indices_)[idx]; 00100 target_indices[cnt] = nn_indices[0]; 00101 cnt++; 00102 } 00103 00104 // Save the nn_dists[0] to a global vector of distances 00105 correspondence_distances_[(*indices_)[idx]] = std::min (nn_dists[0], (float)dist_threshold); 00106 } 00107 if (cnt < min_number_correspondences_) 00108 { 00109 PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ()); 00110 converged_ = false; 00111 return; 00112 } 00113 00114 // Resize to the actual number of valid correspondences 00115 source_indices.resize (cnt); target_indices.resize (cnt); 00116 00117 std::vector<int> source_indices_good; 00118 std::vector<int> target_indices_good; 00119 { 00120 // From the set of correspondences found, attempt to remove outliers 00121 // Create the registration model 00122 typedef typename SampleConsensusModelRegistration<PointSource>::Ptr SampleConsensusModelRegistrationPtr; 00123 SampleConsensusModelRegistrationPtr model; 00124 model.reset (new SampleConsensusModelRegistration<PointSource> (output.makeShared (), source_indices)); 00125 // Pass the target_indices 00126 model->setInputTarget (target_, target_indices); 00127 // Create a RANSAC model 00128 RandomSampleConsensus<PointSource> sac (model, inlier_threshold_); 00129 sac.setMaxIterations (1000); 00130 00131 // Compute the set of inliers 00132 if (!sac.computeModel ()) 00133 { 00134 source_indices_good = source_indices; 00135 target_indices_good = target_indices; 00136 } 00137 else 00138 { 00139 std::vector<int> inliers; 00140 // Get the inliers 00141 sac.getInliers (inliers); 00142 source_indices_good.resize (inliers.size ()); 00143 target_indices_good.resize (inliers.size ()); 00144 00145 boost::unordered_map<int, int> source_to_target; 00146 for (unsigned int i = 0; i < source_indices.size(); ++i) 00147 source_to_target[source_indices[i]] = target_indices[i]; 00148 00149 // Copy just the inliers 00150 std::copy(inliers.begin(), inliers.end(), source_indices_good.begin()); 00151 for (size_t i = 0; i < inliers.size (); ++i) 00152 target_indices_good[i] = source_to_target[inliers[i]]; 00153 } 00154 } 00155 00156 // Check whether we have enough correspondences 00157 cnt = (int)source_indices_good.size (); 00158 if (cnt < min_number_correspondences_) 00159 { 00160 PCL_ERROR ("[pcl::%s::computeTransformation] Not enough correspondences found. Relax your threshold parameters.\n", getClassName ().c_str ()); 00161 converged_ = false; 00162 return; 00163 } 00164 00165 PCL_DEBUG ("[pcl::%s::computeTransformation] Number of correspondences %d [%f%%] out of %lu points [100.0%%], RANSAC rejected: %lu [%f%%].\n", getClassName ().c_str (), cnt, (cnt * 100.0) / indices_->size (), (unsigned long)indices_->size (), (unsigned long)source_indices.size () - cnt, (source_indices.size () - cnt) * 100.0 / source_indices.size ()); 00166 00167 // Estimate the transform 00168 //rigid_transformation_estimation_(output, source_indices_good, *target_, target_indices_good, transformation_); 00169 transformation_estimation_->estimateRigidTransformation (output, source_indices_good, *target_, target_indices_good, transformation_); 00170 00171 // Tranform the data 00172 transformPointCloud (output, output, transformation_); 00173 00174 // Obtain the final transformation 00175 final_transformation_ = transformation_ * final_transformation_; 00176 00177 nr_iterations_++; 00178 00179 // Update the vizualization of icp convergence 00180 if (update_visualizer_ != 0) 00181 update_visualizer_(output, source_indices_good, *target_, target_indices_good ); 00182 00183 // Various/Different convergence termination criteria 00184 // 1. Number of iterations has reached the maximum user imposed number of iterations (via 00185 // setMaximumIterations) 00186 // 2. The epsilon (difference) between the previous transformation and the current estimated transformation 00187 // is smaller than an user imposed value (via setTransformationEpsilon) 00188 // 3. The sum of Euclidean squared errors is smaller than a user defined threshold (via 00189 // setEuclideanFitnessEpsilon) 00190 00191 if (nr_iterations_ >= max_iterations_ || 00192 fabs ((transformation_ - previous_transformation_).sum ()) < transformation_epsilon_ || 00193 fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)) <= euclidean_fitness_epsilon_ 00194 ) 00195 { 00196 converged_ = true; 00197 PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n", 00198 getClassName ().c_str (), nr_iterations_, max_iterations_, fabs ((transformation_ - previous_transformation_).sum ())); 00199 00200 PCL_DEBUG ("nr_iterations_ (%d) >= max_iterations_ (%d)\n", nr_iterations_, max_iterations_); 00201 PCL_DEBUG ("fabs ((transformation_ - previous_transformation_).sum ()) (%f) < transformation_epsilon_ (%f)\n", 00202 fabs ((transformation_ - previous_transformation_).sum ()), transformation_epsilon_); 00203 PCL_DEBUG ("fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)) (%f) <= euclidean_fitness_epsilon_ (%f)\n", 00204 fabs (getFitnessScore (correspondence_distances_, previous_correspondence_distances)), 00205 euclidean_fitness_epsilon_); 00206 00207 } 00208 } 00209 } 00210