40 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
41 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
43 #include <pcl/common/copy_point.h>
46 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
bool
51 PCL_WARN (
"[pcl::registration::%s::initCompute] Datasets containing normals for source have not been given!\n", getClassName ().c_str ());
59 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
66 correspondences.resize (indices_->size ());
68 std::vector<int> nn_indices (k_);
69 std::vector<float> nn_dists (k_);
71 double min_dist = std::numeric_limits<double>::max ();
75 unsigned int nr_valid_correspondences = 0;
79 if (isSamePointType<PointSource, PointTarget> ())
83 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
85 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
88 min_dist = std::numeric_limits<double>::max ();
91 for (std::size_t j = 0; j < nn_indices.size (); j++)
95 pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x;
96 pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y;
97 pt.z = target_->points[nn_indices[j]].z - input_->points[*idx_i].z;
99 const NormalT &normal = source_normals_->points[*idx_i];
100 Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
101 Eigen::Vector3d V (pt.x, pt.y, pt.z);
102 Eigen::Vector3d C = N.cross (V);
105 double dist = C.dot (C);
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;
126 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
128 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
131 min_dist = std::numeric_limits<double>::max ();
134 for (std::size_t j = 0; j < nn_indices.size (); j++)
138 copyPoint (input_->points[*idx_i], pt_src);
142 pt.x = target_->points[nn_indices[j]].x - pt_src.x;
143 pt.y = target_->points[nn_indices[j]].y - pt_src.y;
144 pt.z = target_->points[nn_indices[j]].z - pt_src.z;
146 const NormalT &normal = source_normals_->points[*idx_i];
147 Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
148 Eigen::Vector3d V (pt.x, pt.y, pt.z);
149 Eigen::Vector3d C = N.cross (V);
152 double dist = C.dot (C);
156 min_index = static_cast<int> (j);
159 if (min_dist > max_distance)
164 corr.
distance = nn_dists[min_index];
165 correspondences[nr_valid_correspondences++] = corr;
168 correspondences.resize (nr_valid_correspondences);
173 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
182 if (!initComputeReciprocal ())
185 correspondences.resize (indices_->size ());
187 std::vector<int> nn_indices (k_);
188 std::vector<float> nn_dists (k_);
189 std::vector<int> index_reciprocal (1);
190 std::vector<float> distance_reciprocal (1);
192 double min_dist = std::numeric_limits<double>::max ();
196 unsigned int nr_valid_correspondences = 0;
201 if (isSamePointType<PointSource, PointTarget> ())
205 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
207 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
210 min_dist = std::numeric_limits<double>::max ();
213 for (std::size_t j = 0; j < nn_indices.size (); j++)
217 pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x;
218 pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y;
219 pt.z = target_->points[nn_indices[j]].z - input_->points[*idx_i].z;
221 const NormalT &normal = source_normals_->points[*idx_i];
222 Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
223 Eigen::Vector3d V (pt.x, pt.y, pt.z);
224 Eigen::Vector3d C = N.cross (V);
227 double dist = C.dot (C);
231 min_index = static_cast<int> (j);
234 if (min_dist > max_distance)
238 target_idx = nn_indices[min_index];
239 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
241 if (*idx_i != index_reciprocal[0])
247 corr.
distance = nn_dists[min_index];
248 correspondences[nr_valid_correspondences++] = corr;
256 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
258 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
261 min_dist = std::numeric_limits<double>::max ();
264 for (std::size_t j = 0; j < nn_indices.size (); j++)
268 copyPoint (input_->points[*idx_i], pt_src);
272 pt.x = target_->points[nn_indices[j]].x - pt_src.x;
273 pt.y = target_->points[nn_indices[j]].y - pt_src.y;
274 pt.z = target_->points[nn_indices[j]].z - pt_src.z;
276 const NormalT &normal = source_normals_->points[*idx_i];
277 Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
278 Eigen::Vector3d V (pt.x, pt.y, pt.z);
279 Eigen::Vector3d C = N.cross (V);
282 double dist = C.dot (C);
286 min_index = static_cast<int> (j);
289 if (min_dist > max_distance)
293 target_idx = nn_indices[min_index];
294 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
296 if (*idx_i != index_reciprocal[0])
302 corr.
distance = nn_dists[min_index];
303 correspondences[nr_valid_correspondences++] = corr;
306 correspondences.resize (nr_valid_correspondences);
310 #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_