Point Cloud Library (PCL)  1.3.1
correspondence_rejection_sample_consensus.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  *
00035  */
00036 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_
00037 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_
00038 
00039 #include <boost/unordered_map.hpp>
00040 
00042 template <typename PointT> inline void 
00043 pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::applyRejection (
00044     pcl::Correspondences &correspondences)
00045 {
00046   int nr_correspondences = input_correspondences_->size ();
00047 
00048   std::vector<int> source_indices (nr_correspondences);
00049   std::vector<int> target_indices (nr_correspondences);
00050 
00051   // Copy the query-match indices
00052   for (size_t i = 0; i < input_correspondences_->size (); ++i)
00053   {
00054     source_indices[i] = (*input_correspondences_)[i].index_query;
00055     target_indices[i] = (*input_correspondences_)[i].index_match;
00056   }
00057 
00058    // from pcl/registration/icp.hpp:
00059    std::vector<int> source_indices_good;
00060    std::vector<int> target_indices_good;
00061    {
00062      // From the set of correspondences found, attempt to remove outliers
00063      // Create the registration model
00064      typedef typename pcl::SampleConsensusModelRegistration<PointT>::Ptr SampleConsensusModelRegistrationPtr;
00065      SampleConsensusModelRegistrationPtr model;
00066      model.reset (new pcl::SampleConsensusModelRegistration<PointT> (input_, source_indices));
00067      // Pass the target_indices
00068      model->setInputTarget (target_, target_indices);
00069      // Create a RANSAC model
00070      pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_);
00071      sac.setMaxIterations (max_iterations_);
00072 
00073      // Compute the set of inliers
00074      if (!sac.computeModel ())
00075      {
00076        correspondences = *input_correspondences_;
00077        best_transformation_.setIdentity ();
00078        return;
00079      }
00080      else
00081      {
00082        std::vector<int> inliers;
00083        sac.getInliers (inliers);
00084 
00085        if (inliers.size () < 3)
00086        {
00087          correspondences = *input_correspondences_;
00088          best_transformation_.setIdentity ();
00089          return;
00090        }
00091        boost::unordered_map<int, int> index_to_correspondence;
00092        for (int i = 0; i < nr_correspondences; ++i)
00093          index_to_correspondence[(*input_correspondences_)[i].index_query] = i;
00094 
00095        correspondences.resize (inliers.size ());
00096        for (size_t i = 0; i < inliers.size (); ++i)
00097          correspondences[i] = (*input_correspondences_)[index_to_correspondence[inliers[i]]];
00098          //correspondences[i] = (*input_correspondences_)[inliers[i]];
00099 
00100        // get best transformation
00101        Eigen::VectorXf model_coefficients;
00102        sac.getModelCoefficients (model_coefficients);
00103        best_transformation_.row (0) = model_coefficients.segment<4>(0);
00104        best_transformation_.row (1) = model_coefficients.segment<4>(4);
00105        best_transformation_.row (2) = model_coefficients.segment<4>(8);
00106        best_transformation_.row (3) = model_coefficients.segment<4>(12);
00107      }
00108    }
00109 }
00110 
00112 template <typename PointT> void 
00113 pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::getRemainingCorrespondences (
00114     const pcl::Correspondences& original_correspondences, 
00115     pcl::Correspondences& remaining_correspondences)
00116 {
00117   int nr_correspondences = (int)original_correspondences.size ();
00118   std::vector<int> source_indices (nr_correspondences);
00119   std::vector<int> target_indices (nr_correspondences);
00120 
00121   // Copy the query-match indices
00122   for (size_t i = 0; i < original_correspondences.size (); ++i)
00123   {
00124     source_indices[i] = original_correspondences[i].index_query;
00125     target_indices[i] = original_correspondences[i].index_match;
00126   }
00127 
00128    // from pcl/registration/icp.hpp:
00129    std::vector<int> source_indices_good;
00130    std::vector<int> target_indices_good;
00131    {
00132      // From the set of correspondences found, attempt to remove outliers
00133      // Create the registration model
00134      typedef typename pcl::SampleConsensusModelRegistration<PointT>::Ptr SampleConsensusModelRegistrationPtr;
00135      SampleConsensusModelRegistrationPtr model;
00136      model.reset (new pcl::SampleConsensusModelRegistration<PointT> (input_, source_indices));
00137      // Pass the target_indices
00138      model->setInputTarget (target_, target_indices);
00139      // Create a RANSAC model
00140      pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_);
00141      sac.setMaxIterations (max_iterations_);
00142 
00143      // Compute the set of inliers
00144      if (!sac.computeModel ())
00145      {
00146        remaining_correspondences = original_correspondences;
00147        best_transformation_.setIdentity ();
00148        return;
00149      }
00150      else
00151      {
00152        std::vector<int> inliers;
00153        sac.getInliers (inliers);
00154 
00155        if (inliers.size () < 3)
00156        {
00157          remaining_correspondences = original_correspondences;
00158          best_transformation_.setIdentity ();
00159          return;
00160        }
00161        boost::unordered_map<int, int> index_to_correspondence;
00162        for (int i = 0; i < nr_correspondences; ++i)
00163          index_to_correspondence[original_correspondences[i].index_query] = i;
00164 
00165        remaining_correspondences.resize (inliers.size ());
00166        for (size_t i = 0; i < inliers.size (); ++i)
00167          remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]];
00168 
00169        // get best transformation
00170        Eigen::VectorXf model_coefficients;
00171        sac.getModelCoefficients (model_coefficients);
00172        best_transformation_.row (0) = model_coefficients.segment<4>(0);
00173        best_transformation_.row (1) = model_coefficients.segment<4>(4);
00174        best_transformation_.row (2) = model_coefficients.segment<4>(8);
00175        best_transformation_.row (3) = model_coefficients.segment<4>(12);
00176      }
00177    }
00178 }
00179 
00180 #endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines