Point Cloud Library (PCL) 1.13.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> ());
64 else
65 searcher_.reset (new pcl::search::KdTree<PointT> ());
66 }
67 searcher_->setInputCloud (input_, indices_);
68
69 // Temp variables used by search class
70 Indices nn_indices;
71 std::vector<float> nn_distances;
72
73 // Create a bool vector of processed point indices, and initialize it to false
74 // Need to have it contain all possible points because radius search can not return indices into indices
75 std::vector<bool> processed (input_->size (), false);
76
77 // Process all points indexed by indices_
78 for (const auto& iindex : (*indices_)) // iindex = input index
79 {
80 // Has this point been processed before?
81 if (iindex == UNAVAILABLE || processed[iindex])
82 continue;
83
84 // Set up a new growing cluster
85 Indices current_cluster;
86 int cii = 0; // cii = cluster indices iterator
87
88 // Add the point to the cluster
89 current_cluster.push_back (iindex);
90 processed[iindex] = true;
91
92 // Process the current cluster (it can be growing in size as it is being processed)
93 while (cii < static_cast<int> (current_cluster.size ()))
94 {
95 // Search for neighbors around the current seed point of the current cluster
96 if (searcher_->radiusSearch ((*input_)[current_cluster[cii]], cluster_tolerance_, nn_indices, nn_distances) < 1)
97 {
98 cii++;
99 continue;
100 }
101
102 // Process the neighbors
103 for (int nii = 1; nii < static_cast<int> (nn_indices.size ()); ++nii) // nii = neighbor indices iterator
104 {
105 // Has this point been processed before?
106 if (nn_indices[nii] == UNAVAILABLE || processed[nn_indices[nii]])
107 continue;
108
109 // Validate if condition holds
110 if (condition_function_ ((*input_)[current_cluster[cii]], (*input_)[nn_indices[nii]], nn_distances[nii]))
111 {
112 // Add the point to the cluster
113 current_cluster.push_back (nn_indices[nii]);
114 processed[nn_indices[nii]] = true;
115 }
116 }
117 cii++;
118 }
119
120 // If extracting removed clusters, all clusters need to be saved, otherwise only the ones within the given cluster size range
121 if (extract_removed_clusters_ ||
122 (static_cast<int> (current_cluster.size ()) >= min_cluster_size_ &&
123 static_cast<int> (current_cluster.size ()) <= max_cluster_size_))
124 {
126 pi.header = input_->header;
127 pi.indices.resize (current_cluster.size ());
128 for (int ii = 0; ii < static_cast<int> (current_cluster.size ()); ++ii) // ii = indices iterator
129 pi.indices[ii] = current_cluster[ii];
130
131 if (extract_removed_clusters_ && static_cast<int> (current_cluster.size ()) < min_cluster_size_)
132 small_clusters_->push_back (pi);
133 else if (extract_removed_clusters_ && static_cast<int> (current_cluster.size ()) > max_cluster_size_)
134 large_clusters_->push_back (pi);
135 else
136 clusters.push_back (pi);
137 }
138 }
139
140 deinitCompute ();
141}
142
143#define PCL_INSTANTIATE_ConditionalEuclideanClustering(T) template class PCL_EXPORTS pcl::ConditionalEuclideanClustering<T>;
144
145#endif // PCL_SEGMENTATION_IMPL_CONDITIONAL_EUCLIDEAN_CLUSTERING_HPP_
146
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 neigbhor search in organized point clouds.
Definition organized.h:61
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
std::vector< pcl::PointIndices > IndicesClusters
::pcl::PCLHeader header