Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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 * 00037 */ 00038 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_ 00039 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_ 00040 00041 #include <pcl/sample_consensus/sac_model_registration_2d.h> 00042 #include <pcl/sample_consensus/ransac.h> 00043 00044 /////////////////////////////////////////////////////////////////////////////////////////// 00045 template <typename PointT> void 00046 pcl::registration::CorrespondenceRejectorSampleConsensus2D<PointT>::getRemainingCorrespondences ( 00047 const pcl::Correspondences& original_correspondences, 00048 pcl::Correspondences& remaining_correspondences) 00049 { 00050 if (!input_) 00051 { 00052 PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input cloud dataset was given!\n", getClassName ().c_str ()); 00053 return; 00054 } 00055 00056 if (!target_) 00057 { 00058 PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] No input target dataset was given!\n", getClassName ().c_str ()); 00059 return; 00060 } 00061 00062 if (projection_matrix_ == Eigen::Matrix3f::Identity ()) 00063 { 00064 PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Intrinsic camera parameters not given!\n", getClassName ().c_str ()); 00065 return; 00066 } 00067 00068 int nr_correspondences = static_cast<int> (original_correspondences.size ()); 00069 std::vector<int> source_indices (nr_correspondences); 00070 std::vector<int> target_indices (nr_correspondences); 00071 00072 // Copy the query-match indices 00073 for (size_t i = 0; i < original_correspondences.size (); ++i) 00074 { 00075 source_indices[i] = original_correspondences[i].index_query; 00076 target_indices[i] = original_correspondences[i].index_match; 00077 } 00078 00079 // from pcl/registration/icp.hpp: 00080 std::vector<int> source_indices_good; 00081 std::vector<int> target_indices_good; 00082 00083 // From the set of correspondences found, attempt to remove outliers 00084 typename pcl::SampleConsensusModelRegistration2D<PointT>::Ptr model (new pcl::SampleConsensusModelRegistration2D<PointT> (input_, source_indices)); 00085 // Pass the target_indices 00086 model->setInputTarget (target_, target_indices); 00087 model->setProjectionMatrix (projection_matrix_); 00088 00089 // Create a RANSAC model 00090 pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_); 00091 sac.setMaxIterations (max_iterations_); 00092 00093 // Compute the set of inliers 00094 if (!sac.computeModel ()) 00095 { 00096 PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Error computing model! Returning the original correspondences...\n", getClassName ().c_str ()); 00097 remaining_correspondences = original_correspondences; 00098 best_transformation_.setIdentity (); 00099 return; 00100 } 00101 else 00102 { 00103 if (refine_ && !sac.refineModel (2.0)) 00104 PCL_WARN ("[pcl::registration::%s::getRemainingCorrespondences] Error refining model!\n", getClassName ().c_str ()); 00105 00106 std::vector<int> inliers; 00107 sac.getInliers (inliers); 00108 00109 if (inliers.size () < 3) 00110 { 00111 PCL_ERROR ("[pcl::registration::%s::getRemainingCorrespondences] Less than 3 correspondences found!\n", getClassName ().c_str ()); 00112 remaining_correspondences = original_correspondences; 00113 best_transformation_.setIdentity (); 00114 return; 00115 } 00116 00117 boost::unordered_map<int, int> index_to_correspondence; 00118 for (int i = 0; i < nr_correspondences; ++i) 00119 index_to_correspondence[original_correspondences[i].index_query] = i; 00120 00121 remaining_correspondences.resize (inliers.size ()); 00122 for (size_t i = 0; i < inliers.size (); ++i) 00123 remaining_correspondences[i] = original_correspondences[index_to_correspondence[inliers[i]]]; 00124 00125 // get best transformation 00126 Eigen::VectorXf model_coefficients; 00127 sac.getModelCoefficients (model_coefficients); 00128 best_transformation_.row (0) = model_coefficients.segment<4>(0); 00129 best_transformation_.row (1) = model_coefficients.segment<4>(4); 00130 best_transformation_.row (2) = model_coefficients.segment<4>(8); 00131 best_transformation_.row (3) = model_coefficients.segment<4>(12); 00132 } 00133 } 00134 00135 #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_REJECTION_SAMPLE_CONSENSUS_2D_HPP_ 00136