41 #ifndef PCL_PEOPLE_HEAD_BASED_SUBCLUSTER_HPP_
42 #define PCL_PEOPLE_HEAD_BASED_SUBCLUSTER_HPP_
44 #include <pcl/people/head_based_subcluster.h>
46 template <
typename Po
intT>
51 head_centroid_ =
true;
56 heads_minimum_distance_ = 0.3;
59 sqrt_ground_coeffs_ = std::numeric_limits<float>::quiet_NaN();
62 template <
typename Po
intT>
void
68 template <
typename Po
intT>
void
71 ground_coeffs_ = ground_coeffs;
72 sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm();
75 template <
typename Po
intT>
void
78 cluster_indices_ = cluster_indices;
81 template <
typename Po
intT>
void
87 template <
typename Po
intT>
void
90 min_height_ = min_height;
91 max_height_ = max_height;
94 template <
typename Po
intT>
void
97 min_points_ = min_points;
98 max_points_ = max_points;
101 template <
typename Po
intT>
void
104 heads_minimum_distance_= heads_minimum_distance;
107 template <
typename Po
intT>
void
110 head_centroid_ = head_centroid;
113 template <
typename Po
intT>
void
116 min_height = min_height_;
117 max_height = max_height_;
120 template <
typename Po
intT>
void
123 min_points = min_points_;
124 max_points = max_points_;
127 template <
typename Po
intT>
float
130 return (heads_minimum_distance_);
133 template <
typename Po
intT>
void
137 float min_distance_between_cluster_centers = 0.4;
138 float normalize_factor = std::pow(sqrt_ground_coeffs_, 2);
139 Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head(3);
140 std::vector <std::vector<int> > connected_clusters;
141 connected_clusters.resize(input_clusters.size());
142 std::vector<bool> used_clusters;
143 used_clusters.resize(input_clusters.size());
144 for(std::size_t i = 0; i < input_clusters.size(); i++)
146 Eigen::Vector3f theoretical_center = input_clusters[i].getTCenter();
147 float t = theoretical_center.dot(head_ground_coeffs) / normalize_factor;
148 Eigen::Vector3f current_cluster_center_projection = theoretical_center - head_ground_coeffs * t;
149 for(std::size_t j = i+1; j < input_clusters.size(); j++)
151 theoretical_center = input_clusters[j].getTCenter();
152 float t = theoretical_center.dot(head_ground_coeffs) / normalize_factor;
153 Eigen::Vector3f new_cluster_center_projection = theoretical_center - head_ground_coeffs * t;
154 if (((new_cluster_center_projection - current_cluster_center_projection).norm()) < min_distance_between_cluster_centers)
156 connected_clusters[i].push_back(j);
161 for(std::size_t i = 0; i < connected_clusters.size(); i++)
163 if (!used_clusters[i])
165 used_clusters[i] =
true;
166 if (connected_clusters[i].empty())
168 output_clusters.push_back(input_clusters[i]);
174 point_indices = input_clusters[i].getIndices();
175 for(
const int &cluster : connected_clusters[i])
177 if (!used_clusters[cluster])
179 used_clusters[cluster] =
true;
180 for(std::vector<int>::const_iterator points_iterator = input_clusters[cluster].getIndices().indices.begin();
181 points_iterator != input_clusters[cluster].getIndices().indices.end(); ++points_iterator)
183 point_indices.
indices.push_back(*points_iterator);
188 output_clusters.push_back(cluster);
194 template <
typename Po
intT>
void
199 float normalize_factor = std::pow(sqrt_ground_coeffs_, 2);
200 Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head(3);
201 Eigen::Matrix3Xf maxima_projected(3,maxima_number);
202 Eigen::VectorXi subclusters_number_of_points(maxima_number);
203 std::vector <std::vector <int> > sub_clusters_indices;
204 sub_clusters_indices.resize(maxima_number);
207 for(
int i = 0; i < maxima_number; i++)
209 PointT* current_point = &(*cloud_)[maxima_cloud_indices[i]];
210 Eigen::Vector3f p_current_eigen(current_point->x, current_point->y, current_point->z);
211 float t = p_current_eigen.dot(head_ground_coeffs) / normalize_factor;
212 maxima_projected.col(i).matrix () = p_current_eigen - head_ground_coeffs * t;
213 subclusters_number_of_points(i) = 0;
217 for(std::vector<int>::const_iterator points_iterator = cluster.
getIndices().
indices.begin(); points_iterator != cluster.
getIndices().
indices.end(); ++points_iterator)
219 PointT* current_point = &(*cloud_)[*points_iterator];
220 Eigen::Vector3f p_current_eigen(current_point->x, current_point->y, current_point->z);
221 float t = p_current_eigen.dot(head_ground_coeffs) / normalize_factor;
222 p_current_eigen -= head_ground_coeffs * t;
225 bool correspondence_detected =
false;
226 while ((!correspondence_detected) && (i < maxima_number))
228 if (((p_current_eigen - maxima_projected.col(i)).norm()) < heads_minimum_distance_)
230 correspondence_detected =
true;
231 sub_clusters_indices[i].push_back(*points_iterator);
232 subclusters_number_of_points(i)++;
240 for(
int i = 0; i < maxima_number; i++)
242 if (subclusters_number_of_points(i) > min_points_)
245 point_indices.
indices = sub_clusters_indices[i];
248 subclusters.push_back(cluster);
254 template <
typename Po
intT>
void
258 if (std::isnan(sqrt_ground_coeffs_))
260 PCL_ERROR (
"[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Floor parameters have not been set or they are not valid!\n");
263 if (cluster_indices_.empty ())
265 PCL_ERROR (
"[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Cluster indices have not been set!\n");
268 if (cloud_ ==
nullptr)
270 PCL_ERROR (
"[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Input cloud has not been set!\n");
275 for(std::vector<pcl::PointIndices>::const_iterator it = cluster_indices_.begin(); it != cluster_indices_.end(); ++it)
278 clusters.push_back(cluster);
282 std::vector<pcl::people::PersonCluster<PointT> > new_clusters;
283 for(std::size_t i = 0; i < clusters.size(); i++)
285 if (clusters[i].getHeight() <= max_height_)
286 new_clusters.push_back(clusters[i]);
288 clusters = new_clusters;
289 new_clusters.clear();
292 mergeClustersCloseInFloorCoordinates(clusters, new_clusters);
293 clusters = new_clusters;
295 std::vector<pcl::people::PersonCluster<PointT> > subclusters;
296 int cluster_min_points_sub = int(
float(min_points_) * 1.5);
301 height_map_obj.
setGround(ground_coeffs_);
307 float height = it->getHeight();
308 int number_of_points = it->getNumberPoints();
309 if(height > min_height_ && height < max_height_)
311 if (number_of_points > cluster_min_points_sub)
322 subclusters.push_back(*it);
328 subclusters.push_back(*it);
332 clusters = subclusters;
335 template <
typename Po
intT>