41 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
42 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
44 #include <pcl/common/io.h>
45 #include <pcl/common/copy_point.h>
51 namespace registration
54 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
58 if (cloud->points.empty ())
60 PCL_ERROR (
"[pcl::registration::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
66 if (point_representation_)
67 tree_->setPointRepresentation (point_representation_);
69 target_cloud_updated_ =
true;
73 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
bool
78 PCL_ERROR (
"[pcl::registration::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
83 if (target_cloud_updated_ && !force_no_recompute_)
87 tree_->setInputCloud (target_, target_indices_);
89 tree_->setInputCloud (target_);
91 target_cloud_updated_ =
false;
98 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
bool
102 if (source_cloud_updated_ && !force_no_recompute_reciprocal_)
104 if (point_representation_)
105 tree_reciprocal_->setPointRepresentation (point_representation_);
108 tree_reciprocal_->setInputCloud (getInputSource(), getIndicesSource());
110 tree_reciprocal_->setInputCloud (getInputSource());
112 source_cloud_updated_ =
false;
119 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
126 double max_dist_sqr = max_distance * max_distance;
128 correspondences.resize (indices_->size ());
130 std::vector<int> index (1);
133 unsigned int nr_valid_correspondences = 0;
137 if (isSamePointType<PointSource, PointTarget> ())
140 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
142 tree_->nearestKSearch (input_->points[*idx], 1, index,
distance);
149 correspondences[nr_valid_correspondences++] = corr;
157 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
162 tree_->nearestKSearch (pt, 1, index,
distance);
169 correspondences[nr_valid_correspondences++] = corr;
172 correspondences.resize (nr_valid_correspondences);
177 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
186 if (!initComputeReciprocal())
188 double max_dist_sqr = max_distance * max_distance;
190 correspondences.resize (indices_->size());
191 std::vector<int> index (1);
193 std::vector<int> index_reciprocal (1);
194 std::vector<float> distance_reciprocal (1);
196 unsigned int nr_valid_correspondences = 0;
201 if (isSamePointType<PointSource, PointTarget> ())
204 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
206 tree_->nearestKSearch (input_->points[*idx], 1, index,
distance);
210 target_idx = index[0];
212 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
213 if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
219 correspondences[nr_valid_correspondences++] = corr;
228 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
231 copyPoint (input_->points[*idx], pt_src);
233 tree_->nearestKSearch (pt_src, 1, index,
distance);
237 target_idx = index[0];
240 copyPoint (target_->points[target_idx], pt_tgt);
242 tree_reciprocal_->nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal);
243 if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
249 correspondences[nr_valid_correspondences++] = corr;
252 correspondences.resize (nr_valid_correspondences);
bool initCompute()
Internal computation initialization.
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
bool initComputeReciprocal()
Internal computation initialization for reciprocal correspondences.
void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) override
Determine the correspondences between input and target cloud.
void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) override
Determine the reciprocal correspondences between input and target cloud.
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
float distance(const PointT &p1, const PointT &p2)
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Correspondence represents a match between two entities (e.g., points, descriptors,...
int index_query
Index of the query (source) point.
int index_match
Index of the matching (target) point.