74 correspondences.resize(indices_->size());
77 std::vector<float> nn_dists(k_);
82 unsigned int nr_valid_correspondences = 0;
85 for (
const auto& idx_i : (*indices_)) {
86 const auto& pt = detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx_i);
87 tree_->nearestKSearch(pt, k_, nn_indices, nn_dists);
91 float min_dist = std::numeric_limits<float>::max();
94 for (std::size_t j = 0; j < nn_indices.size(); j++) {
95 float cos_angle = (*source_normals_)[idx_i].normal_x *
96 (*target_normals_)[nn_indices[j]].normal_x +
97 (*source_normals_)[idx_i].normal_y *
98 (*target_normals_)[nn_indices[j]].normal_y +
99 (*source_normals_)[idx_i].normal_z *
100 (*target_normals_)[nn_indices[j]].normal_z;
101 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
103 if (dist < min_dist) {
105 min_index =
static_cast<int>(j);
108 if (min_dist > max_distance)
113 corr.
distance = nn_dists[min_index];
114 correspondences[nr_valid_correspondences++] = corr;
116 correspondences.resize(nr_valid_correspondences);
130 if (!initComputeReciprocal())
133 correspondences.resize(indices_->size());
136 std::vector<float> nn_dists(k_);
138 std::vector<float> distance_reciprocal(1);
143 unsigned int nr_valid_correspondences = 0;
147 for (
const auto& idx_i : (*indices_)) {
151 tree_->nearestKSearch(
152 detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx_i),
159 float min_dist = std::numeric_limits<float>::max();
162 for (std::size_t j = 0; j < nn_indices.size(); j++) {
163 float cos_angle = (*source_normals_)[idx_i].normal_x *
164 (*target_normals_)[nn_indices[j]].normal_x +
165 (*source_normals_)[idx_i].normal_y *
166 (*target_normals_)[nn_indices[j]].normal_y +
167 (*source_normals_)[idx_i].normal_z *
168 (*target_normals_)[nn_indices[j]].normal_z;
169 float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
171 if (dist < min_dist) {
173 min_index =
static_cast<int>(j);
176 if (min_dist > max_distance)
180 target_idx = nn_indices[min_index];
181 tree_reciprocal_->nearestKSearch(
182 detail::pointCopyOrRef<PointSource, PointTarget>(target_, target_idx),
185 distance_reciprocal);
187 if (idx_i != index_reciprocal[0])
192 corr.
distance = nn_dists[min_index];
193 correspondences[nr_valid_correspondences++] = corr;
195 correspondences.resize(nr_valid_correspondences);