40 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
41 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
44 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
bool
49 PCL_WARN (
"[pcl::registration::%s::initCompute] Datasets containing normals for source have not been given!\n", getClassName ().c_str ());
57 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 double min_dist = std::numeric_limits<double>::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<double>::max ();
90 for (
size_t j = 0; j < nn_indices.size (); j++)
94 pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x;
95 pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y;
96 pt.z = target_->points[nn_indices[j]].z - input_->points[*idx_i].z;
98 const NormalT &normal = source_normals_->points[*idx_i];
99 Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
100 Eigen::Vector3d V (pt.x, pt.y, pt.z);
101 Eigen::Vector3d C = N.cross (V);
104 double dist = C.dot (C);
108 min_index =
static_cast<int> (j);
111 if (min_dist > max_distance)
116 corr.
distance = nn_dists[min_index];
117 correspondences[nr_valid_correspondences++] = corr;
125 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
127 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
130 min_dist = std::numeric_limits<double>::max ();
133 for (
size_t j = 0; j < nn_indices.size (); j++)
138 input_->points[*idx_i],
143 pt.x = target_->points[nn_indices[j]].x - pt_src.x;
144 pt.y = target_->points[nn_indices[j]].y - pt_src.y;
145 pt.z = target_->points[nn_indices[j]].z - pt_src.z;
147 const NormalT &normal = source_normals_->points[*idx_i];
148 Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
149 Eigen::Vector3d V (pt.x, pt.y, pt.z);
150 Eigen::Vector3d C = N.cross (V);
153 double dist = C.dot (C);
157 min_index =
static_cast<int> (j);
160 if (min_dist > max_distance)
165 corr.
distance = nn_dists[min_index];
166 correspondences[nr_valid_correspondences++] = corr;
169 correspondences.resize (nr_valid_correspondences);
174 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
void
185 if (!initComputeReciprocal ())
188 correspondences.resize (indices_->size ());
190 std::vector<int> nn_indices (k_);
191 std::vector<float> nn_dists (k_);
192 std::vector<int> index_reciprocal (1);
193 std::vector<float> distance_reciprocal (1);
195 double min_dist = std::numeric_limits<double>::max ();
199 unsigned int nr_valid_correspondences = 0;
204 if (isSamePointType<PointSource, PointTarget> ())
208 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
210 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
213 min_dist = std::numeric_limits<double>::max ();
216 for (
size_t j = 0; j < nn_indices.size (); j++)
220 pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x;
221 pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y;
222 pt.z = target_->points[nn_indices[j]].z - input_->points[*idx_i].z;
224 const NormalT &normal = source_normals_->points[*idx_i];
225 Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
226 Eigen::Vector3d V (pt.x, pt.y, pt.z);
227 Eigen::Vector3d C = N.cross (V);
230 double dist = C.dot (C);
234 min_index =
static_cast<int> (j);
237 if (min_dist > max_distance)
241 target_idx = nn_indices[min_index];
242 tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
244 if (*idx_i != index_reciprocal[0])
250 corr.
distance = nn_dists[min_index];
251 correspondences[nr_valid_correspondences++] = corr;
259 for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
261 tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
264 min_dist = std::numeric_limits<double>::max ();
267 for (
size_t j = 0; j < nn_indices.size (); j++)
272 input_->points[*idx_i],
277 pt.x = target_->points[nn_indices[j]].x - pt_src.x;
278 pt.y = target_->points[nn_indices[j]].y - pt_src.y;
279 pt.z = target_->points[nn_indices[j]].z - pt_src.z;
281 const NormalT &normal = source_normals_->points[*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_->points[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);
315 #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Abstract CorrespondenceEstimationBase class.
void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())
Determine the correspondences between input and target cloud.
virtual void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())
Determine the reciprocal correspondences between input and target cloud.
A point structure representing normal coordinates and the surface curvature estimate.
int index_match
Index of the matching (target) point.
int index_query
Index of the query (source) point.
Correspondence represents a match between two entities (e.g., points, descriptors, etc).
bool initCompute()
Internal computation initalization.
Helper functor structure for concatenate.