correspondence_rejection_sample_consensus.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-2011, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id$
00038  *
00039  */
00040 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_
00041 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_
00042 
00043 #include <boost/unordered_map.hpp>
00044 
00046 template <typename PointT> void
00047 pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::setInputCloud (
00048     const typename pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::PointCloudConstPtr &cloud)
00049 {
00050   setInputSource (cloud);
00051 }
00052 
00054 template <typename PointT> typename pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::PointCloudConstPtr const
00055 pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::getInputCloud ()
00056 {
00057   return (getInputSource ()); 
00058 }
00059 
00061 template <typename PointT> void
00062 pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::setTargetCloud (
00063     const typename pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::PointCloudConstPtr &cloud)
00064 {
00065   setInputTarget (cloud);
00066 }
00067 
00069 template <typename PointT> void
00070 pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::setMaxIterations (
00071     int max_iterations)
00072 {
00073   setMaximumIterations (max_iterations);
00074 }
00075 
00077 template <typename PointT> int
00078 pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::getMaxIterations ()
00079 {
00080   return (getMaximumIterations ());
00081 }
00082 
00084 template <typename PointT> void 
00085 pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::getRemainingCorrespondences (
00086     const pcl::Correspondences& original_correspondences, 
00087     pcl::Correspondences& remaining_correspondences)
00088 {
00089   if (!input_)
00090   {
00091     PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input cloud dataset was given!\n", getClassName ().c_str ());
00092     return;
00093   }
00094 
00095   if (!target_)
00096   {
00097     PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input target dataset was given!\n", getClassName ().c_str ());
00098     return;
00099   }
00100 
00101   int nr_correspondences = static_cast<int> (original_correspondences.size ());
00102   std::vector<int> source_indices (nr_correspondences);
00103   std::vector<int> target_indices (nr_correspondences);
00104 
00105   // Copy the query-match indices
00106   for (size_t i = 0; i < original_correspondences.size (); ++i)
00107   {
00108     source_indices[i] = original_correspondences[i].index_query;
00109     target_indices[i] = original_correspondences[i].index_match;
00110   }
00111 
00112    // from pcl/registration/icp.hpp:
00113    std::vector<int> source_indices_good;
00114    std::vector<int> target_indices_good;
00115    {
00116      // From the set of correspondences found, attempt to remove outliers
00117      // Create the registration model
00118      typedef typename pcl::SampleConsensusModelRegistration<PointT>::Ptr SampleConsensusModelRegistrationPtr;
00119      SampleConsensusModelRegistrationPtr model;
00120      model.reset (new pcl::SampleConsensusModelRegistration<PointT> (input_, source_indices));
00121      // Pass the target_indices
00122      model->setInputTarget (target_, target_indices);
00123      // Create a RANSAC model
00124      pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_);
00125      sac.setMaxIterations (max_iterations_);
00126 
00127      // Compute the set of inliers
00128      if (!sac.computeModel ())
00129      {
00130        remaining_correspondences = original_correspondences;
00131        best_transformation_.setIdentity ();
00132        return;
00133      }
00134      else
00135      {
00136        if (refine_ && !sac.refineModel ())
00137        {
00138          PCL_ERROR ("[pcl::registration::CorrespondenceRejectorSampleConsensus::getRemainingCorrespondences] Could not refine the model! Returning an empty solution.\n");
00139          return;
00140        }
00141        
00142        std::vector<int> inliers;
00143        sac.getInliers (inliers);
00144 
00145        if (inliers.size () < 3)
00146        {
00147          remaining_correspondences = original_correspondences;
00148          best_transformation_.setIdentity ();
00149          return;
00150        }
00151        boost::unordered_map<int, int> index_to_correspondence;
00152        for (int i = 0; i < nr_correspondences; ++i)
00153          index_to_correspondence[original_correspondences[i].index_query] = i;
00154 
00155        remaining_correspondences.resize (inliers.size ());
00156        for (size_t i = 0; i < inliers.size (); ++i)
00157          remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]];
00158 
00159        // get best transformation
00160        Eigen::VectorXf model_coefficients;
00161        sac.getModelCoefficients (model_coefficients);
00162        best_transformation_.row (0) = model_coefficients.segment<4>(0);
00163        best_transformation_.row (1) = model_coefficients.segment<4>(4);
00164        best_transformation_.row (2) = model_coefficients.segment<4>(8);
00165        best_transformation_.row (3) = model_coefficients.segment<4>(12);
00166      }
00167    }
00168 }
00169 
00170 #endif    // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_HPP_


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:23:06