Loading...
Searching...
No Matches
grid_minimum.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2009-2012, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, Inc.
* Copyright (c) 2014, RadiantBlue Technologies, 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.
*
* $Id$
*
*/
#ifndef PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_
#define PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_
#include <pcl/common/common.h>
#include <pcl/common/io.h>
#include <pcl/common/point_tests.h> // for isXYZFinite
#include <pcl/filters/grid_minimum.h>
struct point_index_idx
{
unsigned int idx;
unsigned int cloud_point_index;
point_index_idx (unsigned int idx_, unsigned int cloud_point_index_) : idx (idx_), cloud_point_index (cloud_point_index_) {}
bool operator < (const point_index_idx &p) const { return (idx < p.idx); }
};
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::GridMinimum<PointT>::applyFilter (PointCloud &output)
{
// Has the input dataset been set already?
if (!input_)
{
PCL_WARN ("[pcl::%s::applyFilter] No input dataset given!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.clear ();
return;
}
Indices indices;
output.is_dense = true;
applyFilterIndices (indices);
pcl::copyPointCloud<PointT> (*input_, indices, output);
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::GridMinimum<PointT>::applyFilterIndices (Indices &indices)
{
indices.resize (indices_->size ());
int oii = 0;
// Get the minimum and maximum dimensions
Eigen::Vector4f min_p, max_p;
getMinMax3D<PointT> (*input_, *indices_, min_p, max_p);
// Check that the resolution is not too small, given the size of the data
std::int64_t dx = static_cast<std::int64_t> ((max_p[0] - min_p[0]) * inverse_resolution_)+1;
std::int64_t dy = static_cast<std::int64_t> ((max_p[1] - min_p[1]) * inverse_resolution_)+1;
if ((dx*dy) > static_cast<std::int64_t> (std::numeric_limits<std::int32_t>::max ()))
{
PCL_WARN ("[pcl::%s::applyFilter] Leaf size is too small for the input dataset. Integer indices would overflow.\n", getClassName ().c_str ());
return;
}
Eigen::Vector4i min_b, max_b, div_b, divb_mul;
// Compute the minimum and maximum bounding box values
min_b[0] = static_cast<int> (std::floor (min_p[0] * inverse_resolution_));
max_b[0] = static_cast<int> (std::floor (max_p[0] * inverse_resolution_));
min_b[1] = static_cast<int> (std::floor (min_p[1] * inverse_resolution_));
max_b[1] = static_cast<int> (std::floor (max_p[1] * inverse_resolution_));
// Compute the number of divisions needed along all axis
div_b = max_b - min_b + Eigen::Vector4i::Ones ();
div_b[3] = 0;
// Set up the division multiplier
divb_mul = Eigen::Vector4i (1, div_b[0], 0, 0);
std::vector<point_index_idx> index_vector;
index_vector.reserve (indices_->size ());
// First pass: go over all points and insert them into the index_vector vector
// with calculated idx. Points with the same idx value will contribute to the
// same point of resulting CloudPoint
for (const auto& index : (*indices_))
{
if (!input_->is_dense)
// Check if the point is invalid
if (!isXYZFinite ((*input_)[index]))
continue;
int ijk0 = static_cast<int> (std::floor ((*input_)[index].x * inverse_resolution_) - static_cast<float> (min_b[0]));
int ijk1 = static_cast<int> (std::floor ((*input_)[index].y * inverse_resolution_) - static_cast<float> (min_b[1]));
// Compute the grid cell index
int idx = ijk0 * divb_mul[0] + ijk1 * divb_mul[1];
index_vector.emplace_back(static_cast<unsigned int> (idx), index);
}
// Second pass: sort the index_vector vector using value representing target cell as index
// in effect all points belonging to the same output cell will be next to each other
std::sort (index_vector.begin (), index_vector.end (), std::less<> ());
// Third pass: count output cells
// we need to skip all the same, adjacenent idx values
unsigned int total = 0;
unsigned int index = 0;
// first_and_last_indices_vector[i] represents the index in index_vector of the first point in
// index_vector belonging to the voxel which corresponds to the i-th output point,
// and of the first point not belonging to.
std::vector<std::pair<unsigned int, unsigned int> > first_and_last_indices_vector;
// Worst case size
first_and_last_indices_vector.reserve (index_vector.size ());
while (index < index_vector.size ())
{
unsigned int i = index + 1;
while (i < index_vector.size () && index_vector[i].idx == index_vector[index].idx)
++i;
++total;
first_and_last_indices_vector.emplace_back(index, i);
index = i;
}
// Fourth pass: locate grid minimums
indices.resize (total);
index = 0;
for (const auto &cp : first_and_last_indices_vector)
{
unsigned int first_index = cp.first;
unsigned int last_index = cp.second;
unsigned int min_index = index_vector[first_index].cloud_point_index;
float min_z = (*input_)[index_vector[first_index].cloud_point_index].z;
for (unsigned int i = first_index + 1; i < last_index; ++i)
{
if ((*input_)[index_vector[i].cloud_point_index].z < min_z)
{
min_z = (*input_)[index_vector[i].cloud_point_index].z;
min_index = index_vector[i].cloud_point_index;
}
}
indices[index] = min_index;
++index;
}
oii = indices.size ();
// Resize the output arrays
indices.resize (oii);
}
#define PCL_INSTANTIATE_GridMinimum(T) template class PCL_EXPORTS pcl::GridMinimum<T>;
#endif // PCL_FILTERS_IMPL_VOXEL_GRID_MINIMUM_H_
pcl::GridMinimum::PointCloud
typename FilterIndices< PointT >::PointCloud PointCloud
Definition grid_minimum.h:69
pcl::GridMinimum::applyFilterIndices
void applyFilterIndices(Indices &indices)
Filtered results are indexed by an indices array.
Definition grid_minimum.hpp:81
pcl::GridMinimum::applyFilter
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a 2D grid approach.
Definition grid_minimum.hpp:61
common.h
Define standard C methods and C++ classes that are common to all methods.
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
pcl::isXYZFinite
constexpr bool isXYZFinite(const PointT &) noexcept
Definition point_tests.h:125
point_index_idx
Definition grid_minimum.hpp:51
point_index_idx::point_index_idx
point_index_idx(unsigned int idx_, unsigned int cloud_point_index_)
Definition grid_minimum.hpp:55
point_index_idx::idx
unsigned int idx
Definition grid_minimum.hpp:52
point_index_idx::operator<
bool operator<(const point_index_idx &p) const
Definition grid_minimum.hpp:56
point_index_idx::cloud_point_index
unsigned int cloud_point_index
Definition grid_minimum.hpp:53