41#ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
42#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
44#include <pcl/common/copy_point.h>
45#include <pcl/common/io.h>
49namespace registration {
51template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
56 if (cloud->points.empty()) {
57 PCL_ERROR(
"[pcl::registration::%s::setInputTarget] Invalid or empty point cloud "
59 getClassName().
c_str());
65 if (point_representation_)
66 tree_->setPointRepresentation(point_representation_);
68 target_cloud_updated_ =
true;
71template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
76 PCL_ERROR(
"[pcl::registration::%s::compute] No input target dataset was given!\n",
77 getClassName().
c_str());
82 if (target_cloud_updated_ && !force_no_recompute_) {
85 tree_->setInputCloud(target_, target_indices_);
87 tree_->setInputCloud(target_);
89 target_cloud_updated_ =
false;
95template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
100 if (source_cloud_updated_ && !force_no_recompute_reciprocal_) {
101 if (point_representation_)
102 tree_reciprocal_->setPointRepresentation(point_representation_);
105 tree_reciprocal_->setInputCloud(getInputSource(), getIndicesSource());
107 tree_reciprocal_->setInputCloud(getInputSource());
109 source_cloud_updated_ =
false;
115template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
125 correspondences.resize(indices_->size());
128 std::vector<float> distance(1);
137 for (
const auto& idx : (*indices_)) {
138 tree_->nearestKSearch((*input_)[idx], 1, index, distance);
142 corr.index_query = idx;
143 corr.index_match = index[0];
144 corr.distance = distance[0];
152 for (
const auto& idx : (*indices_)) {
157 tree_->nearestKSearch(
pt, 1, index, distance);
161 corr.index_query = idx;
162 corr.index_match = index[0];
163 corr.distance = distance[0];
167 correspondences.resize(nr_valid_correspondences);
171template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
182 if (!initComputeReciprocal())
186 correspondences.resize(indices_->size());
188 std::vector<float> distance(1);
200 for (
const auto& idx : (*indices_)) {
201 tree_->nearestKSearch((*input_)[idx], 1, index, distance);
207 tree_reciprocal_->nearestKSearch(
212 corr.index_query = idx;
213 corr.index_match = index[0];
214 corr.distance = distance[0];
223 for (
const auto& idx : (*indices_)) {
228 tree_->nearestKSearch(
pt_src, 1, index, distance);
238 tree_reciprocal_->nearestKSearch(
243 corr.index_query = idx;
244 corr.index_match = index[0];
245 corr.distance = distance[0];
Iterator class for point clouds with or without given indices.
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.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Correspondence represents a match between two entities (e.g., points, descriptors,...