Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
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
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
00080 std::vector<int> source_indices_good;
00081 std::vector<int> target_indices_good;
00082
00083
00084 typename pcl::SampleConsensusModelRegistration2D<PointT>::Ptr model (new pcl::SampleConsensusModelRegistration2D<PointT> (input_, source_indices));
00085
00086 model->setInputTarget (target_, target_indices);
00087 model->setProjectionMatrix (projection_matrix_);
00088
00089
00090 pcl::RandomSampleConsensus<PointT> sac (model, inlier_threshold_);
00091 sac.setMaxIterations (max_iterations_);
00092
00093
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
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