153 correspondences.resize(indices_->size());
156 std::vector<float> distance(1);
157 std::vector<pcl::Correspondences> per_thread_correspondences(num_threads_);
158 for (
auto& corrs : per_thread_correspondences) {
159 corrs.reserve(2 * indices_->size() / num_threads_);
161 double max_dist_sqr = max_distance * max_distance;
163#pragma omp parallel for default(none) \
164 shared(max_dist_sqr, per_thread_correspondences) firstprivate(index, distance) \
165 num_threads(num_threads_)
167 for (
int i = 0; i < static_cast<int>(indices_->size()); i++) {
168 const auto& idx = (*indices_)[i];
172 const auto& pt = detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx);
175 tree_->nearestKSearch(pt, 1, index, distance);
176 if (distance[0] > max_dist_sqr)
185 const int thread_num = omp_get_thread_num();
187 const int thread_num = 0;
190 per_thread_correspondences[thread_num].emplace_back(corr);
193 if (num_threads_ == 1) {
194 correspondences = std::move(per_thread_correspondences.front());
197 const unsigned int nr_correspondences = std::accumulate(
198 per_thread_correspondences.begin(),
199 per_thread_correspondences.end(),
200 static_cast<unsigned int>(0),
201 [](
const auto sum,
const auto& corr) { return sum + corr.size(); });
202 correspondences.resize(nr_correspondences);
205 auto insert_loc = correspondences.begin();
206 for (
const auto& corrs : per_thread_correspondences) {
207 const auto new_insert_loc = std::move(corrs.begin(), corrs.end(), insert_loc);
208 std::inplace_merge(correspondences.begin(),
210 insert_loc + corrs.size(),
211 [](
const auto& lhs,
const auto& rhs) {
212 return lhs.index_query < rhs.index_query;
214 insert_loc = new_insert_loc;
231 if (!initComputeReciprocal())
233 double max_dist_sqr = max_distance * max_distance;
235 correspondences.resize(indices_->size());
237 std::vector<float> distance(1);
239 std::vector<float> distance_reciprocal(1);
240 std::vector<pcl::Correspondences> per_thread_correspondences(num_threads_);
241 for (
auto& corrs : per_thread_correspondences) {
242 corrs.reserve(2 * indices_->size() / num_threads_);
245#pragma omp parallel for default(none) \
246 shared(max_dist_sqr, per_thread_correspondences) \
247 firstprivate(index, distance, index_reciprocal, distance_reciprocal) \
248 num_threads(num_threads_)
250 for (
int i = 0; i < static_cast<int>(indices_->size()); i++) {
251 const auto& idx = (*indices_)[i];
256 const auto& pt_src = detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx);
259 tree_->nearestKSearch(pt_src, 1, index, distance);
260 if (distance[0] > max_dist_sqr)
263 const auto target_idx = index[0];
265 detail::pointCopyOrRef<PointSource, PointTarget>(target_, target_idx);
267 tree_reciprocal_->nearestKSearch(pt_tgt, 1, index_reciprocal, distance_reciprocal);
268 if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0])
277 const int thread_num = omp_get_thread_num();
279 const int thread_num = 0;
282 per_thread_correspondences[thread_num].emplace_back(corr);
285 if (num_threads_ == 1) {
286 correspondences = std::move(per_thread_correspondences.front());
289 const unsigned int nr_correspondences = std::accumulate(
290 per_thread_correspondences.begin(),
291 per_thread_correspondences.end(),
292 static_cast<unsigned int>(0),
293 [](
const auto sum,
const auto& corr) { return sum + corr.size(); });
294 correspondences.resize(nr_correspondences);
297 auto insert_loc = correspondences.begin();
298 for (
const auto& corrs : per_thread_correspondences) {
299 const auto new_insert_loc = std::move(corrs.begin(), corrs.end(), insert_loc);
300 std::inplace_merge(correspondences.begin(),
302 insert_loc + corrs.size(),
303 [](
const auto& lhs,
const auto& rhs) {
304 return lhs.index_query < rhs.index_query;
306 insert_loc = new_insert_loc;