Point Cloud Library (PCL) 1.13.0
Loading...
Searching...
No Matches
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
46template <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
58template <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(std::size_t((cluster.getMax()(0) - cluster.getMin()(0)) / bin_size_) + 1, 0);
84 else // camera vertical
85 buckets_.resize(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 = 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] = 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
118template <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(std::size_t(buckets_.size()), 0);
126 maxima_cloud_indices_.resize(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 - 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 - int(offset/2 + 0.5);
195 maxima_cloud_indices_[t] = buckets_cloud_indices_[maxima_indices_[t]];
196
197 maxima_number_++;
198 }
199}
200
201template <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
245template <typename PointT> void
250
251template <typename PointT>
252void 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
258template <typename PointT> void
260{
261 bin_size_ = bin_size;
262}
263
264template <typename PointT> void
266{
267 min_dist_between_maxima_ = minimum_distance_between_maxima;
268}
269
270template <typename PointT> void
272{
273 vertical_ = vertical;
274}
275
276template <typename PointT> std::vector<int>&
278{
279 return (buckets_);
280}
281
282template <typename PointT> float
284{
285 return (bin_size_);
286}
287
288template <typename PointT> float
290{
291 return (min_dist_between_maxima_);
292}
293
294template <typename PointT> int&
296{
297 return (maxima_number_after_filtering_);
298}
299
300template <typename PointT> std::vector<int>&
302{
303 return (maxima_cloud_indices_filtered_);
304}
305
306template <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
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.
A point structure representing Euclidean xyz coordinates, and the RGB color.