40 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
41 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
43 #include <pcl/common/concatenate.h>
44 #include <pcl/common/io.h>
47 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
50 setInputSource (cloud);
57 return (getInputSource ());
61 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
65 if (cloud->points.empty ())
67 PCL_ERROR (
"[pcl::registration::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
73 if (point_representation_)
74 tree_->setPointRepresentation (point_representation_);
76 target_cloud_updated_ =
true;
80 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
bool
85 PCL_ERROR (
"[pcl::registration::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
90 if (target_cloud_updated_ && !force_no_recompute_)
94 tree_->setInputCloud (target_, target_indices_);
96 tree_->setInputCloud (target_);
98 target_cloud_updated_ =
false;
105 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
bool
109 if (source_cloud_updated_ && !force_no_recompute_reciprocal_)
111 if (point_representation_)
112 tree_reciprocal_->setPointRepresentation (point_representation_);
115 tree_reciprocal_->setInputCloud (getInputSource(), getIndicesSource());
117 tree_reciprocal_->setInputCloud (getInputSource());
119 source_cloud_updated_ =
false;
126 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
133 double max_dist_sqr = max_distance * max_distance;
136 correspondences.resize (indices_->size ());
138 std::vector<int> index (1);
139 std::vector<float> distance (1);
141 unsigned int nr_valid_correspondences = 0;
145 if (isSamePointType<PointSource, PointTarget> ())
148 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
150 tree_->nearestKSearch (input_->points[*idx], 1, index, distance);
151 if (distance[0] > max_dist_sqr)
157 correspondences[nr_valid_correspondences++] = corr;
165 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
169 input_->points[*idx],
172 tree_->nearestKSearch (pt, 1, index, distance);
173 if (distance[0] > max_dist_sqr)
179 correspondences[nr_valid_correspondences++] = corr;
182 correspondences.resize (nr_valid_correspondences);
187 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void
200 if (!initComputeReciprocal())
202 double max_dist_sqr = max_distance * max_distance;
204 correspondences.resize (indices_->size());
205 std::vector<int> index (1);
206 std::vector<float> distance (1);
207 std::vector<int> index_reciprocal (1);
208 std::vector<float> distance_reciprocal (1);
210 unsigned int nr_valid_correspondences = 0;
215 if (isSamePointType<PointSource, PointTarget> ())
218 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
220 tree_->nearestKSearch (input_->points[*idx], 1, index, distance);
221 if (distance[0] > max_dist_sqr)
224 target_idx = index[0];
226 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
227 if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
233 correspondences[nr_valid_correspondences++] = corr;
242 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
246 input_->points[*idx],
249 tree_->nearestKSearch (pt_src, 1, index, distance);
250 if (distance[0] > max_dist_sqr)
253 target_idx = index[0];
257 target_->points[target_idx],
260 tree_reciprocal_->nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal);
261 if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
267 correspondences[nr_valid_correspondences++] = corr;
270 correspondences.resize (nr_valid_correspondences);
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
PointCloudConstPtr const getInputCloud()
Get a pointer to the input point cloud dataset.
PointCloudTarget::ConstPtr PointCloudTargetConstPtr
bool initCompute()
Internal computation initalization.
virtual void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())
Determine the reciprocal correspondences between input and target cloud.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
boost::mpl::remove_if< Sequence1, boost::mpl::not_< boost::mpl::contains< Sequence2, boost::mpl::_1 > > >::type type
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...
PointCloudSource::ConstPtr PointCloudSourceConstPtr
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.
int index_query
Index of the query (source) point.
bool initComputeReciprocal()
Internal computation initalization for reciprocal correspondences.
Correspondence represents a match between two entities (e.g., points, descriptors, etc).
Helper functor structure for concatenate.