Point Cloud Library (PCL)  1.14.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(static_cast<std::size_t>((cluster.getMax()(0) - cluster.getMin()(0)) / bin_size_) + 1, 0);
84  else // camera vertical
85  buckets_.resize(static_cast<std::size_t>((cluster.getMax()(1) - cluster.getMin()(1)) / bin_size_) + 1, 0);
86  buckets_cloud_indices_.resize(buckets_.size(), 0);
87 
88  for(const auto& cluster_idx : cluster.getIndices().indices)
89  {
90  PointT* p = &(*cloud_)[cluster_idx];
91  int index;
92  if (!vertical_) // camera horizontal
93  index = static_cast<int>((p->x - cluster.getMin()(0)) / bin_size_);
94  else // camera vertical
95  index = static_cast<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] = cluster_idx; // 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  float offset = 0; // used to center the maximum to the right place
125  maxima_indices_.resize(static_cast<std::size_t>(buckets_.size()), 0);
126  maxima_cloud_indices_.resize(static_cast<std::size_t>(buckets_.size()), 0);
127 
128  // Handle first element:
129  if (buckets_[0] > buckets_[1])
130  {
131  maxima_indices_[maxima_number_] = 0;
132  maxima_cloud_indices_[maxima_number_] = buckets_cloud_indices_[maxima_indices_[maxima_number_]];
133  maxima_number_++;
134  }
135 
136  // Main loop:
137  int i = 1;
138  while (i < (static_cast<int> (buckets_.size()) - 1))
139  {
140  int right = buckets_[i+1]; // current right element
141  if ((buckets_[i] > left) && (buckets_[i] > right))
142  {
143  // Search where to insert the new element (in an ordered array):
144  int t = 0; // position of the new element
145  while ((t < maxima_number_) && (buckets_[i] < buckets_[maxima_indices_[t]]))
146  {
147  t++;
148  }
149  // Move forward the smaller elements:
150  for (int m = maxima_number_; m > t; m--)
151  {
152  maxima_indices_[m] = maxima_indices_[m-1];
153  maxima_cloud_indices_[m] = maxima_cloud_indices_[m-1];
154  }
155  // Insert the new element:
156  maxima_indices_[t] = i - static_cast<int>(offset/2 + 0.5);
157  maxima_cloud_indices_[t] = buckets_cloud_indices_[maxima_indices_[t]];
158  left = buckets_[i+1];
159  i +=2;
160  offset = 0;
161  maxima_number_++;
162  }
163  else
164  {
165  if (buckets_[i] == right)
166  {
167  offset++;
168  }
169  else
170  {
171  left = buckets_[i];
172  offset = 0;
173  }
174  i++;
175  }
176  }
177 
178  // Handle last element:
179  if (buckets_[buckets_.size()-1] > left)
180  {
181  // Search where to insert the new element (in an ordered array):
182  int t = 0; // position of the new element
183  while ((t < maxima_number_) && (buckets_[buckets_.size()-1] < buckets_[maxima_indices_[t]]))
184  {
185  t++;
186  }
187  // Move forward the smaller elements:
188  for (int m = maxima_number_; m > t; m--)
189  {
190  maxima_indices_[m] = maxima_indices_[m-1];
191  maxima_cloud_indices_[m] = maxima_cloud_indices_[m-1];
192  }
193  // Insert the new element:
194  maxima_indices_[t] = i - static_cast<int>(offset/2 + 0.5);
195  maxima_cloud_indices_[t] = buckets_cloud_indices_[maxima_indices_[t]];
196 
197  maxima_number_++;
198  }
199 }
200 
201 template <typename PointT> void
203 {
204  // Filter maxima according to their distance when projected on the ground plane:
205  maxima_number_after_filtering_ = 0;
206  maxima_indices_filtered_.resize(maxima_number_, 0);
207  maxima_cloud_indices_filtered_.resize(maxima_number_, 0);
208  if (maxima_number_ > 0)
209  {
210  for (int i = 0; i < maxima_number_; i++)
211  {
212  bool good_maximum = true;
213 
214  PointT* p_current = &(*cloud_)[maxima_cloud_indices_[i]]; // pointcloud point referring to the current maximum
215  Eigen::Vector3f p_current_eigen(p_current->x, p_current->y, p_current->z); // conversion to eigen
216  float t = p_current_eigen.dot(ground_coeffs_.head<3>()) / std::pow(sqrt_ground_coeffs_, 2); // height from the ground
217  p_current_eigen -= ground_coeffs_.head<3>() * t; // projection of the point on the groundplane
218 
219  int j = i-1;
220  while ((j >= 0) && (good_maximum))
221  {
222  PointT* p_previous = &(*cloud_)[maxima_cloud_indices_[j]]; // pointcloud point referring to an already validated maximum
223  Eigen::Vector3f p_previous_eigen(p_previous->x, p_previous->y, p_previous->z); // conversion to eigen
224  float t = p_previous_eigen.dot(ground_coeffs_.head<3>()) / std::pow(sqrt_ground_coeffs_, 2); // height from the ground
225  p_previous_eigen -= ground_coeffs_.head<3>() * t; // projection of the point on the groundplane
226 
227  // distance of the projection of the points on the groundplane:
228  float distance = (p_current_eigen-p_previous_eigen).norm();
229  if (distance < min_dist_between_maxima_)
230  {
231  good_maximum = false;
232  }
233  j--;
234  }
235  if (good_maximum)
236  {
237  maxima_indices_filtered_[maxima_number_after_filtering_] = maxima_indices_[i];
238  maxima_cloud_indices_filtered_[maxima_number_after_filtering_] = maxima_cloud_indices_[i];
239  maxima_number_after_filtering_++;
240  }
241  }
242  }
243 }
244 
245 template <typename PointT> void
247 {
248  cloud_ = cloud;
249 }
250 
251 template <typename PointT>
252 void pcl::people::HeightMap2D<PointT>::setGround(Eigen::VectorXf& ground_coeffs)
253 {
254  ground_coeffs_ = ground_coeffs;
255  sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm();
256 }
257 
258 template <typename PointT> void
260 {
261  bin_size_ = bin_size;
262 }
263 
264 template <typename PointT> void
266 {
267  min_dist_between_maxima_ = minimum_distance_between_maxima;
268 }
269 
270 template <typename PointT> void
272 {
273  vertical_ = vertical;
274 }
275 
276 template <typename PointT> std::vector<int>&
278 {
279  return (buckets_);
280 }
281 
282 template <typename PointT> float
284 {
285  return (bin_size_);
286 }
287 
288 template <typename PointT> float
290 {
291  return (min_dist_between_maxima_);
292 }
293 
294 template <typename PointT> int&
296 {
297  return (maxima_number_after_filtering_);
298 }
299 
300 template <typename PointT> std::vector<int>&
302 {
303  return (maxima_cloud_indices_filtered_);
304 }
305 
306 template <typename PointT>
308 #endif /* PCL_PEOPLE_HEIGHT_MAP_2D_HPP_ */
void filterMaxima()
Filter maxima of the height map by imposing a minimum distance between them.
void searchLocalMaxima()
Compute local maxima of the height map.
typename PointCloud::Ptr PointCloudPtr
Definition: height_map_2d.h:62
std::vector< int > & getMaximaCloudIndicesFiltered()
Return the point cloud indices corresponding to the maxima computed after the filterMaxima method.
void setBinSize(float bin_size)
Set bin size for the height map.
float getBinSize()
Get bin size for the height map.
void setInputCloud(PointCloudPtr &cloud)
Set initial cluster indices.
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
int & getMaximaNumberAfterFiltering()
Return the maxima number after the filterMaxima method.
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
virtual ~HeightMap2D()
Destructor.
std::vector< int > & getHeightMap()
Get the height map as a vector of int.
void compute(pcl::people::PersonCluster< PointT > &cluster)
Compute the height map with the projection of cluster points onto the ground plane.
float getMinimumDistanceBetweenMaxima()
Get minimum distance between maxima of the height map.
void setMinimumDistanceBetweenMaxima(float minimum_distance_between_maxima)
Set minimum distance between maxima.
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.
Eigen::Vector3f & getMin()
Returns the point formed by min x, min y and min z.
Eigen::Vector3f & getMax()
Returns the point formed by max x, max y and max z.
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
A point structure representing Euclidean xyz coordinates, and the RGB color.