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>
48 namespace registration {
50 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
55 if (!source_normals_) {
56 PCL_WARN(
"[pcl::registration::%s::initCompute] Datasets containing normals for "
57 "source have not been given!\n",
58 getClassName().c_str());
66 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
74 correspondences.resize(indices_->size());
77 std::vector<float> nn_dists(k_);
82 unsigned int nr_valid_correspondences = 0;
87 if (isSamePointType<PointSource, PointTarget>()) {
90 for (
const auto& idx_i : (*indices_)) {
91 tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
95 double min_dist = std::numeric_limits<double>::max();
98 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);
112 if (dist < min_dist) {
114 min_index =
static_cast<int>(j);
117 if (min_dist > max_distance)
122 corr.
distance = nn_dists[min_index];
123 correspondences[nr_valid_correspondences++] = corr;
130 for (
const auto& idx_i : (*indices_)) {
131 tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
135 double min_dist = std::numeric_limits<double>::max();
138 for (std::size_t j = 0; j < nn_indices.size(); j++) {
146 pt.x = (*target_)[nn_indices[j]].x - pt_src.x;
147 pt.y = (*target_)[nn_indices[j]].y - pt_src.y;
148 pt.z = (*target_)[nn_indices[j]].z - pt_src.z;
150 const NormalT& normal = (*source_normals_)[idx_i];
151 Eigen::Vector3d N(normal.normal_x, normal.normal_y, normal.normal_z);
152 Eigen::Vector3d V(pt.x, pt.y, pt.z);
153 Eigen::Vector3d C = N.cross(V);
156 double dist = C.dot(C);
157 if (dist < min_dist) {
159 min_index =
static_cast<int>(j);
162 if (min_dist > max_distance)
167 corr.
distance = nn_dists[min_index];
168 correspondences[nr_valid_correspondences++] = corr;
171 correspondences.resize(nr_valid_correspondences);
175 template <
typename Po
intSource,
typename Po
intTarget,
typename NormalT,
typename Scalar>
186 if (!initComputeReciprocal())
189 correspondences.resize(indices_->size());
192 std::vector<float> nn_dists(k_);
194 std::vector<float> distance_reciprocal(1);
199 unsigned int nr_valid_correspondences = 0;
205 if (isSamePointType<PointSource, PointTarget>()) {
208 for (
const auto& idx_i : (*indices_)) {
209 tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
213 double min_dist = std::numeric_limits<double>::max();
216 for (std::size_t j = 0; j < nn_indices.size(); j++) {
219 pt.x = (*target_)[nn_indices[j]].x - (*input_)[idx_i].x;
220 pt.y = (*target_)[nn_indices[j]].y - (*input_)[idx_i].y;
221 pt.z = (*target_)[nn_indices[j]].z - (*input_)[idx_i].z;
223 const NormalT& normal = (*source_normals_)[idx_i];
224 Eigen::Vector3d N(normal.normal_x, normal.normal_y, normal.normal_z);
225 Eigen::Vector3d V(pt.x, pt.y, pt.z);
226 Eigen::Vector3d C = N.cross(V);
229 double dist = C.dot(C);
230 if (dist < min_dist) {
232 min_index =
static_cast<int>(j);
235 if (min_dist > max_distance)
239 target_idx = nn_indices[min_index];
240 tree_reciprocal_->nearestKSearch(
241 (*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
243 if (idx_i != index_reciprocal[0])
249 corr.
distance = nn_dists[min_index];
250 correspondences[nr_valid_correspondences++] = corr;
257 for (
const auto& idx_i : (*indices_)) {
258 tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
262 double min_dist = std::numeric_limits<double>::max();
265 for (std::size_t j = 0; j < nn_indices.size(); j++) {
273 pt.x = (*target_)[nn_indices[j]].x - pt_src.x;
274 pt.y = (*target_)[nn_indices[j]].y - pt_src.y;
275 pt.z = (*target_)[nn_indices[j]].z - pt_src.z;
277 const NormalT& normal = (*source_normals_)[idx_i];
278 Eigen::Vector3d N(normal.normal_x, normal.normal_y, normal.normal_z);
279 Eigen::Vector3d V(pt.x, pt.y, pt.z);
280 Eigen::Vector3d C = N.cross(V);
283 double dist = C.dot(C);
284 if (dist < min_dist) {
286 min_index =
static_cast<int>(j);
289 if (min_dist > max_distance)
293 target_idx = nn_indices[min_index];
294 tree_reciprocal_->nearestKSearch(
295 (*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
297 if (idx_i != index_reciprocal[0])
303 corr.
distance = nn_dists[min_index];
304 correspondences[nr_valid_correspondences++] = corr;
307 correspondences.resize(nr_valid_correspondences);
Abstract CorrespondenceEstimationBase class.
void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) override
Determine the reciprocal correspondences between input and target cloud.
void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) override
Determine the correspondences between input and target cloud.
bool initCompute()
Internal computation initialization.
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Correspondence represents a match between two entities (e.g., points, descriptors,...
index_t index_query
Index of the query (source) point.
index_t index_match
Index of the matching (target) point.
A point structure representing normal coordinates and the surface curvature estimate.