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(
unsigned int 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(
unsigned int 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(
unsigned int 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(
unsigned int j = 0; j < connected_clusters[i].size(); j++)
177 if (!used_clusters[connected_clusters[i][j]])
179 used_clusters[connected_clusters[i][j]] =
true;
180 for(std::vector<int>::const_iterator points_iterator = input_clusters[connected_clusters[i][j]].getIndices().indices.begin();
181 points_iterator != input_clusters[connected_clusters[i][j]].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_->points[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[*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 = 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 (sqrt_ground_coeffs_ != 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_.size() == 0)
265 PCL_ERROR (
"[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Cluster indices have not been set!\n");
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(
unsigned int 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>
void getDimensionLimits(int &min_points, int &max_points)
Get minimum and maximum allowed number of points for a person cluster.
void setHeightLimits(float min_height, float max_height)
Set minimum and maximum allowed height for a person cluster.
void createSubClusters(pcl::people::PersonCluster< PointT > &cluster, int maxima_number_after_filtering, std::vector< int > &maxima_cloud_indices_filtered, std::vector< pcl::people::PersonCluster< PointT > > &subclusters)
Create subclusters centered on the heads position from the current cluster.
void setInitialClusters(std::vector< pcl::PointIndices > &cluster_indices)
Set initial cluster indices.
void setInputCloud(PointCloudPtr &cloud)
Set initial cluster indices.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
void setInputCloud(PointCloudPtr &cloud)
Set input cloud.
boost::shared_ptr< PointCloud > PointCloudPtr
float getMinimumDistanceBetweenHeads()
Get minimum distance between persons' heads.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
HeightMap2D represents a class for creating a 2D height map from a point cloud and searching for its ...
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
void subcluster(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Compute subclusters and return them into a vector of PersonCluster.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
std::vector< int > & getMaximaCloudIndicesFiltered()
Return the point cloud indices corresponding to the maxima computed after the filterMaxima method...
HeadBasedSubclustering()
Constructor.
void setDimensionLimits(int min_points, int max_points)
Set minimum and maximum allowed number of points for a person cluster.
void getHeightLimits(float &min_height, float &max_height)
Get minimum and maximum allowed height for a person cluster.
void compute(pcl::people::PersonCluster< PointT > &cluster)
Compute the height map with the projection of cluster points onto the ground plane.
void mergeClustersCloseInFloorCoordinates(std::vector< pcl::people::PersonCluster< PointT > > &input_clusters, std::vector< pcl::people::PersonCluster< PointT > > &output_clusters)
Merge clusters close in floor coordinates.
void setMinimumDistanceBetweenMaxima(float minimum_distance_between_maxima)
Set minimum distance between maxima.
void setHeadCentroid(bool head_centroid)
Set head_centroid_ to true (person centroid is in the head) or false (person centroid is the whole bo...
PersonCluster represents a class for representing information about a cluster containing a person...
pcl::PointIndices & getIndices()
Returns the indices of the point cloud points corresponding to the cluster.
virtual ~HeadBasedSubclustering()
Destructor.
std::vector< int > indices
A point structure representing Euclidean xyz coordinates, and the RGB color.
void setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
int & getMaximaNumberAfterFiltering()
Return the maxima number after the filterMaxima method.