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>
int & getMaximaNumberAfterFiltering()
Return the maxima number after the filterMaxima method.
float getMinimumDistanceBetweenHeads()
Get minimum distance between persons' heads.
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 subcluster(std::vector< pcl::people::PersonCluster< PointT > > &clusters)
Compute subclusters and return them into a vector of PersonCluster.
void setHeightLimits(float min_height, float max_height)
Set 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 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.
std::vector< int > indices
HeadBasedSubclustering()
Constructor.
void setHeadCentroid(bool head_centroid)
Set head_centroid_ to true (person centroid is in the head) or false (person centroid is the whole bo...
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
PersonCluster represents a class for representing information about a cluster containing a person...
void setInitialClusters(std::vector< pcl::PointIndices > &cluster_indices)
Set initial cluster indices.
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 setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
boost::shared_ptr< PointCloud > PointCloudPtr
virtual ~HeadBasedSubclustering()
Destructor.
std::vector< int > & getMaximaCloudIndicesFiltered()
Return the point cloud indices corresponding to the maxima computed after the filterMaxima method...
void setInputCloud(PointCloudPtr &cloud)
Set input cloud.
void setInputCloud(PointCloudPtr &cloud)
Set initial cluster indices.
void getDimensionLimits(int &min_points, int &max_points)
Get minimum and maximum allowed number of points for a person cluster.
void setMinimumDistanceBetweenMaxima(float minimum_distance_between_maxima)
Set minimum distance between maxima.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
A point structure representing Euclidean xyz coordinates, and the RGB color.
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 setMinimumDistanceBetweenHeads(float heads_minimum_distance)
Set minimum distance between persons' heads.
pcl::PointIndices & getIndices()
Returns the indices of the point cloud points corresponding to the cluster.