138 tree_->nearestKSearch((*input_)[idx], 1, index, distance);
157 tree_->nearestKSearch(pt, 1, index, distance);
182 if (!initComputeReciprocal())
184 double max_dist_sqr = max_distance * max_distance;
186 correspondences.resize(indices_->size());
188 std::vector<float> distance(1);
190 std::vector<float> distance_reciprocal(1);
192 unsigned int nr_valid_correspondences = 0;
200 for (
const auto& idx : (*indices_)) {
201 tree_->nearestKSearch((*input_)[idx], 1, index, distance);
202 if (distance[0] > max_dist_sqr)
205 target_idx = index[0];
207 tree_reciprocal_->nearestKSearch(
208 (*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
209 if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0])
215 correspondences[nr_valid_correspondences++] = corr;
223 for (
const auto& idx : (*indices_)) {
228 tree_->nearestKSearch(pt_src, 1, index, distance);
229 if (distance[0] > max_dist_sqr)
232 target_idx = index[0];
236 copyPoint((*target_)[target_idx], pt_tgt);
238 tree_reciprocal_->nearestKSearch(
239 pt_tgt, 1, index_reciprocal, distance_reciprocal);
240 if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0])
246 correspondences[nr_valid_correspondences++] = corr;
249 correspondences.resize(nr_valid_correspondences);