Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
conditional_euclidean_clustering.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the copyright holder(s) nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 */
36
37#ifndef PCL_SEGMENTATION_IMPL_CONDITIONAL_EUCLIDEAN_CLUSTERING_HPP_
38#define PCL_SEGMENTATION_IMPL_CONDITIONAL_EUCLIDEAN_CLUSTERING_HPP_
39
40#include <pcl/segmentation/conditional_euclidean_clustering.h>
41#include <pcl/search/organized.h> // for OrganizedNeighbor
42#include <pcl/search/kdtree.h> // for KdTree
43
44template<typename PointT> void
46{
47 // Prepare output (going to use push_back)
48 clusters.clear ();
49 if (extract_removed_clusters_)
50 {
51 small_clusters_->clear ();
52 large_clusters_->clear ();
53 }
54
55 // Validity checks
56 if (!initCompute () || input_->points.empty () || indices_->empty () || !condition_function_)
57 return;
58
59 // Initialize the search class
60 if (!searcher_)
61 {
62 if (input_->isOrganized ())
63 searcher_.reset (new pcl::search::OrganizedNeighbor<PointT> (false)); // not requiring sorted results is much faster
64 else
65 searcher_.reset (new pcl::search::KdTree<PointT> (false)); // not requiring sorted results is much faster
66 }
67 searcher_->setInputCloud (input_, indices_);
68 // If searcher_ gives sorted results, we can skip the first one because it is the query point itself
69 const int nn_start_idx = searcher_->getSortedResults () ? 1 : 0;
70
71 // Temp variables used by search class
72 Indices nn_indices;
73 std::vector<float> nn_distances;
74
75 // Create a bool vector of processed point indices, and initialize it to false
76 // Need to have it contain all possible points because radius search can not return indices into indices
77 std::vector<bool> processed (input_->size (), false);
78
79 // Process all points indexed by indices_
80 for (const auto& iindex : (*indices_)) // iindex = input index
81 {
82 // Has this point been processed before?
83 if (iindex == UNAVAILABLE || processed[iindex])
84 continue;
85
86 // Set up a new growing cluster
87 Indices current_cluster;
88 int cii = 0; // cii = cluster indices iterator
89
90 // Add the point to the cluster
91 current_cluster.push_back (iindex);
92 processed[iindex] = true;
93
94 // Process the current cluster (it can be growing in size as it is being processed)
95 while (cii < static_cast<int> (current_cluster.size ()))
96 {
97 // Search for neighbors around the current seed point of the current cluster
98 if (searcher_->radiusSearch ((*input_)[current_cluster[cii]], cluster_tolerance_, nn_indices, nn_distances) < 1)
99 {
100 cii++;
101 continue;
102 }
103
104 // Process the neighbors
105 for (int nii = nn_start_idx; nii < static_cast<int> (nn_indices.size ()); ++nii) // nii = neighbor indices iterator
106 {
107 // Has this point been processed before?
108 if (nn_indices[nii] == UNAVAILABLE || processed[nn_indices[nii]])
109 continue;
110
111 // Validate if condition holds
112 if (condition_function_ ((*input_)[current_cluster[cii]], (*input_)[nn_indices[nii]], nn_distances[nii]))
113 {
114 // Add the point to the cluster
115 current_cluster.push_back (nn_indices[nii]);
116 processed[nn_indices[nii]] = true;
117 }
118 }
119 cii++;
120 }
121
122 // If extracting removed clusters, all clusters need to be saved, otherwise only the ones within the given cluster size range
123 if (extract_removed_clusters_ ||
124 (static_cast<int> (current_cluster.size ()) >= min_cluster_size_ &&
125 static_cast<int> (current_cluster.size ()) <= max_cluster_size_))
126 {
128 pi.header = input_->header;
129 pi.indices.resize (current_cluster.size ());
130 for (int ii = 0; ii < static_cast<int> (current_cluster.size ()); ++ii) // ii = indices iterator
131 pi.indices[ii] = current_cluster[ii];
132
133 if (extract_removed_clusters_ && static_cast<int> (current_cluster.size ()) < min_cluster_size_)
134 small_clusters_->push_back (pi);
135 else if (extract_removed_clusters_ && static_cast<int> (current_cluster.size ()) > max_cluster_size_)
136 large_clusters_->push_back (pi);
137 else
138 clusters.push_back (pi);
139 }
140 }
141
142 deinitCompute ();
143}
144
145#define PCL_INSTANTIATE_ConditionalEuclideanClustering(T) template class PCL_EXPORTS pcl::ConditionalEuclideanClustering<T>;
146
147#endif // PCL_SEGMENTATION_IMPL_CONDITIONAL_EUCLIDEAN_CLUSTERING_HPP_
148
void segment(IndicesClusters &clusters)
Segment the input into separate clusters.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition kdtree.h:62
OrganizedNeighbor is a class for optimized nearest neighbor search in organized projectable point clo...
Definition organized.h:66
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
std::vector< pcl::PointIndices > IndicesClusters
::pcl::PCLHeader header