Loading...
Searching...
No Matches
height_map_2d.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2013-, Open Perception, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* height_map_2d.hpp
* Created on: Nov 30, 2012
* Author: Matteo Munaro
*/
#include <pcl/people/height_map_2d.h>
#ifndef PCL_PEOPLE_HEIGHT_MAP_2D_HPP_
#define PCL_PEOPLE_HEIGHT_MAP_2D_HPP_
template <typename PointT>
pcl::people::HeightMap2D<PointT>::HeightMap2D ()
{
// set default values for optional parameters:
vertical_ = false;
min_dist_between_maxima_ = 0.3;
bin_size_ = 0.06;
// set flag values for mandatory parameters:
sqrt_ground_coeffs_ = std::numeric_limits<float>::quiet_NaN();
}
template <typename PointT> void
pcl::people::HeightMap2D<PointT>::compute (pcl::people::PersonCluster<PointT>& cluster)
{
// Check if all mandatory variables have been set:
if (std::isnan(sqrt_ground_coeffs_))
{
PCL_ERROR ("[pcl::people::HeightMap2D::compute] Floor parameters have not been set or they are not valid!\n");
return;
}
if (cloud_ == nullptr)
{
PCL_ERROR ("[pcl::people::HeightMap2D::compute] Input cloud has not been set!\n");
return;
}
// Reset variables:
buckets_.clear();
buckets_cloud_indices_.clear();
maxima_indices_.clear();
maxima_cloud_indices_.clear();
maxima_indices_filtered_.clear();
maxima_cloud_indices_filtered_.clear();
// Create a height map with the projection of cluster points onto the ground plane:
if (!vertical_) // camera horizontal
buckets_.resize(static_cast<std::size_t>((cluster.getMax()(0) - cluster.getMin()(0)) / bin_size_) + 1, 0);
else // camera vertical
buckets_.resize(static_cast<std::size_t>((cluster.getMax()(1) - cluster.getMin()(1)) / bin_size_) + 1, 0);
buckets_cloud_indices_.resize(buckets_.size(), 0);
for(const auto& cluster_idx : cluster.getIndices().indices)
{
PointT* p = &(*cloud_)[cluster_idx];
int index;
if (!vertical_) // camera horizontal
index = static_cast<int>((p->x - cluster.getMin()(0)) / bin_size_);
else // camera vertical
index = static_cast<int>((p->y - cluster.getMin()(1)) / bin_size_);
if (index > (static_cast<int> (buckets_.size ()) - 1))
std::cout << "Error: out of array - " << index << " of " << buckets_.size() << std::endl;
else
{
Eigen::Vector4f new_point(p->x, p->y, p->z, 1.0f); // select point from cluster
float heightp = std::fabs(new_point.dot(ground_coeffs_)); // compute point height from the groundplane
heightp /= sqrt_ground_coeffs_;
if ((heightp * 60) > buckets_[index]) // compare the height of the new point with the existing one
{
buckets_[index] = heightp * 60; // maximum height
buckets_cloud_indices_[index] = cluster_idx; // point cloud index of the point with maximum height
}
}
}
// Compute local maxima of the height map:
searchLocalMaxima();
// Filter maxima by imposing a minimum distance between them (minimum distance between people heads):
filterMaxima();
}
template <typename PointT> void
pcl::people::HeightMap2D<PointT>::searchLocalMaxima ()
{
// Search for local maxima:
maxima_number_ = 0;
int left = buckets_[0]; // current left element
float offset = 0; // used to center the maximum to the right place
maxima_indices_.resize(static_cast<std::size_t>(buckets_.size()), 0);
maxima_cloud_indices_.resize(static_cast<std::size_t>(buckets_.size()), 0);
// Handle first element:
if (buckets_[0] > buckets_[1])
{
maxima_indices_[maxima_number_] = 0;
maxima_cloud_indices_[maxima_number_] = buckets_cloud_indices_[maxima_indices_[maxima_number_]];
maxima_number_++;
}
// Main loop:
int i = 1;
while (i < (static_cast<int> (buckets_.size()) - 1))
{
int right = buckets_[i+1]; // current right element
if ((buckets_[i] > left) && (buckets_[i] > right))
{
// Search where to insert the new element (in an ordered array):
int t = 0; // position of the new element
while ((t < maxima_number_) && (buckets_[i] < buckets_[maxima_indices_[t]]))
{
t++;
}
// Move forward the smaller elements:
for (int m = maxima_number_; m > t; m--)
{
maxima_indices_[m] = maxima_indices_[m-1];
maxima_cloud_indices_[m] = maxima_cloud_indices_[m-1];
}
// Insert the new element:
maxima_indices_[t] = i - static_cast<int>(offset/2 + 0.5);
maxima_cloud_indices_[t] = buckets_cloud_indices_[maxima_indices_[t]];
left = buckets_[i+1];
i +=2;
offset = 0;
maxima_number_++;
}
else
{
if (buckets_[i] == right)
{
offset++;
}
else
{
left = buckets_[i];
offset = 0;
}
i++;
}
}
// Handle last element:
if (buckets_[buckets_.size()-1] > left)
{
// Search where to insert the new element (in an ordered array):
int t = 0; // position of the new element
while ((t < maxima_number_) && (buckets_[buckets_.size()-1] < buckets_[maxima_indices_[t]]))
{
t++;
}
// Move forward the smaller elements:
for (int m = maxima_number_; m > t; m--)
{
maxima_indices_[m] = maxima_indices_[m-1];
maxima_cloud_indices_[m] = maxima_cloud_indices_[m-1];
}
// Insert the new element:
maxima_indices_[t] = i - static_cast<int>(offset/2 + 0.5);
maxima_cloud_indices_[t] = buckets_cloud_indices_[maxima_indices_[t]];
maxima_number_++;
}
}
template <typename PointT> void
pcl::people::HeightMap2D<PointT>::filterMaxima ()
{
// Filter maxima according to their distance when projected on the ground plane:
maxima_number_after_filtering_ = 0;
maxima_indices_filtered_.resize(maxima_number_, 0);
maxima_cloud_indices_filtered_.resize(maxima_number_, 0);
if (maxima_number_ > 0)
{
for (int i = 0; i < maxima_number_; i++)
{
bool good_maximum = true;
PointT* p_current = &(*cloud_)[maxima_cloud_indices_[i]]; // pointcloud point referring to the current maximum
Eigen::Vector3f p_current_eigen(p_current->x, p_current->y, p_current->z); // conversion to eigen
float t = p_current_eigen.dot(ground_coeffs_.head(3)) / std::pow(sqrt_ground_coeffs_, 2); // height from the ground
p_current_eigen -= ground_coeffs_.head(3) * t; // projection of the point on the groundplane
int j = i-1;
while ((j >= 0) && (good_maximum))
{
PointT* p_previous = &(*cloud_)[maxima_cloud_indices_[j]]; // pointcloud point referring to an already validated maximum
Eigen::Vector3f p_previous_eigen(p_previous->x, p_previous->y, p_previous->z); // conversion to eigen
float t = p_previous_eigen.dot(ground_coeffs_.head(3)) / std::pow(sqrt_ground_coeffs_, 2); // height from the ground
p_previous_eigen -= ground_coeffs_.head(3) * t; // projection of the point on the groundplane
// distance of the projection of the points on the groundplane:
float distance = (p_current_eigen-p_previous_eigen).norm();
if (distance < min_dist_between_maxima_)
{
good_maximum = false;
}
j--;
}
if (good_maximum)
{
maxima_indices_filtered_[maxima_number_after_filtering_] = maxima_indices_[i];
maxima_cloud_indices_filtered_[maxima_number_after_filtering_] = maxima_cloud_indices_[i];
maxima_number_after_filtering_++;
}
}
}
}
template <typename PointT> void
pcl::people::HeightMap2D<PointT>::setInputCloud (PointCloudPtr& cloud)
{
cloud_ = cloud;
}
template <typename PointT>
void pcl::people::HeightMap2D<PointT>::setGround(Eigen::VectorXf& ground_coeffs)
{
ground_coeffs_ = ground_coeffs;
sqrt_ground_coeffs_ = (ground_coeffs - Eigen::Vector4f(0.0f, 0.0f, 0.0f, ground_coeffs(3))).norm();
}
template <typename PointT> void
pcl::people::HeightMap2D<PointT>::setBinSize (float bin_size)
{
bin_size_ = bin_size;
}
template <typename PointT> void
pcl::people::HeightMap2D<PointT>::setMinimumDistanceBetweenMaxima (float minimum_distance_between_maxima)
{
min_dist_between_maxima_ = minimum_distance_between_maxima;
}
template <typename PointT> void
pcl::people::HeightMap2D<PointT>::setSensorPortraitOrientation (bool vertical)
{
vertical_ = vertical;
}
template <typename PointT> std::vector<int>&
pcl::people::HeightMap2D<PointT>::getHeightMap ()
{
return (buckets_);
}
template <typename PointT> float
pcl::people::HeightMap2D<PointT>::getBinSize ()
{
return (bin_size_);
}
template <typename PointT> float
pcl::people::HeightMap2D<PointT>::getMinimumDistanceBetweenMaxima ()
{
return (min_dist_between_maxima_);
}
template <typename PointT> int&
pcl::people::HeightMap2D<PointT>::getMaximaNumberAfterFiltering ()
{
return (maxima_number_after_filtering_);
}
template <typename PointT> std::vector<int>&
pcl::people::HeightMap2D<PointT>::getMaximaCloudIndicesFiltered ()
{
return (maxima_cloud_indices_filtered_);
}
template <typename PointT>
pcl::people::HeightMap2D<PointT>::~HeightMap2D () = default;
#endif /* PCL_PEOPLE_HEIGHT_MAP_2D_HPP_ */
pcl::people::HeightMap2D::filterMaxima
void filterMaxima()
Filter maxima of the height map by imposing a minimum distance between them.
Definition height_map_2d.hpp:202
pcl::people::HeightMap2D::HeightMap2D
HeightMap2D()
Constructor.
Definition height_map_2d.hpp:47
pcl::people::HeightMap2D::searchLocalMaxima
void searchLocalMaxima()
Compute local maxima of the height map.
Definition height_map_2d.hpp:119
pcl::people::HeightMap2D::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition height_map_2d.h:62
pcl::people::HeightMap2D::getMaximaCloudIndicesFiltered
std::vector< int > & getMaximaCloudIndicesFiltered()
Return the point cloud indices corresponding to the maxima computed after the filterMaxima method.
Definition height_map_2d.hpp:301
pcl::people::HeightMap2D::setBinSize
void setBinSize(float bin_size)
Set bin size for the height map.
Definition height_map_2d.hpp:259
pcl::people::HeightMap2D::getBinSize
float getBinSize()
Get bin size for the height map.
Definition height_map_2d.hpp:283
pcl::people::HeightMap2D::setInputCloud
void setInputCloud(PointCloudPtr &cloud)
Set initial cluster indices.
Definition height_map_2d.hpp:246
pcl::people::HeightMap2D::setSensorPortraitOrientation
void setSensorPortraitOrientation(bool vertical)
Set sensor orientation to landscape mode (false) or portrait mode (true).
Definition height_map_2d.hpp:271
pcl::people::HeightMap2D::getMaximaNumberAfterFiltering
int & getMaximaNumberAfterFiltering()
Return the maxima number after the filterMaxima method.
Definition height_map_2d.hpp:295
pcl::people::HeightMap2D::setGround
void setGround(Eigen::VectorXf &ground_coeffs)
Set the ground coefficients.
Definition height_map_2d.hpp:252
pcl::people::HeightMap2D::~HeightMap2D
virtual ~HeightMap2D()
Destructor.
pcl::people::HeightMap2D::getHeightMap
std::vector< int > & getHeightMap()
Get the height map as a vector of int.
Definition height_map_2d.hpp:277
pcl::people::HeightMap2D::compute
void compute(pcl::people::PersonCluster< PointT > &cluster)
Compute the height map with the projection of cluster points onto the ground plane.
Definition height_map_2d.hpp:59
pcl::people::HeightMap2D::getMinimumDistanceBetweenMaxima
float getMinimumDistanceBetweenMaxima()
Get minimum distance between maxima of the height map.
Definition height_map_2d.hpp:289
pcl::people::HeightMap2D::setMinimumDistanceBetweenMaxima
void setMinimumDistanceBetweenMaxima(float minimum_distance_between_maxima)
Set minimum distance between maxima.
Definition height_map_2d.hpp:265
pcl::people::PersonCluster
PersonCluster represents a class for representing information about a cluster containing a person.
Definition person_cluster.h:59
pcl::people::PersonCluster::getIndices
pcl::PointIndices & getIndices()
Returns the indices of the point cloud points corresponding to the cluster.
Definition person_cluster.hpp:255
pcl::people::PersonCluster::getMin
Eigen::Vector3f & getMin()
Returns the point formed by min x, min y and min z.
Definition person_cluster.hpp:331
pcl::people::PersonCluster::getMax
Eigen::Vector3f & getMax()
Returns the point formed by max x, max y and max z.
Definition person_cluster.hpp:337
pcl::PointIndices::indices
Indices indices
Definition PointIndices.h:20
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition point_types.hpp:623