40 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
41 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
43 #include <pcl/common/io.h>
44 #include <pcl/common/copy_point.h>
47 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
51 if (cloud->points.empty ())
53 PCL_ERROR (
"[pcl::registration::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
59 if (point_representation_)
60 tree_->setPointRepresentation (point_representation_);
62 target_cloud_updated_ =
true;
66 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
bool
71 PCL_ERROR (
"[pcl::registration::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
76 if (target_cloud_updated_ && !force_no_recompute_)
80 tree_->setInputCloud (target_, target_indices_);
82 tree_->setInputCloud (target_);
84 target_cloud_updated_ =
false;
91 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
bool
95 if (source_cloud_updated_ && !force_no_recompute_reciprocal_)
97 if (point_representation_)
98 tree_reciprocal_->setPointRepresentation (point_representation_);
101 tree_reciprocal_->setInputCloud (getInputSource(), getIndicesSource());
103 tree_reciprocal_->setInputCloud (getInputSource());
105 source_cloud_updated_ =
false;
112 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
119 double max_dist_sqr = max_distance * max_distance;
121 correspondences.resize (indices_->size ());
123 std::vector<int> index (1);
126 unsigned int nr_valid_correspondences = 0;
130 if (isSamePointType<PointSource, PointTarget> ())
133 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
135 tree_->nearestKSearch (input_->points[*idx], 1, index,
distance);
142 correspondences[nr_valid_correspondences++] = corr;
150 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
155 tree_->nearestKSearch (pt, 1, index,
distance);
162 correspondences[nr_valid_correspondences++] = corr;
165 correspondences.resize (nr_valid_correspondences);
170 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
179 if (!initComputeReciprocal())
181 double max_dist_sqr = max_distance * max_distance;
183 correspondences.resize (indices_->size());
184 std::vector<int> index (1);
186 std::vector<int> index_reciprocal (1);
187 std::vector<float> distance_reciprocal (1);
189 unsigned int nr_valid_correspondences = 0;
194 if (isSamePointType<PointSource, PointTarget> ())
197 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
199 tree_->nearestKSearch (input_->points[*idx], 1, index,
distance);
203 target_idx = index[0];
205 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
206 if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
212 correspondences[nr_valid_correspondences++] = corr;
221 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
224 copyPoint (input_->points[*idx], pt_src);
226 tree_->nearestKSearch (pt_src, 1, index,
distance);
230 target_idx = index[0];
233 copyPoint (target_->points[target_idx], pt_tgt);
235 tree_reciprocal_->nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal);
236 if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
242 correspondences[nr_valid_correspondences++] = corr;
245 correspondences.resize (nr_valid_correspondences);