39 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_
40 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_
42 #include <pcl/common/copy_point.h>
45 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
bool
48 if (!source_normals_ || !target_normals_)
50 PCL_WARN (
"[pcl::registration::%s::initCompute] Datasets containing normals for source/target have not been given!\n", getClassName ().c_str ());
58 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
65 correspondences.resize (indices_->size ());
67 std::vector<int> nn_indices (k_);
68 std::vector<float> nn_dists (k_);
70 float min_dist = std::numeric_limits<float>::max ();
74 unsigned int nr_valid_correspondences = 0;
78 if (isSamePointType<PointSource, PointTarget> ())
82 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
84 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
87 min_dist = std::numeric_limits<float>::max ();
90 for (std::size_t j = 0; j < nn_indices.size (); j++)
92 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
93 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
94 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
95 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
100 min_index =
static_cast<int> (j);
103 if (min_dist > max_distance)
108 corr.
distance = nn_dists[min_index];
109 correspondences[nr_valid_correspondences++] = corr;
117 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
119 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
122 min_dist = std::numeric_limits<float>::max ();
125 for (std::size_t j = 0; j < nn_indices.size (); j++)
129 copyPoint (input_->points[*idx_i], pt_src);
131 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
132 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
133 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
134 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
139 min_index =
static_cast<int> (j);
142 if (min_dist > max_distance)
147 corr.
distance = nn_dists[min_index];
148 correspondences[nr_valid_correspondences++] = corr;
151 correspondences.resize (nr_valid_correspondences);
156 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
164 if(!initComputeReciprocal())
167 correspondences.resize (indices_->size ());
169 std::vector<int> nn_indices (k_);
170 std::vector<float> nn_dists (k_);
171 std::vector<int> index_reciprocal (1);
172 std::vector<float> distance_reciprocal (1);
174 float min_dist = std::numeric_limits<float>::max ();
178 unsigned int nr_valid_correspondences = 0;
183 if (isSamePointType<PointSource, PointTarget> ())
187 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
189 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
192 min_dist = std::numeric_limits<float>::max ();
195 for (std::size_t j = 0; j < nn_indices.size (); j++)
197 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
198 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
199 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
200 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
205 min_index =
static_cast<int> (j);
208 if (min_dist > max_distance)
212 target_idx = nn_indices[min_index];
213 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
215 if (*idx_i != index_reciprocal[0])
220 corr.
distance = nn_dists[min_index];
221 correspondences[nr_valid_correspondences++] = corr;
229 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
231 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
234 min_dist = std::numeric_limits<float>::max ();
237 for (std::size_t j = 0; j < nn_indices.size (); j++)
241 copyPoint (input_->points[*idx_i], pt_src);
243 float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
244 source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
245 source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
246 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
251 min_index =
static_cast<int> (j);
254 if (min_dist > max_distance)
258 target_idx = nn_indices[min_index];
259 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
261 if (*idx_i != index_reciprocal[0])
266 corr.
distance = nn_dists[min_index];
267 correspondences[nr_valid_correspondences++] = corr;
270 correspondences.resize (nr_valid_correspondences);
274 #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_