Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2013-, Open Perception, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of the copyright holder(s) nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * head_based_subcluster.hpp 00037 * Created on: Nov 30, 2012 00038 * Author: Matteo Munaro 00039 */ 00040 00041 #ifndef PCL_PEOPLE_HEAD_BASED_SUBCLUSTER_HPP_ 00042 #define PCL_PEOPLE_HEAD_BASED_SUBCLUSTER_HPP_ 00043 00044 #include <pcl/people/head_based_subcluster.h> 00045 00046 template <typename PointT> 00047 pcl::people::HeadBasedSubclustering<PointT>::HeadBasedSubclustering () 00048 { 00049 // set default values for optional parameters: 00050 vertical_ = false; 00051 head_centroid_ = true; 00052 min_height_ = 1.3; 00053 max_height_ = 2.3; 00054 min_points_ = 30; 00055 max_points_ = 5000; 00056 heads_minimum_distance_ = 0.3; 00057 00058 // set flag values for mandatory parameters: 00059 sqrt_ground_coeffs_ = std::numeric_limits<float>::quiet_NaN(); 00060 } 00061 00062 template <typename PointT> void 00063 pcl::people::HeadBasedSubclustering<PointT>::setInputCloud (PointCloudPtr& cloud) 00064 { 00065 cloud_ = cloud; 00066 } 00067 00068 template <typename PointT> void 00069 pcl::people::HeadBasedSubclustering<PointT>::setGround (Eigen::VectorXf& ground_coeffs) 00070 { 00071 ground_coeffs_ = ground_coeffs; 00072 sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm(); 00073 } 00074 00075 template <typename PointT> void 00076 pcl::people::HeadBasedSubclustering<PointT>::setInitialClusters (std::vector<pcl::PointIndices>& cluster_indices) 00077 { 00078 cluster_indices_ = cluster_indices; 00079 } 00080 00081 template <typename PointT> void 00082 pcl::people::HeadBasedSubclustering<PointT>::setSensorPortraitOrientation (bool vertical) 00083 { 00084 vertical_ = vertical; 00085 } 00086 00087 template <typename PointT> void 00088 pcl::people::HeadBasedSubclustering<PointT>::setHeightLimits (float min_height, float max_height) 00089 { 00090 min_height_ = min_height; 00091 max_height_ = max_height; 00092 } 00093 00094 template <typename PointT> void 00095 pcl::people::HeadBasedSubclustering<PointT>::setDimensionLimits (int min_points, int max_points) 00096 { 00097 min_points_ = min_points; 00098 max_points_ = max_points; 00099 } 00100 00101 template <typename PointT> void 00102 pcl::people::HeadBasedSubclustering<PointT>::setMinimumDistanceBetweenHeads (float heads_minimum_distance) 00103 { 00104 heads_minimum_distance_= heads_minimum_distance; 00105 } 00106 00107 template <typename PointT> void 00108 pcl::people::HeadBasedSubclustering<PointT>::setHeadCentroid (bool head_centroid) 00109 { 00110 head_centroid_ = head_centroid; 00111 } 00112 00113 template <typename PointT> void 00114 pcl::people::HeadBasedSubclustering<PointT>::getHeightLimits (float& min_height, float& max_height) 00115 { 00116 min_height = min_height_; 00117 max_height = max_height_; 00118 } 00119 00120 template <typename PointT> void 00121 pcl::people::HeadBasedSubclustering<PointT>::getDimensionLimits (int& min_points, int& max_points) 00122 { 00123 min_points = min_points_; 00124 max_points = max_points_; 00125 } 00126 00127 template <typename PointT> float 00128 pcl::people::HeadBasedSubclustering<PointT>::getMinimumDistanceBetweenHeads () 00129 { 00130 return (heads_minimum_distance_); 00131 } 00132 00133 template <typename PointT> void 00134 pcl::people::HeadBasedSubclustering<PointT>::mergeClustersCloseInFloorCoordinates (std::vector<pcl::people::PersonCluster<PointT> >& input_clusters, 00135 std::vector<pcl::people::PersonCluster<PointT> >& output_clusters) 00136 { 00137 float min_distance_between_cluster_centers = 0.4; // meters 00138 float normalize_factor = std::pow(sqrt_ground_coeffs_, 2); // sqrt_ground_coeffs ^ 2 (precomputed for speed) 00139 Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head(3); // ground plane normal (precomputed for speed) 00140 std::vector <std::vector<int> > connected_clusters; 00141 connected_clusters.resize(input_clusters.size()); 00142 std::vector<bool> used_clusters; // 0 in correspondence of clusters remained to process, 1 for already used clusters 00143 used_clusters.resize(input_clusters.size()); 00144 for(unsigned int i = 0; i < input_clusters.size(); i++) // for every cluster 00145 { 00146 Eigen::Vector3f theoretical_center = input_clusters[i].getTCenter(); 00147 float t = theoretical_center.dot(head_ground_coeffs) / normalize_factor; // height from the ground 00148 Eigen::Vector3f current_cluster_center_projection = theoretical_center - head_ground_coeffs * t; // projection of the point on the groundplane 00149 for(unsigned int j = i+1; j < input_clusters.size(); j++) // for every remaining cluster 00150 { 00151 theoretical_center = input_clusters[j].getTCenter(); 00152 float t = theoretical_center.dot(head_ground_coeffs) / normalize_factor; // height from the ground 00153 Eigen::Vector3f new_cluster_center_projection = theoretical_center - head_ground_coeffs * t; // projection of the point on the groundplane 00154 if (((new_cluster_center_projection - current_cluster_center_projection).norm()) < min_distance_between_cluster_centers) 00155 { 00156 connected_clusters[i].push_back(j); 00157 } 00158 } 00159 } 00160 00161 for(unsigned int i = 0; i < connected_clusters.size(); i++) // for every cluster 00162 { 00163 if (!used_clusters[i]) // if this cluster has not been used yet 00164 { 00165 used_clusters[i] = true; 00166 if (connected_clusters[i].empty()) // no other clusters to merge 00167 { 00168 output_clusters.push_back(input_clusters[i]); 00169 } 00170 else 00171 { 00172 // Copy cluster points into new cluster: 00173 pcl::PointIndices point_indices; 00174 point_indices = input_clusters[i].getIndices(); 00175 for(unsigned int j = 0; j < connected_clusters[i].size(); j++) 00176 { 00177 if (!used_clusters[connected_clusters[i][j]]) // if this cluster has not been used yet 00178 { 00179 used_clusters[connected_clusters[i][j]] = true; 00180 for(std::vector<int>::const_iterator points_iterator = input_clusters[connected_clusters[i][j]].getIndices().indices.begin(); 00181 points_iterator != input_clusters[connected_clusters[i][j]].getIndices().indices.end(); points_iterator++) 00182 { 00183 point_indices.indices.push_back(*points_iterator); 00184 } 00185 } 00186 } 00187 pcl::people::PersonCluster<PointT> cluster(cloud_, point_indices, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_); 00188 output_clusters.push_back(cluster); 00189 } 00190 } 00191 } 00192 } 00193 00194 template <typename PointT> void 00195 pcl::people::HeadBasedSubclustering<PointT>::createSubClusters (pcl::people::PersonCluster<PointT>& cluster, int maxima_number, 00196 std::vector<int>& maxima_cloud_indices, std::vector<pcl::people::PersonCluster<PointT> >& subclusters) 00197 { 00198 // create new clusters from the current cluster and put corresponding indices into sub_clusters_indices: 00199 float normalize_factor = std::pow(sqrt_ground_coeffs_, 2); // sqrt_ground_coeffs ^ 2 (precomputed for speed) 00200 Eigen::Vector3f head_ground_coeffs = ground_coeffs_.head(3); // ground plane normal (precomputed for speed) 00201 Eigen::Matrix3Xf maxima_projected(3,maxima_number); // matrix containing the projection of maxima onto the ground plane 00202 Eigen::VectorXi subclusters_number_of_points(maxima_number); // subclusters number of points 00203 std::vector <std::vector <int> > sub_clusters_indices; // vector of vectors with the cluster indices for every maximum 00204 sub_clusters_indices.resize(maxima_number); // resize to number of maxima 00205 00206 // Project maxima on the ground plane: 00207 for(int i = 0; i < maxima_number; i++) // for every maximum 00208 { 00209 PointT* current_point = &cloud_->points[maxima_cloud_indices[i]]; // current maximum point cloud point 00210 Eigen::Vector3f p_current_eigen(current_point->x, current_point->y, current_point->z); // conversion to eigen 00211 float t = p_current_eigen.dot(head_ground_coeffs) / normalize_factor; // height from the ground 00212 maxima_projected.col(i).matrix () = p_current_eigen - head_ground_coeffs * t; // projection of the point on the groundplane 00213 subclusters_number_of_points(i) = 0; // intialize number of points 00214 } 00215 00216 // Associate cluster points to one of the maximum: 00217 for(std::vector<int>::const_iterator points_iterator = cluster.getIndices().indices.begin(); points_iterator != cluster.getIndices().indices.end(); points_iterator++) 00218 { 00219 PointT* current_point = &cloud_->points[*points_iterator]; // current point cloud point 00220 Eigen::Vector3f p_current_eigen(current_point->x, current_point->y, current_point->z); // conversion to eigen 00221 float t = p_current_eigen.dot(head_ground_coeffs) / normalize_factor; // height from the ground 00222 p_current_eigen = p_current_eigen - head_ground_coeffs * t; // projection of the point on the groundplane 00223 00224 int i = 0; 00225 bool correspondence_detected = false; 00226 while ((!correspondence_detected) && (i < maxima_number)) 00227 { 00228 if (((p_current_eigen - maxima_projected.col(i)).norm()) < heads_minimum_distance_) 00229 { 00230 correspondence_detected = true; 00231 sub_clusters_indices[i].push_back(*points_iterator); 00232 subclusters_number_of_points(i)++; 00233 } 00234 else 00235 i++; 00236 } 00237 } 00238 00239 // Create a subcluster if the number of points associated to a maximum is over a threshold: 00240 for(int i = 0; i < maxima_number; i++) // for every maximum 00241 { 00242 if (subclusters_number_of_points(i) > min_points_) 00243 { 00244 pcl::PointIndices point_indices; 00245 point_indices.indices = sub_clusters_indices[i]; // indices associated to the i-th maximum 00246 00247 pcl::people::PersonCluster<PointT> cluster(cloud_, point_indices, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_); 00248 subclusters.push_back(cluster); 00249 //std::cout << "Cluster number of points: " << subclusters_number_of_points(i) << std::endl; 00250 } 00251 } 00252 } 00253 00254 template <typename PointT> void 00255 pcl::people::HeadBasedSubclustering<PointT>::subcluster (std::vector<pcl::people::PersonCluster<PointT> >& clusters) 00256 { 00257 // Check if all mandatory variables have been set: 00258 if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_) 00259 { 00260 PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Floor parameters have not been set or they are not valid!\n"); 00261 return; 00262 } 00263 if (cluster_indices_.size() == 0) 00264 { 00265 PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Cluster indices have not been set!\n"); 00266 return; 00267 } 00268 if (cloud_ == NULL) 00269 { 00270 PCL_ERROR ("[pcl::people::pcl::people::HeadBasedSubclustering::subcluster] Input cloud has not been set!\n"); 00271 return; 00272 } 00273 00274 // Person clusters creation from clusters indices: 00275 for(std::vector<pcl::PointIndices>::const_iterator it = cluster_indices_.begin(); it != cluster_indices_.end(); ++it) 00276 { 00277 pcl::people::PersonCluster<PointT> cluster(cloud_, *it, ground_coeffs_, sqrt_ground_coeffs_, head_centroid_, vertical_); // PersonCluster creation 00278 clusters.push_back(cluster); 00279 } 00280 00281 // Remove clusters with too high height from the ground plane: 00282 std::vector<pcl::people::PersonCluster<PointT> > new_clusters; 00283 for(unsigned int i = 0; i < clusters.size(); i++) // for every cluster 00284 { 00285 if (clusters[i].getHeight() <= max_height_) 00286 new_clusters.push_back(clusters[i]); 00287 } 00288 clusters = new_clusters; 00289 new_clusters.clear(); 00290 00291 // Merge clusters close in floor coordinates: 00292 mergeClustersCloseInFloorCoordinates(clusters, new_clusters); 00293 clusters = new_clusters; 00294 00295 std::vector<pcl::people::PersonCluster<PointT> > subclusters; 00296 int cluster_min_points_sub = int(float(min_points_) * 1.5); 00297 // int cluster_max_points_sub = max_points_; 00298 00299 // create HeightMap2D object: 00300 pcl::people::HeightMap2D<PointT> height_map_obj; 00301 height_map_obj.setGround(ground_coeffs_); 00302 height_map_obj.setInputCloud(cloud_); 00303 height_map_obj.setSensorPortraitOrientation(vertical_); 00304 height_map_obj.setMinimumDistanceBetweenMaxima(heads_minimum_distance_); 00305 for(typename std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it) // for every cluster 00306 { 00307 float height = it->getHeight(); 00308 int number_of_points = it->getNumberPoints(); 00309 if(height > min_height_ && height < max_height_) 00310 { 00311 if (number_of_points > cluster_min_points_sub) // && number_of_points < cluster_max_points_sub) 00312 { 00313 // Compute height map associated to the current cluster and its local maxima (heads): 00314 height_map_obj.compute(*it); 00315 if (height_map_obj.getMaximaNumberAfterFiltering() > 1) // if more than one maximum 00316 { 00317 // create new clusters from the current cluster and put corresponding indices into sub_clusters_indices: 00318 createSubClusters(*it, height_map_obj.getMaximaNumberAfterFiltering(), height_map_obj.getMaximaCloudIndicesFiltered(), subclusters); 00319 } 00320 else 00321 { // Only one maximum --> copy original cluster: 00322 subclusters.push_back(*it); 00323 } 00324 } 00325 else 00326 { 00327 // Cluster properties not good for sub-clustering --> copy original cluster: 00328 subclusters.push_back(*it); 00329 } 00330 } 00331 } 00332 clusters = subclusters; // substitute clusters with subclusters 00333 } 00334 00335 template <typename PointT> 00336 pcl::people::HeadBasedSubclustering<PointT>::~HeadBasedSubclustering () 00337 { 00338 // TODO Auto-generated destructor stub 00339 } 00340 #endif /* PCL_PEOPLE_HEAD_BASED_SUBCLUSTER_HPP_ */