74 correspondences.resize(indices_->size());
77 std::vector<float> nn_dists(k_);
82 unsigned int nr_valid_correspondences = 0;
86 for (
const auto& idx_i : (*indices_)) {
90 tree_->nearestKSearch(
91 detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx_i),
98 double min_dist = std::numeric_limits<double>::max();
101 for (std::size_t j = 0; j < nn_indices.size(); j++) {
104 pt.x = (*target_)[nn_indices[j]].x - (*input_)[idx_i].x;
105 pt.y = (*target_)[nn_indices[j]].y - (*input_)[idx_i].y;
106 pt.z = (*target_)[nn_indices[j]].z - (*input_)[idx_i].z;
108 const NormalT& normal = (*source_normals_)[idx_i];
109 Eigen::Vector3d N(normal.normal_x, normal.normal_y, normal.normal_z);
110 Eigen::Vector3d V(pt.x, pt.y, pt.z);
111 Eigen::Vector3d C = N.cross(V);
114 double dist = C.dot(C);
115 if (dist < min_dist) {
117 min_index =
static_cast<int>(j);
120 if (min_dist > max_distance)
125 corr.
distance = nn_dists[min_index];
126 correspondences[nr_valid_correspondences++] = corr;
128 correspondences.resize(nr_valid_correspondences);
143 if (!initComputeReciprocal())
146 correspondences.resize(indices_->size());
149 std::vector<float> nn_dists(k_);
151 std::vector<float> distance_reciprocal(1);
156 unsigned int nr_valid_correspondences = 0;
161 for (
const auto& idx_i : (*indices_)) {
165 tree_->nearestKSearch(
166 detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx_i),
173 double min_dist = std::numeric_limits<double>::max();
176 for (std::size_t j = 0; j < nn_indices.size(); j++) {
179 pt.x = (*target_)[nn_indices[j]].x - (*input_)[idx_i].x;
180 pt.y = (*target_)[nn_indices[j]].y - (*input_)[idx_i].y;
181 pt.z = (*target_)[nn_indices[j]].z - (*input_)[idx_i].z;
183 const NormalT& normal = (*source_normals_)[idx_i];
184 Eigen::Vector3d N(normal.normal_x, normal.normal_y, normal.normal_z);
185 Eigen::Vector3d V(pt.x, pt.y, pt.z);
186 Eigen::Vector3d C = N.cross(V);
189 double dist = C.dot(C);
190 if (dist < min_dist) {
192 min_index =
static_cast<int>(j);
195 if (min_dist > max_distance)
199 target_idx = nn_indices[min_index];
200 tree_reciprocal_->nearestKSearch(
201 detail::pointCopyOrRef<PointSource, PointTarget>(target_, target_idx),
204 distance_reciprocal);
206 if (idx_i != index_reciprocal[0])
212 corr.
distance = nn_dists[min_index];
213 correspondences[nr_valid_correspondences++] = corr;
215 correspondences.resize(nr_valid_correspondences);