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>
47 namespace registration {
49 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
54 if (!source_normals_ || !target_normals_) {
55 PCL_WARN(
"[pcl::registration::%s::initCompute] Datasets containing normals for "
56 "source/target have not been given!\n",
57 getClassName().c_str());
66 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
74 correspondences.resize(indices_->size());
77 std::vector<float> nn_dists(k_);
82 unsigned int nr_valid_correspondences = 0;
87 if (isSamePointType<PointSource, PointTarget>()) {
90 for (
const auto& idx_i : (*indices_)) {
91 tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
95 float min_dist = std::numeric_limits<float>::max();
98 for (std::size_t j = 0; j < nn_indices.size(); j++) {
99 float cos_angle = (*source_normals_)[idx_i].normal_x *
100 (*target_normals_)[nn_indices[j]].normal_x +
101 (*source_normals_)[idx_i].normal_y *
102 (*target_normals_)[nn_indices[j]].normal_y +
103 (*source_normals_)[idx_i].normal_z *
104 (*target_normals_)[nn_indices[j]].normal_z;
105 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
107 if (dist < min_dist) {
109 min_index =
static_cast<int>(j);
112 if (min_dist > max_distance)
117 corr.
distance = nn_dists[min_index];
118 correspondences[nr_valid_correspondences++] = corr;
125 for (
const auto& idx_i : (*indices_)) {
126 tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
130 float min_dist = std::numeric_limits<float>::max();
133 for (std::size_t j = 0; j < nn_indices.size(); j++) {
139 float cos_angle = (*source_normals_)[idx_i].normal_x *
140 (*target_normals_)[nn_indices[j]].normal_x +
141 (*source_normals_)[idx_i].normal_y *
142 (*target_normals_)[nn_indices[j]].normal_y +
143 (*source_normals_)[idx_i].normal_z *
144 (*target_normals_)[nn_indices[j]].normal_z;
145 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
147 if (dist < min_dist) {
149 min_index =
static_cast<int>(j);
152 if (min_dist > max_distance)
157 corr.
distance = nn_dists[min_index];
158 correspondences[nr_valid_correspondences++] = corr;
161 correspondences.resize(nr_valid_correspondences);
165 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
175 if (!initComputeReciprocal())
178 correspondences.resize(indices_->size());
181 std::vector<float> nn_dists(k_);
183 std::vector<float> distance_reciprocal(1);
188 unsigned int nr_valid_correspondences = 0;
194 if (isSamePointType<PointSource, PointTarget>()) {
197 for (
const auto& idx_i : (*indices_)) {
198 tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
202 float min_dist = std::numeric_limits<float>::max();
205 for (std::size_t j = 0; j < nn_indices.size(); j++) {
206 float cos_angle = (*source_normals_)[idx_i].normal_x *
207 (*target_normals_)[nn_indices[j]].normal_x +
208 (*source_normals_)[idx_i].normal_y *
209 (*target_normals_)[nn_indices[j]].normal_y +
210 (*source_normals_)[idx_i].normal_z *
211 (*target_normals_)[nn_indices[j]].normal_z;
212 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
214 if (dist < min_dist) {
216 min_index =
static_cast<int>(j);
219 if (min_dist > max_distance)
223 target_idx = nn_indices[min_index];
224 tree_reciprocal_->nearestKSearch(
225 (*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
227 if (idx_i != index_reciprocal[0])
232 corr.
distance = nn_dists[min_index];
233 correspondences[nr_valid_correspondences++] = corr;
240 for (
const auto& idx_i : (*indices_)) {
241 tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
245 float min_dist = std::numeric_limits<float>::max();
248 for (std::size_t j = 0; j < nn_indices.size(); j++) {
254 float cos_angle = (*source_normals_)[idx_i].normal_x *
255 (*target_normals_)[nn_indices[j]].normal_x +
256 (*source_normals_)[idx_i].normal_y *
257 (*target_normals_)[nn_indices[j]].normal_y +
258 (*source_normals_)[idx_i].normal_z *
259 (*target_normals_)[nn_indices[j]].normal_z;
260 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
262 if (dist < min_dist) {
264 min_index =
static_cast<int>(j);
267 if (min_dist > max_distance)
271 target_idx = nn_indices[min_index];
272 tree_reciprocal_->nearestKSearch(
273 (*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
275 if (idx_i != index_reciprocal[0])
280 corr.
distance = nn_dists[min_index];
281 correspondences[nr_valid_correspondences++] = corr;
284 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
IndicesAllocator<> Indices
Type used for indices in PCL.
Correspondence represents a match between two entities (e.g., points, descriptors,...
index_t index_query
Index of the query (source) point.
index_t index_match
Index of the matching (target) point.