Point Cloud Library (PCL)  1.3.1
icp.hpp
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines