Skip to content

Commit 7f2d8d7

Browse files
mviethmvieth
authored
Move further towards KdTreeNanoflann (#6417)
* Move further towards KdTreeNanoflann * Add/remove includes, suggested by Copilot
1 parent c836340 commit 7f2d8d7

File tree

24 files changed

+87
-83
lines changed

24 files changed

+87
-83
lines changed

‎examples/keypoints/example_sift_keypoint_estimation.cpp‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -67,8 +67,6 @@ main(int, char** argv)
6767
// Estimate the sift interest points using Intensity values from RGB values
6868
pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointWithScale> sift;
6969
pcl::PointCloud<pcl::PointWithScale> result;
70-
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB> ());
71-
sift.setSearchMethod(tree);
7270
sift.setScales(min_scale, n_octaves, n_scales_per_octave);
7371
sift.setMinimumContrast(min_contrast);
7472
sift.setInputCloud(cloud);

‎examples/keypoints/example_sift_normal_keypoint_estimation.cpp‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -72,10 +72,8 @@ main(int, char** argv)
7272
// Estimate the normals of the cloud_xyz
7373
pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
7474
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointNormal>);
75-
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n(new pcl::search::KdTree<pcl::PointXYZ>());
7675

7776
ne.setInputCloud(cloud_xyz);
78-
ne.setSearchMethod(tree_n);
7977
ne.setRadiusSearch(0.2);
8078
ne.compute(*cloud_normals);
8179

@@ -90,8 +88,6 @@ main(int, char** argv)
9088
// Estimate the sift interest points using normals values from xyz as the Intensity variants
9189
pcl::SIFTKeypoint<pcl::PointNormal, pcl::PointWithScale> sift;
9290
pcl::PointCloud<pcl::PointWithScale> result;
93-
pcl::search::KdTree<pcl::PointNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointNormal> ());
94-
sift.setSearchMethod(tree);
9591
sift.setScales(min_scale, n_octaves, n_scales_per_octave);
9692
sift.setMinimumContrast(min_contrast);
9793
sift.setInputCloud(cloud_normals);

‎examples/keypoints/example_sift_z_keypoint_estimation.cpp‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -84,8 +84,6 @@ main(int, char** argv)
8484
// Estimate the sift interest points using z values from xyz as the Intensity variants
8585
pcl::SIFTKeypoint<pcl::PointXYZ, pcl::PointWithScale> sift;
8686
pcl::PointCloud<pcl::PointWithScale> result;
87-
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ> ());
88-
sift.setSearchMethod(tree);
8987
sift.setScales(min_scale, n_octaves, n_scales_per_octave);
9088
sift.setMinimumContrast(min_contrast);
9189
sift.setInputCloud(cloud_xyz);

‎examples/segmentation/example_extract_clusters_normals.cpp‎

Lines changed: 36 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,8 @@
4343
#include <pcl/io/pcd_io.h>
4444
#include <pcl/features/normal_3d.h>
4545
#include <pcl/segmentation/extract_clusters.h>
46+
#define PCL_NO_PRECOMPILE // ConditionalEuclideanClustering is not always instantiated with PointNormal
47+
#include <pcl/segmentation/conditional_euclidean_clustering.h>
4648
#include <pcl/point_types.h>
4749

4850
int
@@ -68,8 +70,6 @@ main (int argc, char **argv)
6870
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
6971
ne.setInputCloud (cloud_ptr);
7072

71-
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_n (new pcl::search::KdTree<pcl::PointXYZ>());
72-
ne.setSearchMethod (tree_n);
7373
ne.setRadiusSearch (0.03);
7474
ne.compute (*cloud_normals);
7575
std::cout << "Estimated the normals" << std::endl;
@@ -106,6 +106,40 @@ main (int argc, char **argv)
106106
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false);
107107
++j;
108108
}
109+
110+
// Alternative: using ConditionalEuclideanClustering instead of extractEuclideanClusters
111+
auto cloud_with_normals = pcl::make_shared<pcl::PointCloud<pcl::PointNormal>>();
112+
pcl::concatenateFields(*cloud_ptr, *cloud_normals, *cloud_with_normals);
113+
pcl::ConditionalEuclideanClustering<pcl::PointNormal> cec;
114+
const double cos_eps_angle = std::cos(eps_angle);
115+
std::function<bool (const pcl::PointNormal&, const pcl::PointNormal&, float)> condition_function =
116+
[cos_eps_angle](const pcl::PointNormal& a, const pcl::PointNormal& b, float /*dist*/)
117+
{ return std::abs(a.getNormalVector3fMap().dot(b.getNormalVector3fMap())) > cos_eps_angle; };
118+
cec.setConditionFunction(condition_function);
119+
cec.setClusterTolerance(tolerance);
120+
cec.setMinClusterSize(min_cluster_size);
121+
cec.setInputCloud(cloud_with_normals);
122+
std::vector<pcl::PointIndices> cluster_indices2;
123+
cec.segment(cluster_indices2);
124+
125+
std::cout << "No of clusters formed are " << cluster_indices2.size () << std::endl;
126+
// Saving the clusters in separate pcd files
127+
j = 0;
128+
for (const auto& cluster : cluster_indices2)
129+
{
130+
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
131+
for (const auto &index : cluster.indices) {
132+
cloud_cluster->push_back((*cloud_ptr)[index]);
133+
}
134+
cloud_cluster->width = cloud_cluster->size ();
135+
cloud_cluster->height = 1;
136+
cloud_cluster->is_dense = true;
109137

138+
std::cout << "PointCloud representing the Cluster using xyzn: " << cloud_cluster->size () << " data " << std::endl;
139+
std::stringstream ss;
140+
ss << "./cloud_cluster2_" << j << ".pcd";
141+
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false);
142+
++j;
143+
}
110144
return (0);
111145
}

‎features/include/pcl/features/impl/cvfh.hpp‎

Lines changed: 2 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@
4444
#include <pcl/features/cvfh.h>
4545
#include <pcl/features/normal_3d.h>
4646
#include <pcl/common/centroid.h>
47-
#include <pcl/search/kdtree.h> // for KdTree
47+
#include <pcl/search/auto.h> // for autoSelectMethod
4848

4949
//////////////////////////////////////////////////////////////////////////////////////////////
5050
template<typename PointInT, typename PointNT, typename PointOutT> void
@@ -234,18 +234,12 @@ pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut
234234
if(normals_filtered_cloud->size() >= min_points_)
235235
{
236236
//recompute normals and use them for clustering
237-
KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false));
238-
normals_tree_filtered->setInputCloud (normals_filtered_cloud);
239-
240-
241237
pcl::NormalEstimation<PointNormal, PointNormal> n3d;
242238
n3d.setRadiusSearch (radius_normals_);
243-
n3d.setSearchMethod (normals_tree_filtered);
244239
n3d.setInputCloud (normals_filtered_cloud);
245240
n3d.compute (*normals_filtered_cloud);
246241

247-
KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false));
248-
normals_tree->setInputCloud (normals_filtered_cloud);
242+
KdTreePtr normals_tree (pcl::search::autoSelectMethod<pcl::PointNormal>(normals_filtered_cloud, false, pcl::search::Purpose::radius_search));
249243

