40 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_
41 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_
43 #include <pcl/common/copy_point.h>
49 namespace registration
52 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
bool
55 if (!source_normals_ || !target_normals_)
57 PCL_WARN (
"[pcl::registration::%s::initCompute] Datasets containing normals for source/target have not been given!\n", getClassName ().c_str ());
65 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
72 correspondences.resize (indices_->size ());
74 std::vector<int> nn_indices (k_);
75 std::vector<float> nn_dists (k_);
80 unsigned int nr_valid_correspondences = 0;
84 if (isSamePointType<PointSource, PointTarget> ())
88 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
90 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
93 float min_dist = std::numeric_limits<float>::max ();
96 for (std::size_t j = 0; j < nn_indices.size (); j++)
98 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
99 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
100 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
101 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
106 min_index =
static_cast<int> (j);
109 if (min_dist > max_distance)
114 corr.
distance = nn_dists[min_index];
115 correspondences[nr_valid_correspondences++] = corr;
123 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
125 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
128 float min_dist = std::numeric_limits<float>::max ();
131 for (std::size_t j = 0; j < nn_indices.size (); j++)
135 copyPoint (input_->points[*idx_i], pt_src);
137 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
138 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
139 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
140 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
145 min_index =
static_cast<int> (j);
148 if (min_dist > max_distance)
153 corr.
distance = nn_dists[min_index];
154 correspondences[nr_valid_correspondences++] = corr;
157 correspondences.resize (nr_valid_correspondences);
162 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
170 if(!initComputeReciprocal())
173 correspondences.resize (indices_->size ());
175 std::vector<int> nn_indices (k_);
176 std::vector<float> nn_dists (k_);
177 std::vector<int> index_reciprocal (1);
178 std::vector<float> distance_reciprocal (1);
183 unsigned int nr_valid_correspondences = 0;
188 if (isSamePointType<PointSource, PointTarget> ())
192 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
194 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
197 float min_dist = std::numeric_limits<float>::max ();
200 for (std::size_t j = 0; j < nn_indices.size (); j++)
202 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
203 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
204 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
205 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
210 min_index =
static_cast<int> (j);
213 if (min_dist > max_distance)
217 target_idx = nn_indices[min_index];
218 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
220 if (*idx_i != index_reciprocal[0])
225 corr.
distance = nn_dists[min_index];
226 correspondences[nr_valid_correspondences++] = corr;
234 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
236 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
239 float min_dist = std::numeric_limits<float>::max ();
242 for (std::size_t j = 0; j < nn_indices.size (); j++)
246 copyPoint (input_->points[*idx_i], pt_src);
248 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
249 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
250 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
251 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
256 min_index =
static_cast<int> (j);
259 if (min_dist > max_distance)
263 target_idx = nn_indices[min_index];
264 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
266 if (*idx_i != index_reciprocal[0])
271 corr.
distance = nn_dists[min_index];
272 correspondences[nr_valid_correspondences++] = corr;
275 correspondences.resize (nr_valid_correspondences);
bool initCompute()
Internal computation initialization.
virtual void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())
Determine the reciprocal correspondences between input and target cloud.
void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())
Determine the correspondences between input and target cloud.
Abstract CorrespondenceEstimationBase class.
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,...
int index_query
Index of the query (source) point.
int index_match
Index of the matching (target) point.