ia_ransac.hpp
Go to the documentation of this file.
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  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: ia_ransac.hpp 5066 2012-03-14 06:42:21Z rusu $
00037  *
00038  */
00039 
00040 #ifndef IA_RANSAC_HPP_
00041 #define IA_RANSAC_HPP_
00042 
00043 #include <pcl/common/distances.h>
00044 
00046 template <typename PointSource, typename PointTarget, typename FeatureT> void 
00047 pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::setSourceFeatures (const FeatureCloudConstPtr &features)
00048 {
00049   if (features == NULL || features->empty ())
00050   {
00051     PCL_ERROR ("[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
00052     return;
00053   }
00054   input_features_ = features;
00055 }
00056 
00058 template <typename PointSource, typename PointTarget, typename FeatureT> void 
00059 pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::setTargetFeatures (const FeatureCloudConstPtr &features)
00060 {
00061   if (features == NULL || features->empty ())
00062   {
00063     PCL_ERROR ("[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
00064     return;
00065   }
00066   target_features_ = features;
00067   feature_tree_->setInputCloud (target_features_);
00068 }
00069 
00071 template <typename PointSource, typename PointTarget, typename FeatureT> void 
00072 pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::selectSamples (
00073     const PointCloudSource &cloud, int nr_samples, float min_sample_distance, 
00074     std::vector<int> &sample_indices)
00075 {
00076   if (nr_samples > static_cast<int> (cloud.points.size ()))
00077   {
00078     PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ());
00079     PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%zu)!\n",
00080                nr_samples, cloud.points.size ());
00081     return;
00082   }
00083 
00084   // Iteratively draw random samples until nr_samples is reached
00085   int iterations_without_a_sample = 0;
00086   int max_iterations_without_a_sample = static_cast<int> (3 * cloud.points.size ());
00087   sample_indices.clear ();
00088   while (static_cast<int> (sample_indices.size ()) < nr_samples)
00089   {
00090     // Choose a sample at random
00091     int sample_index = getRandomIndex (static_cast<int> (cloud.points.size ()));
00092 
00093     // Check to see if the sample is 1) unique and 2) far away from the other samples
00094     bool valid_sample = true;
00095     for (size_t i = 0; i < sample_indices.size (); ++i)
00096     {
00097       float distance_between_samples = euclideanDistance (cloud.points[sample_index], cloud.points[sample_indices[i]]);
00098 
00099       if (sample_index == sample_indices[i] || distance_between_samples < min_sample_distance)
00100       {
00101         valid_sample = false;
00102         break;
00103       }
00104     }
00105 
00106     // If the sample is valid, add it to the output
00107     if (valid_sample)
00108     {
00109       sample_indices.push_back (sample_index);
00110       iterations_without_a_sample = 0;
00111     }
00112     else
00113     {
00114       ++iterations_without_a_sample;
00115     }
00116 
00117     // If no valid samples can be found, relax the inter-sample distance requirements
00118     if (iterations_without_a_sample >= max_iterations_without_a_sample)
00119     {
00120       PCL_WARN ("[pcl::%s::selectSamples] ", getClassName ().c_str ());
00121       PCL_WARN ("No valid sample found after %d iterations. Relaxing min_sample_distance_ to %f\n",
00122                 iterations_without_a_sample, 0.5*min_sample_distance);
00123 
00124       min_sample_distance_ *= 0.5f;
00125       min_sample_distance = min_sample_distance_;
00126       iterations_without_a_sample = 0;
00127     }
00128   }
00129 
00130 }
00131 
00133 template <typename PointSource, typename PointTarget, typename FeatureT> void 
00134 pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::findSimilarFeatures (
00135     const FeatureCloud &input_features, const std::vector<int> &sample_indices, 
00136     std::vector<int> &corresponding_indices)
00137 {
00138   std::vector<int> nn_indices (k_correspondences_);
00139   std::vector<float> nn_distances (k_correspondences_);
00140 
00141   corresponding_indices.resize (sample_indices.size ());
00142   for (size_t i = 0; i < sample_indices.size (); ++i)
00143   {
00144     // Find the k features nearest to input_features.points[sample_indices[i]]
00145     feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances);
00146 
00147     // Select one at random and add it to corresponding_indices
00148     int random_correspondence = getRandomIndex (k_correspondences_);
00149     corresponding_indices[i] = nn_indices[random_correspondence];
00150   }
00151 }
00152 
00154 template <typename PointSource, typename PointTarget, typename FeatureT> float 
00155 pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeErrorMetric (
00156     const PointCloudSource &cloud, float)
00157 {
00158   std::vector<int> nn_index (1);
00159   std::vector<float> nn_distance (1);
00160 
00161   const ErrorFunctor & compute_error = *error_functor_;
00162   float error = 0;
00163 
00164   for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i)
00165   {
00166     // Find the distance between cloud.points[i] and its nearest neighbor in the target point cloud
00167     tree_->nearestKSearch (cloud, i, 1, nn_index, nn_distance);
00168 
00169     // Compute the error
00170     error += compute_error (nn_distance[0]);
00171   }
00172   return (error);
00173 }
00174 
00176 template <typename PointSource, typename PointTarget, typename FeatureT> void 
00177 pcl::SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess)
00178 {
00179   if (!input_features_)
00180   {
00181     PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
00182     PCL_ERROR ("No source features were given! Call setSourceFeatures before aligning.\n");
00183     return;
00184   }
00185   if (!target_features_)
00186   {
00187     PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
00188     PCL_ERROR ("No target features were given! Call setTargetFeatures before aligning.\n");
00189     return;
00190   }
00191 
00192   if (!error_functor_)
00193   {
00194     error_functor_.reset (new TruncatedError (static_cast<float> (corr_dist_threshold_)));
00195   }
00196 
00197   std::vector<int> sample_indices (nr_samples_);
00198   std::vector<int> corresponding_indices (nr_samples_);
00199   PointCloudSource input_transformed;
00200   float error, lowest_error (0);
00201 
00202   final_transformation_ = guess;
00203   int i_iter = 0;
00204   if (!guess.isApprox(Eigen::Matrix4f::Identity (), 0.01f)) 
00205   { //If guess is not the Identity matrix we check it.
00206           transformPointCloud (*input_, input_transformed, final_transformation_);
00207           lowest_error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));
00208           i_iter = 1;
00209   }
00210 
00211   for (; i_iter < max_iterations_; ++i_iter)
00212   {
00213     // Draw nr_samples_ random samples
00214     selectSamples (*input_, nr_samples_, min_sample_distance_, sample_indices);
00215 
00216     // Find corresponding features in the target cloud
00217     findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);
00218 
00219     // Estimate the transform from the samples to their corresponding points
00220     transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
00221 
00222     // Tranform the data and compute the error
00223     transformPointCloud (*input_, input_transformed, transformation_);
00224     error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));
00225 
00226     // If the new error is lower, update the final transformation
00227     if (i_iter == 0 || error < lowest_error)
00228     {
00229       lowest_error = error;
00230       final_transformation_ = transformation_;
00231     }
00232   }
00233 
00234   // Apply the final transformation
00235   transformPointCloud (*input_, output, final_transformation_);
00236 }
00237 
00238 #endif  //#ifndef IA_RANSAC_HPP_
00239 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:25