250244
extractEuclideanClustersSmooth (*normals_filtered_cloud,
251245
*normals_filtered_cloud,

‎features/include/pcl/features/impl/flare.hpp‎

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -41,8 +41,7 @@
4141

4242
#include <pcl/features/flare.h>
4343
#include <pcl/common/geometry.h>
44-
#include <pcl/search/kdtree.h> // for KdTree
45-
#include <pcl/search/organized.h> // for OrganizedNeighbor
44+
#include <pcl/search/auto.h> // for autoSelectMethod
4645

4746
//////////////////////////////////////////////////////////////////////////////////////////////
4847
template<typename PointInT, typename PointNT, typename PointOutT, typename SignedDistanceT> bool
@@ -76,10 +75,7 @@ template<typename PointInT, typename PointNT, typename PointOutT, typename Signe
7675
// Check if a space search locator was given for sampled_surface_
7776
if (!sampled_tree_)
7877
{
79-
if (sampled_surface_->isOrganized () && surface_->isOrganized () && input_->isOrganized ())
80-
sampled_tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
81-
else
82-
sampled_tree_.reset (new pcl::search::KdTree<PointInT> (false));
78+
sampled_tree_.reset (pcl::search::autoSelectMethod<PointInT>(sampled_surface_, false, pcl::search::Purpose::radius_search));
8379
}
8480

8581
if (sampled_tree_->getInputCloud () != sampled_surface_) // Make sure the tree searches the sampled surface

‎features/include/pcl/features/impl/our_cvfh.hpp‎

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -47,7 +47,7 @@
4747
#include <pcl/common/io.h> // for copyPointCloud
4848
#include <pcl/common/common.h> // for getMaxDistance
4949
#include <pcl/common/transforms.h>
50-
#include <pcl/search/kdtree.h> // for KdTree
50+
#include <pcl/search/auto.h> // for autoSelectMethod
5151

5252
//////////////////////////////////////////////////////////////////////////////////////////////
5353
template<typename PointInT, typename PointNT, typename PointOutT> void
@@ -603,17 +603,13 @@ pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloud
603603
{
604604
//recompute normals and use them for clustering
605605
{
606-
KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false));
607-
normals_tree_filtered->setInputCloud (normals_filtered_cloud);
608606
pcl::NormalEstimation<PointNormal, PointNormal> n3d;
609607
n3d.setRadiusSearch (radius_normals_);
610-
n3d.setSearchMethod (normals_tree_filtered);
611608
n3d.setInputCloud (normals_filtered_cloud);
612609
n3d.compute (*normals_filtered_cloud);
613610
}
614611

615-
KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false));
616-
normals_tree->setInputCloud (normals_filtered_cloud);
612+
KdTreePtr normals_tree (pcl::search::autoSelectMethod<pcl::PointNormal>(normals_filtered_cloud, false, pcl::search::Purpose::radius_search));
617613

618614
extractEuclideanClustersSmooth (*normals_filtered_cloud, *normals_filtered_cloud, cluster_tolerance_, normals_tree, clusters,
619615
eps_angle_threshold_, static_cast<unsigned int> (min_points_));

‎gpu/examples/segmentation/src/seg.cpp‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,6 @@
44
#include <pcl/filters/extract_indices.h>
55
#include <pcl/filters/voxel_grid.h>
66
#include <pcl/features/normal_3d.h>
7-
#include <pcl/kdtree/kdtree.h>
87
#include <pcl/sample_consensus/method_types.h>
98
#include <pcl/sample_consensus/model_types.h>
109
#include <pcl/segmentation/sac_segmentation.h>

‎gpu/people/tools/people_tracking.cpp‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,6 @@
99
#include <pcl/point_types.h>
1010
#include <pcl/point_cloud.h>
1111
#include <pcl/io/pcd_io.h>
12-
#include <pcl/kdtree/kdtree.h>
1312
#include <pcl/segmentation/extract_clusters.h>
1413
#include <pcl/segmentation/extract_labeled_clusters.h>
1514
#include <pcl/segmentation/seeded_hue_segmentation.h>

‎keypoints/include/pcl/keypoints/impl/sift_keypoint.hpp‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@
4141
#include <pcl/keypoints/sift_keypoint.h>
4242
#include <pcl/common/io.h>
4343
#include <pcl/filters/voxel_grid.h>
44-
#include <pcl/search/kdtree.h>
44+
#include <pcl/search/auto.h> // for autoSelectMethod
4545

4646
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
4747
template <typename PointInT, typename PointOutT> void
@@ -90,7 +90,6 @@ pcl::SIFTKeypoint<PointInT, PointOutT>::initCompute ()
9090
}
9191

9292
this->setKSearch (1);
93-
tree_.reset (new pcl::search::KdTree<PointInT> (true));
9493
return (true);
9594
}
9695

@@ -134,7 +133,7 @@ pcl::SIFTKeypoint<PointInT, PointOutT>::detectKeypoints (PointCloudOut &output)
134133
break;
135134

136135
// Update the KdTree with the downsampled points
137-
tree_->setInputCloud (cloud);
136+
tree_.reset (pcl::search::autoSelectMethod<PointInT>(cloud, true));
138137

139138
// Detect keypoints for the current scale
140139
detectKeypointsForOctave (*cloud, *tree_, scale, nr_scales_per_octave_, output);

0 commit comments

Comments
 (0)