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 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;
135 correspondences.resize (indices_->size ());
137 std::vector<int> index (1);
138 std::vector<float> distance (1);
140 unsigned int nr_valid_correspondences = 0;
144 if (isSamePointType<PointSource, PointTarget> ())
147 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
149 tree_->nearestKSearch (input_->points[*idx], 1, index, distance);
150 if (distance[0] > max_dist_sqr)
156 correspondences[nr_valid_correspondences++] = corr;
164 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
169 tree_->nearestKSearch (pt, 1, index, distance);
170 if (distance[0] > max_dist_sqr)
176 correspondences[nr_valid_correspondences++] = corr;
179 correspondences.resize (nr_valid_correspondences);
184 template <
typename Po
intSource,
typename Po
intTarget,
typename Scalar>
void 193 if (!initComputeReciprocal())
195 double max_dist_sqr = max_distance * max_distance;
197 correspondences.resize (indices_->size());
198 std::vector<int> index (1);
199 std::vector<float> distance (1);
200 std::vector<int> index_reciprocal (1);
201 std::vector<float> distance_reciprocal (1);
203 unsigned int nr_valid_correspondences = 0;
208 if (isSamePointType<PointSource, PointTarget> ())
211 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
213 tree_->nearestKSearch (input_->points[*idx], 1, index, distance);
214 if (distance[0] > max_dist_sqr)
217 target_idx = index[0];
219 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
220 if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
226 correspondences[nr_valid_correspondences++] = corr;
235 for (std::vector<int>::const_iterator idx = indices_->begin (); idx != indices_->end (); ++idx)
238 copyPoint (input_->points[*idx], pt_src);
240 tree_->nearestKSearch (pt_src, 1, index, distance);
241 if (distance[0] > max_dist_sqr)
244 target_idx = index[0];
247 copyPoint (target_->points[target_idx], pt_tgt);
249 tree_reciprocal_->nearestKSearch (pt_tgt, 1, index_reciprocal, distance_reciprocal);
250 if (distance_reciprocal[0] > max_dist_sqr || *idx != index_reciprocal[0])
256 correspondences[nr_valid_correspondences++] = corr;
259 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.
void setInputCloud(const PointCloudSourceConstPtr &cloud)
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target) ...
Correspondence represents a match between two entities (e.g., points, descriptors, etc).
int index_query
Index of the query (source) point.
PointCloudSourceConstPtr const getInputCloud()
Get a pointer to the input point cloud dataset target.
bool initCompute()
Internal computation initalization.
PointCloudTarget::ConstPtr PointCloudTargetConstPtr
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
bool initComputeReciprocal()
Internal computation initalization 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.
PointCloudSource::ConstPtr PointCloudSourceConstPtr