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);
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);