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 * height_map_2d.hpp 00037 * Created on: Nov 30, 2012 00038 * Author: Matteo Munaro 00039 */ 00040 00041 #include <pcl/people/height_map_2d.h> 00042 00043 #ifndef PCL_PEOPLE_HEIGHT_MAP_2D_HPP_ 00044 #define PCL_PEOPLE_HEIGHT_MAP_2D_HPP_ 00045 00046 template <typename PointT> 00047 pcl::people::HeightMap2D<PointT>::HeightMap2D () 00048 { 00049 // set default values for optional parameters: 00050 vertical_ = false; 00051 min_dist_between_maxima_ = 0.3; 00052 bin_size_ = 0.06; 00053 00054 // set flag values for mandatory parameters: 00055 sqrt_ground_coeffs_ = std::numeric_limits<float>::quiet_NaN(); 00056 } 00057 00058 template <typename PointT> void 00059 pcl::people::HeightMap2D<PointT>::compute (pcl::people::PersonCluster<PointT>& cluster) 00060 { 00061 // Check if all mandatory variables have been set: 00062 if (sqrt_ground_coeffs_ != sqrt_ground_coeffs_) 00063 { 00064 PCL_ERROR ("[pcl::people::HeightMap2D::compute] Floor parameters have not been set or they are not valid!\n"); 00065 return; 00066 } 00067 if (cloud_ == NULL) 00068 { 00069 PCL_ERROR ("[pcl::people::HeightMap2D::compute] Input cloud has not been set!\n"); 00070 return; 00071 } 00072 00073 // Reset variables: 00074 buckets_.clear(); 00075 buckets_cloud_indices_.clear(); 00076 maxima_indices_.clear(); 00077 maxima_cloud_indices_.clear(); 00078 maxima_indices_filtered_.clear(); 00079 maxima_cloud_indices_filtered_.clear(); 00080 00081 // Create a height map with the projection of cluster points onto the ground plane: 00082 if (!vertical_) // camera horizontal 00083 buckets_.resize(size_t((cluster.getMax()(0) - cluster.getMin()(0)) / bin_size_) + 1, 0); 00084 else // camera vertical 00085 buckets_.resize(size_t((cluster.getMax()(1) - cluster.getMin()(1)) / bin_size_) + 1, 0); 00086 buckets_cloud_indices_.resize(buckets_.size(), 0); 00087 00088 for(std::vector<int>::const_iterator pit = cluster.getIndices().indices.begin(); pit != cluster.getIndices().indices.end(); pit++) 00089 { 00090 PointT* p = &cloud_->points[*pit]; 00091 int index; 00092 if (!vertical_) // camera horizontal 00093 index = int((p->x - cluster.getMin()(0)) / bin_size_); 00094 else // camera vertical 00095 index = int((p->y - cluster.getMin()(1)) / bin_size_); 00096 if(index > (buckets_.size() - 1)) 00097 std::cout << "Error: out of array - " << index << " of " << buckets_.size() << std::endl; 00098 else 00099 { 00100 Eigen::Vector4f new_point(p->x, p->y, p->z, 1.0f); // select point from cluster 00101 float heightp = std::fabs(new_point.dot(ground_coeffs_)); // compute point height from the groundplane 00102 heightp /= sqrt_ground_coeffs_; 00103 if ((heightp * 60) > buckets_[index]) // compare the height of the new point with the existing one 00104 { 00105 buckets_[index] = heightp * 60; // maximum height 00106 buckets_cloud_indices_[index] = *pit; // point cloud index of the point with maximum height 00107 } 00108 } 00109 } 00110 00111 // Compute local maxima of the height map: 00112 searchLocalMaxima(); 00113 00114 // Filter maxima by imposing a minimum distance between them (minimum distance between people heads): 00115 filterMaxima(); 00116 } 00117 00118 template <typename PointT> void 00119 pcl::people::HeightMap2D<PointT>::searchLocalMaxima () 00120 { 00121 // Search for local maxima: 00122 maxima_number_ = 0; 00123 int left = buckets_[0]; // current left element 00124 int right = 0; // current right element 00125 float offset = 0; // used to center the maximum to the right place 00126 maxima_indices_.resize(size_t(buckets_.size()), 0); 00127 maxima_cloud_indices_.resize(size_t(buckets_.size()), 0); 00128 00129 // Handle first element: 00130 if (buckets_[0] > buckets_[1]) 00131 { 00132 maxima_indices_[maxima_number_] = 0; 00133 maxima_cloud_indices_[maxima_number_] = buckets_cloud_indices_[maxima_indices_[maxima_number_]]; 00134 maxima_number_++; 00135 } 00136 00137 // Main loop: 00138 int i = 1; 00139 while(i < (buckets_.size()-1)) 00140 { 00141 right = buckets_[i+1]; 00142 if ((buckets_[i] > left) && (buckets_[i] > right)) 00143 { 00144 // Search where to insert the new element (in an ordered array): 00145 int t = 0; // position of the new element 00146 while ((t < maxima_number_) && (buckets_[i] < buckets_[maxima_indices_[t]])) 00147 { 00148 t++; 00149 } 00150 // Move forward the smaller elements: 00151 for (int m = maxima_number_; m > t; m--) 00152 { 00153 maxima_indices_[m] = maxima_indices_[m-1]; 00154 maxima_cloud_indices_[m] = maxima_cloud_indices_[m-1]; 00155 } 00156 // Insert the new element: 00157 maxima_indices_[t] = i - int(offset/2 + 0.5); 00158 maxima_cloud_indices_[t] = buckets_cloud_indices_[maxima_indices_[t]]; 00159 left = buckets_[i+1]; 00160 i = i+2; 00161 offset = 0; 00162 maxima_number_++; 00163 } 00164 else 00165 { 00166 if (buckets_[i] == right) 00167 { 00168 offset++; 00169 } 00170 else 00171 { 00172 left = buckets_[i]; 00173 offset = 0; 00174 } 00175 i++; 00176 } 00177 } 00178 00179 // Handle last element: 00180 if (buckets_[buckets_.size()-1] > left) 00181 { 00182 // Search where to insert the new element (in an ordered array): 00183 int t = 0; // position of the new element 00184 while ((t < maxima_number_) && (buckets_[buckets_.size()-1] < buckets_[maxima_indices_[t]])) 00185 { 00186 t++; 00187 } 00188 // Move forward the smaller elements: 00189 for (int m = maxima_number_; m > t; m--) 00190 { 00191 maxima_indices_[m] = maxima_indices_[m-1]; 00192 maxima_cloud_indices_[m] = maxima_cloud_indices_[m-1]; 00193 } 00194 // Insert the new element: 00195 maxima_indices_[t] = i - int(offset/2 + 0.5); 00196 maxima_cloud_indices_[t] = buckets_cloud_indices_[maxima_indices_[t]]; 00197 00198 maxima_number_++; 00199 } 00200 } 00201 00202 template <typename PointT> void 00203 pcl::people::HeightMap2D<PointT>::filterMaxima () 00204 { 00205 // Filter maxima according to their distance when projected on the ground plane: 00206 maxima_number_after_filtering_ = 0; 00207 maxima_indices_filtered_.resize(maxima_number_, 0); 00208 maxima_cloud_indices_filtered_.resize(maxima_number_, 0); 00209 if (maxima_number_ > 0) 00210 { 00211 for (int i = 0; i < maxima_number_; i++) 00212 { 00213 bool good_maximum = true; 00214 float distance = 0; 00215 00216 PointT* p_current = &cloud_->points[maxima_cloud_indices_[i]]; // pointcloud point referring to the current maximum 00217 Eigen::Vector3f p_current_eigen(p_current->x, p_current->y, p_current->z); // conversion to eigen 00218 float t = p_current_eigen.dot(ground_coeffs_.head(3)) / std::pow(sqrt_ground_coeffs_, 2); // height from the ground 00219 p_current_eigen = p_current_eigen - ground_coeffs_.head(3) * t; // projection of the point on the groundplane 00220 00221 int j = i-1; 00222 while ((j >= 0) && (good_maximum)) 00223 { 00224 PointT* p_previous = &cloud_->points[maxima_cloud_indices_[j]]; // pointcloud point referring to an already validated maximum 00225 Eigen::Vector3f p_previous_eigen(p_previous->x, p_previous->y, p_previous->z); // conversion to eigen 00226 float t = p_previous_eigen.dot(ground_coeffs_.head(3)) / std::pow(sqrt_ground_coeffs_, 2); // height from the ground 00227 p_previous_eigen = p_previous_eigen - ground_coeffs_.head(3) * t; // projection of the point on the groundplane 00228 00229 // distance of the projection of the points on the groundplane: 00230 distance = (p_current_eigen-p_previous_eigen).norm(); 00231 if (distance < min_dist_between_maxima_) 00232 { 00233 good_maximum = false; 00234 } 00235 j--; 00236 } 00237 if (good_maximum) 00238 { 00239 maxima_indices_filtered_[maxima_number_after_filtering_] = maxima_indices_[i]; 00240 maxima_cloud_indices_filtered_[maxima_number_after_filtering_] = maxima_cloud_indices_[i]; 00241 maxima_number_after_filtering_++; 00242 } 00243 } 00244 } 00245 } 00246 00247 template <typename PointT> void 00248 pcl::people::HeightMap2D<PointT>::setInputCloud (PointCloudPtr& cloud) 00249 { 00250 cloud_ = cloud; 00251 } 00252 00253 template <typename PointT> 00254 void pcl::people::HeightMap2D<PointT>::setGround(Eigen::VectorXf& ground_coeffs) 00255 { 00256 ground_coeffs_ = ground_coeffs; 00257 sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm(); 00258 } 00259 00260 template <typename PointT> void 00261 pcl::people::HeightMap2D<PointT>::setBinSize (float bin_size) 00262 { 00263 bin_size_ = bin_size; 00264 } 00265 00266 template <typename PointT> void 00267 pcl::people::HeightMap2D<PointT>::setMinimumDistanceBetweenMaxima (float minimum_distance_between_maxima) 00268 { 00269 min_dist_between_maxima_ = minimum_distance_between_maxima; 00270 } 00271 00272 template <typename PointT> void 00273 pcl::people::HeightMap2D<PointT>::setSensorPortraitOrientation (bool vertical) 00274 { 00275 vertical_ = vertical; 00276 } 00277 00278 template <typename PointT> std::vector<int>& 00279 pcl::people::HeightMap2D<PointT>::getHeightMap () 00280 { 00281 return (buckets_); 00282 } 00283 00284 template <typename PointT> float 00285 pcl::people::HeightMap2D<PointT>::getBinSize () 00286 { 00287 return (bin_size_); 00288 } 00289 00290 template <typename PointT> float 00291 pcl::people::HeightMap2D<PointT>::getMinimumDistanceBetweenMaxima () 00292 { 00293 return (min_dist_between_maxima_); 00294 } 00295 00296 template <typename PointT> int& 00297 pcl::people::HeightMap2D<PointT>::getMaximaNumberAfterFiltering () 00298 { 00299 return (maxima_number_after_filtering_); 00300 } 00301 00302 template <typename PointT> std::vector<int>& 00303 pcl::people::HeightMap2D<PointT>::getMaximaCloudIndicesFiltered () 00304 { 00305 return (maxima_cloud_indices_filtered_); 00306 } 00307 00308 template <typename PointT> 00309 pcl::people::HeightMap2D<PointT>::~HeightMap2D () 00310 { 00311 // TODO Auto-generated destructor stub 00312 } 00313 #endif /* PCL_PEOPLE_HEIGHT_MAP_2D_HPP_ */