Skip to content

Commit 279d91f

Browse files
authored
Move further towards KdTreeNanoflann (#6413)
* More further towards KdTreeNanoflann Extend `autoSelectMethod` to types that do not have xyz-coordinates (i.e. features types) * Changes per review * Add octree to autoSelectMethod
1 parent aaecae7 commit 279d91f

15 files changed

Lines changed: 101 additions & 47 deletions

File tree

common/include/pcl/impl/point_types.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -173,6 +173,7 @@ namespace pcl
173173
template<> struct descriptorSize<GASDSignature7992> { static constexpr const int value = 7992; };
174174
template<> struct descriptorSize<GFPFHSignature16> { static constexpr const int value = 16; };
175175
template<> struct descriptorSize<Narf36> { static constexpr const int value = 36; };
176+
template<> struct descriptorSize<NormalBasedSignature12> { static constexpr const int value = 12; };
176177
template<int N> struct descriptorSize<Histogram<N>> { static constexpr const int value = N; };
177178

178179

common/include/pcl/point_representation.h

Lines changed: 32 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -204,6 +204,8 @@ namespace pcl
204204
using Ptr = shared_ptr<DefaultPointRepresentation<PointDefault> >;
205205
using ConstPtr = shared_ptr<const DefaultPointRepresentation<PointDefault> >;
206206

207+
static constexpr const std::int32_t NR_DIMS = std::min<std::int32_t>(sizeof (PointDefault) / sizeof (float), 3);
208+
207209
DefaultPointRepresentation ()
208210
{
209211
// If point type is unknown, assume it's a struct/array of floats, and compute the number of dimensions
@@ -312,6 +314,8 @@ namespace pcl
312314
using ConstPtr = shared_ptr<const DefaultFeatureRepresentation<PointDefault>>;
313315
using FieldList = typename pcl::traits::fieldList<PointDefault>::type;
314316

317+
static constexpr const std::int32_t NR_DIMS = pcl::detail::traits::descriptorSize_v<PointDefault>;
318+
315319
DefaultFeatureRepresentation ()
316320
{
317321
nr_dimensions_ = 0; // zero-out the nr_dimensions_ before it gets incremented
@@ -336,9 +340,11 @@ namespace pcl
336340
class DefaultPointRepresentation <PointXYZ> : public PointRepresentation <PointXYZ>
337341
{
338342
public:
343+
static constexpr const std::int32_t NR_DIMS = 3;
344+
339345
DefaultPointRepresentation ()
340346
{
341-
nr_dimensions_ = 3;
347+
nr_dimensions_ = NR_DIMS;
342348
trivial_ = true;
343349
}
344350

@@ -356,9 +362,11 @@ namespace pcl
356362
class DefaultPointRepresentation <PointXYZI> : public PointRepresentation <PointXYZI>
357363
{
358364
public:
365+
static constexpr const std::int32_t NR_DIMS = 3;
366+
359367
DefaultPointRepresentation ()
360368
{
361-
nr_dimensions_ = 3;
369+
nr_dimensions_ = NR_DIMS;
362370
trivial_ = true;
363371
}
364372

@@ -377,9 +385,11 @@ namespace pcl
377385
class DefaultPointRepresentation <PointNormal> : public PointRepresentation <PointNormal>
378386
{
379387
public:
388+
static constexpr const std::int32_t NR_DIMS = 3;
389+
380390
DefaultPointRepresentation ()
381391
{
382-
nr_dimensions_ = 3;
392+
nr_dimensions_ = NR_DIMS;
383393
trivial_ = true;
384394
}
385395

@@ -404,12 +414,14 @@ namespace pcl
404414

405415
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
406416
template <>
407-
class DefaultPointRepresentation <PPFSignature> : public DefaultFeatureRepresentation <PPFSignature>
417+
class DefaultPointRepresentation <PPFSignature> : public PointRepresentation <PPFSignature>
408418
{
409419
public:
420+
static constexpr const std::int32_t NR_DIMS = 4;
421+
410422
DefaultPointRepresentation ()
411423
{
412-
nr_dimensions_ = 4;
424+
nr_dimensions_ = NR_DIMS;
413425
trivial_ = true;
414426
}
415427

@@ -453,9 +465,11 @@ namespace pcl
453465
class DefaultPointRepresentation <Narf36> : public PointRepresentation <Narf36>
454466
{
455467
public:
468+
static constexpr const std::int32_t NR_DIMS = 36;
469+
456470
DefaultPointRepresentation ()
457471
{
458-
nr_dimensions_ = 36;
472+
nr_dimensions_ = NR_DIMS;
459473
trivial_=false;
460474
}
461475

@@ -476,9 +490,11 @@ namespace pcl
476490
class DefaultPointRepresentation<ShapeContext1980> : public PointRepresentation<ShapeContext1980>
477491
{
478492
public:
493+
static constexpr const std::int32_t NR_DIMS = 1980;
494+
479495
DefaultPointRepresentation ()
480496
{
481-
nr_dimensions_ = 1980;
497+
nr_dimensions_ = NR_DIMS;
482498
}
483499

484500
void
@@ -494,9 +510,11 @@ namespace pcl
494510
class DefaultPointRepresentation<UniqueShapeContext1960> : public PointRepresentation<UniqueShapeContext1960>
495511
{
496512
public:
513+
static constexpr const std::int32_t NR_DIMS = 1960;
514+
497515
DefaultPointRepresentation ()
498516
{
499-
nr_dimensions_ = 1960;
517+
nr_dimensions_ = NR_DIMS;
500518
}
501519

502520
void
@@ -512,9 +530,11 @@ namespace pcl
512530
class DefaultPointRepresentation<SHOT352> : public PointRepresentation<SHOT352>
513531
{
514532
public:
533+
static constexpr const std::int32_t NR_DIMS = 352;
534+
515535
DefaultPointRepresentation ()
516536
{
517-
nr_dimensions_ = 352;
537+
nr_dimensions_ = NR_DIMS;
518538
}
519539

520540
void
@@ -530,9 +550,11 @@ namespace pcl
530550
class DefaultPointRepresentation<SHOT1344> : public PointRepresentation<SHOT1344>
531551
{
532552
public:
553+
static constexpr const std::int32_t NR_DIMS = 1344;
554+
533555
DefaultPointRepresentation ()
534556
{
535-
nr_dimensions_ = 1344;
557+
nr_dimensions_ = NR_DIMS;
536558
}
537559

538560
void

people/include/pcl/people/impl/ground_based_people_detection_app.hpp

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -375,13 +375,10 @@ pcl::people::GroundBasedPeopleDetectionApp<PointT>::compute (std::vector<pcl::pe
375375

376376
// Euclidean Clustering:
377377
std::vector<pcl::PointIndices> cluster_indices;
378-
typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
379-
tree->setInputCloud(no_ground_cloud_);
380378
pcl::EuclideanClusterExtraction<PointT> ec;
381379
ec.setClusterTolerance(2 * voxel_size_);
382380
ec.setMinClusterSize(min_points_);
383381
ec.setMaxClusterSize(max_points_);
384-
ec.setSearchMethod(tree);
385382
ec.setInputCloud(no_ground_cloud_);
386383
ec.extract(cluster_indices);
387384

registration/include/pcl/registration/ia_fpcs.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -86,7 +86,7 @@ class FPCSInitialAlignment : public Registration<PointSource, PointTarget, Scala
8686
using ConstPtr =
8787
shared_ptr<const FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>>;
8888

89-
using KdTreeReciprocal = pcl::search::KdTree<PointSource>;
89+
using KdTreeReciprocal = pcl::search::Search<PointSource>;
9090
using KdTreeReciprocalPtr = typename KdTreeReciprocal::Ptr;
9191

9292
using PointCloudTarget = pcl::PointCloud<PointTarget>;

registration/include/pcl/registration/ia_ransac.h

Lines changed: 2 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -137,13 +137,10 @@ class SampleConsensusInitialAlignment : public Registration<PointSource, PointTa
137137

138138
using ErrorFunctorPtr = typename ErrorFunctor::Ptr;
139139

140-
using FeatureKdTreePtr = typename KdTreeFLANN<FeatureT>::Ptr;
140+
using FeatureKdTreePtr = typename pcl::search::Search<FeatureT>::Ptr;
141141
/** \brief Constructor. */
142142
SampleConsensusInitialAlignment()
143-
: input_features_()
144-
, target_features_()
145-
, feature_tree_(new pcl::KdTreeFLANN<FeatureT>)
146-
, error_functor_()
143+
: input_features_(), target_features_(), error_functor_()
147144
{
148145
reg_name_ = "SampleConsensusInitialAlignment";
149146
max_iterations_ = 1000;

registration/include/pcl/registration/impl/ia_fpcs.hpp

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -671,8 +671,11 @@ pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scala
671671
}
672672

673673
// initialize new kd tree of intermediate points from first point pair correspondences
674-
KdTreeReciprocalPtr tree_e(new KdTreeReciprocal);
675-
tree_e->setInputCloud(cloud_e);
674+
KdTreeReciprocalPtr tree_e(pcl::search::autoSelectMethod<PointSource>(
675+
cloud_e,
676+
true,
677+
pcl::search::Purpose::radius_search)); // maybe check again if results do not have
678+
// to be sorted
676679

677680
pcl::Indices ids;
678681
std::vector<float> dists_sqr;

registration/include/pcl/registration/impl/ia_ransac.hpp

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,7 @@
4242
#define IA_RANSAC_HPP_
4343

4444
#include <pcl/common/distances.h>
45+
#include <pcl/search/auto.h> // for autoSelectMethod
4546

4647
namespace pcl {
4748

@@ -71,7 +72,8 @@ SampleConsensusInitialAlignment<PointSource, PointTarget, FeatureT>::setTargetFe
7172
return;
7273
}
7374
target_features_ = features;
74-
feature_tree_->setInputCloud(target_features_);
75+
feature_tree_.reset(pcl::search::autoSelectMethod<FeatureT>(
76+
target_features_, false, pcl::search::Purpose::many_knn_search));
7577
}
7678

7779
template <typename PointSource, typename PointTarget, typename FeatureT>

registration/include/pcl/registration/impl/sample_consensus_prerejective.hpp

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,8 @@
4141
#ifndef PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
4242
#define PCL_REGISTRATION_SAMPLE_CONSENSUS_PREREJECTIVE_HPP_
4343

44+
#include <pcl/search/auto.h> // for autoSelectMethod
45+
4446
namespace pcl {
4547

4648
template <typename PointSource, typename PointTarget, typename FeatureT>
@@ -69,7 +71,8 @@ SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>::setTargetFeatur
6971
return;
7072
}
7173
target_features_ = features;
72-
feature_tree_->setInputCloud(target_features_);
74+
feature_tree_.reset(pcl::search::autoSelectMethod<FeatureT>(
75+
target_features_, false, pcl::search::Purpose::many_knn_search));
7376
}
7477

7578
template <typename PointSource, typename PointTarget, typename FeatureT>

registration/include/pcl/registration/sample_consensus_prerejective.h

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -109,7 +109,7 @@ class SampleConsensusPrerejective : public Registration<PointSource, PointTarget
109109
using ConstPtr =
110110
shared_ptr<const SampleConsensusPrerejective<PointSource, PointTarget, FeatureT>>;
111111

112-
using FeatureKdTreePtr = typename KdTreeFLANN<FeatureT>::Ptr;
112+
using FeatureKdTreePtr = typename pcl::search::Search<FeatureT>::Ptr;
113113

114114
using CorrespondenceRejectorPoly =
115115
pcl::registration::CorrespondenceRejectorPoly<PointSource, PointTarget>;
@@ -121,7 +121,6 @@ class SampleConsensusPrerejective : public Registration<PointSource, PointTarget
121121
SampleConsensusPrerejective()
122122
: input_features_()
123123
, target_features_()
124-
, feature_tree_(new pcl::KdTreeFLANN<FeatureT>)
125124
, correspondence_rejector_poly_(new CorrespondenceRejectorPoly)
126125
{
127126
reg_name_ = "SampleConsensusPrerejective";

search/include/pcl/search/impl/auto.hpp

Lines changed: 32 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -15,35 +15,58 @@
1515
#include <pcl/search/brute_force.h>
1616
#include <pcl/search/kdtree.h>
1717
#include <pcl/search/kdtree_nanoflann.h>
18+
#include <pcl/search/octree.h>
1819
#include <pcl/search/organized.h>
1920

2021
template<typename PointT>
2122
pcl::search::Search<PointT> * pcl::search::autoSelectMethod(const typename pcl::PointCloud<PointT>::ConstPtr& cloud, const pcl::IndicesConstPtr& indices, bool sorted_results, pcl::search::Purpose purpose) {
2223
pcl::search::Search<PointT> * searcher = nullptr;
23-
if (cloud->isOrganized ()) {
24-
searcher = new pcl::search::OrganizedNeighbor<PointT> (sorted_results);
25-
if(searcher->setInputCloud (cloud, indices)) { // may return false if OrganizedNeighbor cannot work with the cloud, then use another search method instead
26-
return searcher;
24+
if constexpr (pcl::traits::has_xyz_v<PointT>) {
25+
if (cloud->isOrganized ()) {
26+
searcher = new pcl::search::OrganizedNeighbor<PointT> (sorted_results);
27+
if(searcher->setInputCloud (cloud, indices)) { // may return false if OrganizedNeighbor cannot work with the cloud, then use another search method instead
28+
return searcher;
29+
}
30+
delete searcher;
2731
}
2832
}
33+
2934
#if PCL_HAS_NANOFLANN
30-
searcher = new pcl::search::KdTreeNanoflann<PointT> (sorted_results, (purpose == pcl::search::Purpose::one_knn_search ? 10 : 20));
35+
// we get the number of search dimensions as a compile-time-constant via NR_DIMS. NR_DIMS may be -1 if it is not possible to determine the dimensions at compile-time (only at run-time), however then searching may be slower. If NR_DIMS is not -1, it must be the same as the return value of getNumberOfDimensions().
36+
searcher = new pcl::search::KdTreeNanoflann<PointT, pcl::DefaultPointRepresentation<PointT>::NR_DIMS> (sorted_results, (purpose == pcl::search::Purpose::one_knn_search ? 10 : 20));
3137
if(searcher->setInputCloud (cloud, indices)) {
3238
return searcher;
3339
}
40+
delete searcher;
3441
#else
3542
pcl::utils::ignore(purpose);
3643
#endif
44+
3745
#if PCL_HAS_FLANN
3846
searcher = new pcl::search::KdTree<PointT> (sorted_results);
3947
if(searcher->setInputCloud (cloud, indices)) {
4048
return searcher;
4149
}
50+
delete searcher;
4251
#endif
43-
// If nothing else works, use brute force method
44-
searcher = new pcl::search::BruteForce<PointT> (sorted_results);
45-
searcher->setInputCloud (cloud, indices);
46-
return searcher;
52+
53+
if constexpr (pcl::traits::has_xyz_v<PointT>) {
54+
searcher = new pcl::search::Octree<PointT> (0.01); // TODO a better heuristic to choose octree resolution?
55+
searcher->setSortedResults (sorted_results);
56+
if(searcher->setInputCloud (cloud, indices)) {
57+
return searcher;
58+
}
59+
delete searcher;
60+
}
61+
62+
// If nothing else works, and the point type has xyz coordinates, use brute force method
63+
if constexpr (pcl::traits::has_xyz_v<PointT>) {
64+
searcher = new pcl::search::BruteForce<PointT> (sorted_results);
65+
searcher->setInputCloud (cloud, indices);
66+
return searcher;
67+
}
68+
PCL_ERROR("[pcl::search::autoSelectMethod] No suitable method found. Make sure you have nanoflann and/or FLANN installed.\n");
69+
return nullptr;
4770
}
4871

4972
#define PCL_INSTANTIATE_AutoSelectMethod(T) template PCL_EXPORTS pcl::search::Search<T> * pcl::search::autoSelectMethod<T>(const typename pcl::PointCloud<T>::ConstPtr& cloud, const pcl::IndicesConstPtr& indices, bool sorted_results, pcl::search::Purpose purpose);

0 commit comments

Comments
 (0)