41 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
42 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
44 #include <pcl/common/copy_point.h>
50 namespace registration
53 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
bool
58 PCL_WARN (
"[pcl::registration::%s::initCompute] Datasets containing normals for source have not been given!\n", getClassName ().c_str ());
66 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
73 correspondences.resize (indices_->size ());
75 std::vector<int> nn_indices (k_);
76 std::vector<float> nn_dists (k_);
81 unsigned int nr_valid_correspondences = 0;
85 if (isSamePointType<PointSource, PointTarget> ())
89 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
91 tree_->nearestKSearch ((*input_)[*idx_i], k_, nn_indices, nn_dists);
94 double min_dist = std::numeric_limits<double>::max ();
97 for (std::size_t j = 0; j < nn_indices.size (); j++)
101 pt.x = (*target_)[nn_indices[j]].x - (*input_)[*idx_i].x;
102 pt.y = (*target_)[nn_indices[j]].y - (*input_)[*idx_i].y;
103 pt.z = (*target_)[nn_indices[j]].z - (*input_)[*idx_i].z;
105 const NormalT &normal = (*source_normals_)[*idx_i];
106 Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
107 Eigen::Vector3d V (pt.x, pt.y, pt.z);
108 Eigen::Vector3d C = N.cross (V);
111 double dist = C.dot (C);
115 min_index =
static_cast<int> (j);
118 if (min_dist > max_distance)
123 corr.
distance = nn_dists[min_index];
124 correspondences[nr_valid_correspondences++] = corr;
132 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
134 tree_->nearestKSearch ((*input_)[*idx_i], k_, nn_indices, nn_dists);
137 double min_dist = std::numeric_limits<double>::max ();
140 for (std::size_t j = 0; j < nn_indices.size (); j++)
148 pt.x = (*target_)[nn_indices[j]].x - pt_src.x;
149 pt.y = (*target_)[nn_indices[j]].y - pt_src.y;
150 pt.z = (*target_)[nn_indices[j]].z - pt_src.z;
152 const NormalT &normal = (*source_normals_)[*idx_i];
153 Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
154 Eigen::Vector3d V (pt.x, pt.y, pt.z);
155 Eigen::Vector3d C = N.cross (V);
158 double dist = C.dot (C);
162 min_index =
static_cast<int> (j);
165 if (min_dist > max_distance)
170 corr.
distance = nn_dists[min_index];
171 correspondences[nr_valid_correspondences++] = corr;
174 correspondences.resize (nr_valid_correspondences);
179 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
188 if (!initComputeReciprocal ())
191 correspondences.resize (indices_->size ());
193 std::vector<int> nn_indices (k_);
194 std::vector<float> nn_dists (k_);
195 std::vector<int> index_reciprocal (1);
196 std::vector<float> distance_reciprocal (1);
201 unsigned int nr_valid_correspondences = 0;
206 if (isSamePointType<PointSource, PointTarget> ())
210 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
212 tree_->nearestKSearch ((*input_)[*idx_i], k_, nn_indices, nn_dists);
215 double min_dist = std::numeric_limits<double>::max ();
218 for (std::size_t j = 0; j < nn_indices.size (); j++)
222 pt.x = (*target_)[nn_indices[j]].x - (*input_)[*idx_i].x;
223 pt.y = (*target_)[nn_indices[j]].y - (*input_)[*idx_i].y;
224 pt.z = (*target_)[nn_indices[j]].z - (*input_)[*idx_i].z;
226 const NormalT &normal = (*source_normals_)[*idx_i];
227 Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
228 Eigen::Vector3d V (pt.x, pt.y, pt.z);
229 Eigen::Vector3d C = N.cross (V);
232 double dist = C.dot (C);
236 min_index =
static_cast<int> (j);
239 if (min_dist > max_distance)
243 target_idx = nn_indices[min_index];
244 tree_reciprocal_->nearestKSearch ((*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
246 if (*idx_i != index_reciprocal[0])
252 corr.
distance = nn_dists[min_index];
253 correspondences[nr_valid_correspondences++] = corr;
261 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
263 tree_->nearestKSearch ((*input_)[*idx_i], k_, nn_indices, nn_dists);
266 double min_dist = std::numeric_limits<double>::max ();
269 for (std::size_t j = 0; j < nn_indices.size (); j++)
277 pt.x = (*target_)[nn_indices[j]].x - pt_src.x;
278 pt.y = (*target_)[nn_indices[j]].y - pt_src.y;
279 pt.z = (*target_)[nn_indices[j]].z - pt_src.z;
281 const NormalT &normal = (*source_normals_)[*idx_i];
282 Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
283 Eigen::Vector3d V (pt.x, pt.y, pt.z);
284 Eigen::Vector3d C = N.cross (V);
287 double dist = C.dot (C);
291 min_index =
static_cast<int> (j);
294 if (min_dist > max_distance)
298 target_idx = nn_indices[min_index];
299 tree_reciprocal_->nearestKSearch ((*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
301 if (*idx_i != index_reciprocal[0])
307 corr.
distance = nn_dists[min_index];
308 correspondences[nr_valid_correspondences++] = corr;
311 correspondences.resize (nr_valid_correspondences);
318 #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_