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