89 {
90 std::vector<int> nn_indices;
91 nn_indices.reserve(200);
92 std::vector<bool> processed(cloud_->size(), false);
93
94 for (std::size_t iii = 0; iii < cloud_->size(); ++iii) {
95 if (processed[iii]) {
96 continue;
97 }
98
99 std::vector<int> current_cluster;
100 current_cluster.reserve(200);
101 std::size_t cii = 0;
102 current_cluster.push_back(static_cast<int>(iii));
103 processed[iii] = true;
104
105
106 while (cii < current_cluster.size()) {
107 nn_indices.clear();
108 if (candidate_function_(cloud_->at(current_cluster[cii]),
109 candidate_param_, &nn_indices) < 1) {
110 cii++;
111 continue;
112 }
113 for (std::size_t nii = 1; nii < nn_indices.size(); ++nii) {
114 if (nn_indices[nii] < 0 ||
115 nn_indices[nii] >= static_cast<int>(processed.size()) ||
116 processed[nn_indices[nii]]) {
117 continue;
118 }
119 if (condition_function_(cloud_->at(current_cluster[cii]),
120 cloud_->at(nn_indices[nii]),
121 condition_param_)) {
122 current_cluster.push_back(nn_indices[nii]);
123 processed[nn_indices[nii]] = true;
124 }
125 }
126 cii++;
127 }
128
129 if (extract_removed_clusters_ ||
130 (current_cluster.size() >= min_cluster_size_ &&
131 current_cluster.size() <= max_cluster_size_)) {
132 base::PointIndices pi;
133 pi.indices.resize(current_cluster.size());
134 for (int ii = 0; ii < static_cast<int>(current_cluster.size()); ++ii) {
135 pi.indices[ii] = current_cluster[ii];
136 }
137
138 if (extract_removed_clusters_ &&
139 current_cluster.size() < min_cluster_size_) {
140 small_clusters_->push_back(pi);
141 } else if (extract_removed_clusters_ &&
142 current_cluster.size() > max_cluster_size_) {
143 large_clusters_->push_back(pi);
144 } else {
145 xy_clusters->push_back(pi);
146 }
147 }
148 }
149}