41 #include <pcl/people/height_map_2d.h>
43 #ifndef PCL_PEOPLE_HEIGHT_MAP_2D_HPP_
44 #define PCL_PEOPLE_HEIGHT_MAP_2D_HPP_
46 template <
typename Po
intT>
51 min_dist_between_maxima_ = 0.3;
55 sqrt_ground_coeffs_ = std::numeric_limits<float>::quiet_NaN();
58 template <
typename Po
intT>
void
62 if (std::isnan(sqrt_ground_coeffs_))
64 PCL_ERROR (
"[pcl::people::HeightMap2D::compute] Floor parameters have not been set or they are not valid!\n");
67 if (cloud_ ==
nullptr)
69 PCL_ERROR (
"[pcl::people::HeightMap2D::compute] Input cloud has not been set!\n");
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();
83 buckets_.resize(std::size_t((cluster.
getMax()(0) - cluster.
getMin()(0)) / bin_size_) + 1, 0);
85 buckets_.resize(std::size_t((cluster.
getMax()(1) - cluster.
getMin()(1)) / bin_size_) + 1, 0);
86 buckets_cloud_indices_.resize(buckets_.size(), 0);
90 PointT* p = &cloud_->points[*pit];
93 index = int((p->x - cluster.
getMin()(0)) / bin_size_);
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;
100 Eigen::Vector4f new_point(p->x, p->y, p->z, 1.0f);
101 float heightp = std::fabs(new_point.dot(ground_coeffs_));
102 heightp /= sqrt_ground_coeffs_;
103 if ((heightp * 60) > buckets_[index])
105 buckets_[index] = heightp * 60;
106 buckets_cloud_indices_[index] = *pit;
118 template <
typename Po
intT>
void
123 int left = buckets_[0];
126 maxima_indices_.resize(std::size_t(buckets_.size()), 0);
127 maxima_cloud_indices_.resize(std::size_t(buckets_.size()), 0);
130 if (buckets_[0] > buckets_[1])
132 maxima_indices_[maxima_number_] = 0;
133 maxima_cloud_indices_[maxima_number_] = buckets_cloud_indices_[maxima_indices_[maxima_number_]];
139 while (i < (static_cast<int> (buckets_.size()) - 1))
141 right = buckets_[i+1];
142 if ((buckets_[i] > left) && (buckets_[i] > right))
146 while ((t < maxima_number_) && (buckets_[i] < buckets_[maxima_indices_[t]]))
151 for (
int m = maxima_number_; m > t; m--)
153 maxima_indices_[m] = maxima_indices_[m-1];
154 maxima_cloud_indices_[m] = maxima_cloud_indices_[m-1];
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];
166 if (buckets_[i] == right)
180 if (buckets_[buckets_.size()-1] > left)
184 while ((t < maxima_number_) && (buckets_[buckets_.size()-1] < buckets_[maxima_indices_[t]]))
189 for (
int m = maxima_number_; m > t; m--)
191 maxima_indices_[m] = maxima_indices_[m-1];
192 maxima_cloud_indices_[m] = maxima_cloud_indices_[m-1];
195 maxima_indices_[t] = i - int(offset/2 + 0.5);
196 maxima_cloud_indices_[t] = buckets_cloud_indices_[maxima_indices_[t]];
202 template <
typename Po
intT>
void
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)
211 for (
int i = 0; i < maxima_number_; i++)
213 bool good_maximum =
true;
216 PointT* p_current = &cloud_->points[maxima_cloud_indices_[i]];
217 Eigen::Vector3f p_current_eigen(p_current->x, p_current->y, p_current->z);
218 float t = p_current_eigen.dot(ground_coeffs_.head(3)) / std::pow(sqrt_ground_coeffs_, 2);
219 p_current_eigen -= ground_coeffs_.head(3) * t;
222 while ((j >= 0) && (good_maximum))
224 PointT* p_previous = &cloud_->points[maxima_cloud_indices_[j]];
225 Eigen::Vector3f p_previous_eigen(p_previous->x, p_previous->y, p_previous->z);
226 float t = p_previous_eigen.dot(ground_coeffs_.head(3)) / std::pow(sqrt_ground_coeffs_, 2);
227 p_previous_eigen -= ground_coeffs_.head(3) * t;
230 distance = (p_current_eigen-p_previous_eigen).norm();
231 if (
distance < min_dist_between_maxima_)
233 good_maximum =
false;
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_++;
247 template <
typename Po
intT>
void
253 template <
typename Po
intT>
256 ground_coeffs_ = ground_coeffs;
257 sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm();
260 template <
typename Po
intT>
void
263 bin_size_ = bin_size;
266 template <
typename Po
intT>
void
269 min_dist_between_maxima_ = minimum_distance_between_maxima;
272 template <
typename Po
intT>
void
275 vertical_ = vertical;
278 template <
typename Po
intT> std::vector<int>&
284 template <
typename Po
intT>
float
290 template <
typename Po
intT>
float
293 return (min_dist_between_maxima_);
296 template <
typename Po
intT>
int&
299 return (maxima_number_after_filtering_);
302 template <
typename Po
intT> std::vector<int>&
305 return (maxima_cloud_indices_filtered_);
308 template <
typename Po
intT>