39 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_
40 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_
43 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
bool
46 if (!source_normals_ || !target_normals_)
48 PCL_WARN (
"[pcl::registration::%s::initCompute] Datasets containing normals for source/target have not been given!\n", getClassName ().c_str ());
56 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
64 correspondences.resize (indices_->size ());
66 std::vector<int> nn_indices (k_);
67 std::vector<float> nn_dists (k_);
69 float min_dist = std::numeric_limits<float>::max ();
73 unsigned int nr_valid_correspondences = 0;
77 if (isSamePointType<PointSource, PointTarget> ())
81 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
83 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
86 min_dist = std::numeric_limits<float>::max ();
89 for (
size_t j = 0; j < nn_indices.size (); j++)
91 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
92 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
93 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
94 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
99 min_index =
static_cast<int> (j);
102 if (min_dist > max_distance)
107 corr.
distance = nn_dists[min_index];
108 correspondences[nr_valid_correspondences++] = corr;
116 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
118 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
121 min_dist = std::numeric_limits<float>::max ();
124 for (
size_t j = 0; j < nn_indices.size (); j++)
129 input_->points[*idx_i],
132 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
133 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
134 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
135 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
140 min_index =
static_cast<int> (j);
143 if (min_dist > max_distance)
148 corr.
distance = nn_dists[min_index];
149 correspondences[nr_valid_correspondences++] = corr;
152 correspondences.resize (nr_valid_correspondences);
157 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
167 if(!initComputeReciprocal())
170 correspondences.resize (indices_->size ());
172 std::vector<int> nn_indices (k_);
173 std::vector<float> nn_dists (k_);
174 std::vector<int> index_reciprocal (1);
175 std::vector<float> distance_reciprocal (1);
177 float min_dist = std::numeric_limits<float>::max ();
181 unsigned int nr_valid_correspondences = 0;
186 if (isSamePointType<PointSource, PointTarget> ())
190 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
192 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
195 min_dist = std::numeric_limits<float>::max ();
198 for (
size_t j = 0; j < nn_indices.size (); j++)
200 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
201 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
202 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
203 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
208 min_index =
static_cast<int> (j);
211 if (min_dist > max_distance)
215 target_idx = nn_indices[min_index];
216 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
218 if (*idx_i != index_reciprocal[0])
223 corr.
distance = nn_dists[min_index];
224 correspondences[nr_valid_correspondences++] = corr;
232 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
234 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
237 min_dist = std::numeric_limits<float>::max ();
240 for (
size_t j = 0; j < nn_indices.size (); j++)
245 input_->points[*idx_i],
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);
279 #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_