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);
int index_match
Index of the matching (target) point.
virtual void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())
Determine the correspondences between input and target cloud.
Correspondence represents a match between two entities (e.g., points, descriptors,...
int index_query
Index of the query (source) point.
bool initCompute()
Internal computation initialization.
PointCloudTarget::ConstPtr PointCloudTargetConstPtr
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
float distance(const PointT &p1, const PointT &p2)
bool initComputeReciprocal()
Internal computation initialization for reciprocal correspondences.
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...
virtual void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())
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.