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_ESTIMATION_H_
00039 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
00040
00041 #include <pcl/common/concatenate.h>
00042
00043
00045 template <typename PointSource, typename PointTarget> inline void
00046 pcl::registration::CorrespondenceEstimation<PointSource, PointTarget>::setInputTarget (
00047 const PointCloudTargetConstPtr &cloud)
00048 {
00049 if (cloud->points.empty ())
00050 {
00051 PCL_ERROR ("[pcl::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
00052 return;
00053 }
00054 target_ = cloud;
00055 tree_->setInputCloud (target_);
00056 }
00057
00059 template <typename PointSource, typename PointTarget> void
00060 pcl::registration::CorrespondenceEstimation<PointSource, PointTarget>::determineCorrespondences (
00061 pcl::Correspondences &correspondences, float max_distance)
00062 {
00063 typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget;
00064
00065 if (!initCompute ())
00066 return;
00067
00068 if (!target_)
00069 {
00070 PCL_WARN ("[pcl::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
00071 return;
00072 }
00073
00074 float max_dist_sqr = max_distance * max_distance;
00075
00076 correspondences.resize (indices_->size ());
00077 std::vector<int> index (1);
00078 std::vector<float> distance (1);
00079 pcl::Correspondence corr;
00080 for (size_t i = 0; i < indices_->size (); ++i)
00081 {
00082
00083 PointTarget pt;
00084 pcl::for_each_type <FieldListTarget> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
00085 input_->points[(*indices_)[i]],
00086 pt));
00087
00088
00089 if (tree_->nearestKSearch (pt, 1, index, distance))
00090 {
00091 if (distance[0] <= max_dist_sqr)
00092 {
00093 corr.index_query = static_cast<int> (i);
00094 corr.index_match = index[0];
00095 corr.distance = distance[0];
00096 correspondences[i] = corr;
00097 continue;
00098 }
00099 }
00100
00101 }
00102 deinitCompute ();
00103 }
00104
00106 template <typename PointSource, typename PointTarget> void
00107 pcl::registration::CorrespondenceEstimation<PointSource, PointTarget>::determineReciprocalCorrespondences (
00108 pcl::Correspondences &correspondences)
00109 {
00110 typedef typename pcl::traits::fieldList<PointSource>::type FieldListSource;
00111 typedef typename pcl::traits::fieldList<PointTarget>::type FieldListTarget;
00112 typedef typename pcl::intersect<FieldListSource, FieldListTarget>::type FieldList;
00113
00114 if (!initCompute ())
00115 return;
00116
00117 if (!target_)
00118 {
00119 PCL_WARN ("[pcl::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
00120 return;
00121 }
00122
00123
00124 pcl::KdTreeFLANN<PointSource> tree_reciprocal;
00125 tree_reciprocal.setInputCloud (input_, indices_);
00126
00127 correspondences.resize (indices_->size());
00128 std::vector<int> index (1);
00129 std::vector<float> distance (1);
00130 std::vector<int> index_reciprocal (1);
00131 std::vector<float> distance_reciprocal (1);
00132 pcl::Correspondence corr;
00133 unsigned int nr_valid_correspondences = 0;
00134
00135 for (size_t i = 0; i < indices_->size (); ++i)
00136 {
00137
00138 PointTarget pt_src;
00139 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointSource, PointTarget> (
00140 input_->points[(*indices_)[i]],
00141 pt_src));
00142
00143
00144 tree_->nearestKSearch (pt_src, 1, index, distance);
00145
00146
00147 PointSource pt_tgt;
00148 pcl::for_each_type <FieldList> (pcl::NdConcatenateFunctor <PointTarget, PointSource> (
00149 target_->points[index[0]],
00150 pt_tgt));
00151
00152 tree_reciprocal.nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal);
00153
00154 if ((*indices_)[i] == index_reciprocal[0])
00155 {
00156 corr.index_query = (*indices_)[i];
00157 corr.index_match = index[0];
00158 corr.distance = distance[0];
00159 correspondences[nr_valid_correspondences] = corr;
00160 ++nr_valid_correspondences;
00161 }
00162 }
00163 correspondences.resize (nr_valid_correspondences);
00164
00165 deinitCompute ();
00166 }
00167
00168
00169
00170 #endif