Point Cloud Library (PCL)  1.9.1-dev
height_map_2d.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2013-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * height_map_2d.hpp
37  * Created on: Nov 30, 2012
38  * Author: Matteo Munaro
39  */
40 
41 #include <pcl/people/height_map_2d.h>
42 
43 #ifndef PCL_PEOPLE_HEIGHT_MAP_2D_HPP_
44 #define PCL_PEOPLE_HEIGHT_MAP_2D_HPP_
45 
46 template <typename PointT>
48 {
49  // set default values for optional parameters:
50  vertical_ = false;
51  min_dist_between_maxima_ = 0.3;
52  bin_size_ = 0.06;
53 
54  // set flag values for mandatory parameters:
55  sqrt_ground_coeffs_ = std::numeric_limits<float>::quiet_NaN();
56 }
57 
58 template <typename PointT> void
60 {
61  // Check if all mandatory variables have been set:
62  if (std::isnan(sqrt_ground_coeffs_))
63  {
64  PCL_ERROR ("[pcl::people::HeightMap2D::compute] Floor parameters have not been set or they are not valid!\n");
65  return;
66  }
67  if (cloud_ == nullptr)
68  {
69  PCL_ERROR ("[pcl::people::HeightMap2D::compute] Input cloud has not been set!\n");
70  return;
71  }
72 
73  // Reset variables:
74  buckets_.clear();
75  buckets_cloud_indices_.clear();
76  maxima_indices_.clear();
77  maxima_cloud_indices_.clear();
78  maxima_indices_filtered_.clear();
79  maxima_cloud_indices_filtered_.clear();
80 
81  // Create a height map with the projection of cluster points onto the ground plane:
82  if (!vertical_) // camera horizontal
83  buckets_.resize(size_t((cluster.getMax()(0) - cluster.getMin()(0)) / bin_size_) + 1, 0);
84  else // camera vertical
85  buckets_.resize(size_t((cluster.getMax()(1) - cluster.getMin()(1)) / bin_size_) + 1, 0);
86  buckets_cloud_indices_.resize(buckets_.size(), 0);
87 
88  for(std::vector<int>::const_iterator pit = cluster.getIndices().indices.begin(); pit != cluster.getIndices().indices.end(); pit++)
89  {
90  PointT* p = &cloud_->points[*pit];
91  int index;
92  if (!vertical_) // camera horizontal
93  index = int((p->x - cluster.getMin()(0)) / bin_size_);
94  else // camera vertical
95  index = int((p->y - cluster.getMin()(1)) / bin_size_);
96  if (index > (static_cast<int> (buckets_.size ()) - 1))
97  std::cout << "Error: out of array - " << index << " of " << buckets_.size() << std::endl;
98  else
99  {
100  Eigen::Vector4f new_point(p->x, p->y, p->z, 1.0f); // select point from cluster
101  float heightp = std::fabs(new_point.dot(ground_coeffs_)); // compute point height from the groundplane
102  heightp /= sqrt_ground_coeffs_;
103  if ((heightp * 60) > buckets_[index]) // compare the height of the new point with the existing one
104  {
105  buckets_[index] = heightp * 60; // maximum height
106  buckets_cloud_indices_[index] = *pit; // point cloud index of the point with maximum height
107  }
108  }
109  }
110 
111  // Compute local maxima of the height map:
112  searchLocalMaxima();
113 
114  // Filter maxima by imposing a minimum distance between them (minimum distance between people heads):
115  filterMaxima();
116 }
117 
118 template <typename PointT> void
120 {
121  // Search for local maxima:
122  maxima_number_ = 0;
123  int left = buckets_[0]; // current left element
124  int right = 0; // current right element
125  float offset = 0; // used to center the maximum to the right place
126  maxima_indices_.resize(size_t(buckets_.size()), 0);
127  maxima_cloud_indices_.resize(size_t(buckets_.size()), 0);
128 
129  // Handle first element:
130  if (buckets_[0] > buckets_[1])
131  {
132  maxima_indices_[maxima_number_] = 0;
133  maxima_cloud_indices_[maxima_number_] = buckets_cloud_indices_[maxima_indices_[maxima_number_]];
134  maxima_number_++;
135  }
136 
137  // Main loop:
138  int i = 1;
139  while (i < (static_cast<int> (buckets_.size()) - 1))
140  {
141  right = buckets_[i+1];
142  if ((buckets_[i] > left) && (buckets_[i] > right))
143  {
144  // Search where to insert the new element (in an ordered array):
145  int t = 0; // position of the new element
146  while ((t < maxima_number_) && (buckets_[i] < buckets_[maxima_indices_[t]]))
147  {
148  t++;
149  }
150  // Move forward the smaller elements:
151  for (int m = maxima_number_; m > t; m--)
152  {
153  maxima_indices_[m] = maxima_indices_[m-1];
154  maxima_cloud_indices_[m] = maxima_cloud_indices_[m-1];
155  }
156  // Insert the new element:
157  maxima_indices_[t] = i - int(offset/2 + 0.5);
158  maxima_cloud_indices_[t] = buckets_cloud_indices_[maxima_indices_[t]];
159  left = buckets_[i+1];
160  i +=2;
161  offset = 0;
162  maxima_number_++;
163  }
164  else
165  {
166  if (buckets_[i] == right)
167  {
168  offset++;
169  }
170  else
171  {
172  left = buckets_[i];
173  offset = 0;
174  }
175  i++;
176  }
177  }
178 
179  // Handle last element:
180  if (buckets_[buckets_.size()-1] > left)
181  {
182  // Search where to insert the new element (in an ordered array):
183  int t = 0; // position of the new element
184  while ((t < maxima_number_) && (buckets_[buckets_.size()-1] < buckets_[maxima_indices_[t]]))
185  {
186  t++;
187  }
188  // Move forward the smaller elements:
189  for (int m = maxima_number_; m > t; m--)
190  {
191  maxima_indices_[m] = maxima_indices_[m-1];
192  maxima_cloud_indices_[m] = maxima_cloud_indices_[m-1];
193  }
194  // Insert the new element:
195  maxima_indices_[t] = i - int(offset/2 + 0.5);
196  maxima_cloud_indices_[t] = buckets_cloud_indices_[maxima_indices_[t]];
197 
198  maxima_number_++;
199  }
200 }
201 
202 template <typename PointT> void
204 {
205  // Filter maxima according to their distance when projected on the ground plane:
206  maxima_number_after_filtering_ = 0;
207  maxima_indices_filtered_.resize(maxima_number_, 0);
208  maxima_cloud_indices_filtered_.resize(maxima_number_, 0);
209  if (maxima_number_ > 0)
210  {
211  for (int i = 0; i < maxima_number_; i++)
212  {
213  bool good_maximum = true;
214  float distance = 0;
215 
216  PointT* p_current = &cloud_->points[maxima_cloud_indices_[i]]; // pointcloud point referring to the current maximum
217  Eigen::Vector3f p_current_eigen(p_current->x, p_current->y, p_current->z); // conversion to eigen
218  float t = p_current_eigen.dot(ground_coeffs_.head(3)) / std::pow(sqrt_ground_coeffs_, 2); // height from the ground
219  p_current_eigen -= ground_coeffs_.head(3) * t; // projection of the point on the groundplane
220 
221  int j = i-1;
222  while ((j >= 0) && (good_maximum))
223  {
224  PointT* p_previous = &cloud_->points[maxima_cloud_indices_[j]]; // pointcloud point referring to an already validated maximum
225  Eigen::Vector3f p_previous_eigen(p_previous->x, p_previous->y, p_previous->z); // conversion to eigen
226  float t = p_previous_eigen.dot(ground_coeffs_.head(3)) / std::pow(sqrt_ground_coeffs_, 2); // height from the ground
227  p_previous_eigen -= ground_coeffs_.head(3) * t; // projection of the point on the groundplane
228 
229  // distance of the projection of the points on the groundplane:
230  distance = (p_current_eigen-p_previous_eigen).norm();
231  if (distance < min_dist_between_maxima_)
232  {
233  good_maximum = false;
234  }
235  j--;
236  }
237  if (good_maximum)
238  {
239  maxima_indices_filtered_[maxima_number_after_filtering_] = maxima_indices_[i];
240  maxima_cloud_indices_filtered_[maxima_number_after_filtering_] = maxima_cloud_indices_[i];
241  maxima_number_after_filtering_++;
242  }
243  }
244  }
245 }
246 
247 template <typename PointT> void
249 {
250  cloud_ = cloud;
251 }
252 
253 template <typename PointT>
254 void pcl::people::HeightMap2D<PointT>::setGround(Eigen::VectorXf& ground_coeffs)
255 {
256  ground_coeffs_ = ground_coeffs;
257  sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm();
258 }
259 
260 template <typename PointT> void
262 {
263  bin_size_ = bin_size;
264 }
265 
266 template <typename PointT> void
268 {
269  min_dist_between_maxima_ = minimum_distance_between_maxima;
270 }
271 
272 template <typename PointT> void
274 {
275  vertical_ = vertical;
276 }
277 
278 template <typename PointT> std::vector<int>&
280 {
281  return (buckets_);
282 }
283 
284 template <typename PointT> float
286 {
287  return (bin_size_);
288 }
289 
290 template <typename PointT> float
292 {
293  return (min_dist_between_maxima_);
294 }
295 
296 template <typename PointT> int&
298 {
299  return (maxima_number_after_filtering_);
300 }
301 
302 template <typename PointT> std::vector<int>&
304 {
305  return (maxima_cloud_indices_filtered_);
306 }
307 
308 template <typename PointT>
310 {
311  // TODO Auto-generated destructor stub
312 }
313 #endif /* PCL_PEOPLE_HEIGHT_MAP_2D_HPP_ */
int & getMaximaNumberAfterFiltering()
Return the maxima number after the filterMaxima method.
Eigen::Vector3f & getMax()
Returns the point formed by max x, max y and max z.
HeightMap2D()
Constructor.
void compute(pcl::people::PersonCluster< PointT > &cluster)
Compute the height map with the projection of cluster points onto the ground plane.
virtual ~HeightMap2D()
Destructor.
std::vector< int > indices
Definition: PointIndices.h:19
void setBinSize(float bin_size)
Set bin size for the height map.
float getBinSize()
Get bin size for the height map.
boost::shared_ptr< PointCloud > PointCloudPtr
Definition: height_map_2d.h:62
PersonCluster represents a class for representing information about a cluster containing a person...
void searchLocalMaxima()
Compute local maxima of the height map.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
std::vector< int > & getHeightMap()
Get the height map as a vector of int.
std::vector< int > & getMaximaCloudIndicesFiltered()
Return the point cloud indices corresponding to the maxima computed after the filterMaxima method...
float getMinimumDistanceBetweenMaxima()
Get minimum distance between maxima of the height map.
void setInputCloud(PointCloudPtr &cloud)
Set initial cluster indices.
void setMinimumDistanceBetweenMaxima(float minimum_distance_between_maxima)
Set minimum distance between maxima.
A point structure representing Euclidean xyz coordinates, and the RGB color.
Eigen::Vector3f & getMin()
Returns the point formed by min x, min y and min z.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
void filterMaxima()
Filter maxima of the height map by imposing a minimum distance between them.
pcl::PointIndices & getIndices()
Returns the indices of the point cloud points corresponding to the cluster.