Loading...
Searching...
No Matches
approximate_voxel_grid.h
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, 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: voxel_grid.h 1374 2011-06-19 02:29:56Z bouffa $
*
*/
#pragma once
#include <pcl/filters/filter.h>
namespace pcl
{
/** \brief Helper functor structure for copying data between an Eigen::VectorXf and a PointT. */
template <typename PointT>
struct xNdCopyEigenPointFunctor
{
using Pod = typename traits::POD<PointT>::type;
xNdCopyEigenPointFunctor (const Eigen::VectorXf &p1, PointT &p2)
: p1_ (p1),
p2_ (reinterpret_cast<Pod&>(p2))
{ }
template<typename Key> inline void operator() ()
{
//boost::fusion::at_key<Key> (p2_) = p1_[f_idx_++];
using T = typename pcl::traits::datatype<PointT, Key>::type;
std::uint8_t* data_ptr = reinterpret_cast<std::uint8_t*>(&p2_) + pcl::traits::offset<PointT, Key>::value;
*reinterpret_cast<T*>(data_ptr) = static_cast<T> (p1_[f_idx_++]);
}
private:
const Eigen::VectorXf &p1_;
Pod &p2_;
int f_idx_{0};
};
/** \brief Helper functor structure for copying data between an Eigen::VectorXf and a PointT. */
template <typename PointT>
struct xNdCopyPointEigenFunctor
{
using Pod = typename traits::POD<PointT>::type;
xNdCopyPointEigenFunctor (const PointT &p1, Eigen::VectorXf &p2)
: p1_ (reinterpret_cast<const Pod&>(p1)), p2_ (p2) { }
template<typename Key> inline void operator() ()
{
//p2_[f_idx_++] = boost::fusion::at_key<Key> (p1_);
using T = typename pcl::traits::datatype<PointT, Key>::type;
const std::uint8_t* data_ptr = reinterpret_cast<const std::uint8_t*>(&p1_) + pcl::traits::offset<PointT, Key>::value;
p2_[f_idx_++] = static_cast<float> (*reinterpret_cast<const T*>(data_ptr));
}
private:
const Pod &p1_;
Eigen::VectorXf &p2_;
int f_idx_{0};
};
/** \brief ApproximateVoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data.
* Thus, this is similar to the \ref VoxelGrid filter.
* This class works best if points that are stored in memory next to each other (in the input point cloud), are also somewhat close in 3D euclidean space (this is for example usually the case for organized point clouds). If the points have no storage order (e.g. in synthetic, randomly generated point clouds), this class will give very poor results, and \ref VoxelGrid should be used instead.
* \sa VoxelGrid
* \author James Bowman, Radu B. Rusu
* \ingroup filters
*/
template <typename PointT>
class ApproximateVoxelGrid: public Filter<PointT>
{
using Filter<PointT>::filter_name_;
using Filter<PointT>::getClassName;
using Filter<PointT>::input_;
using Filter<PointT>::indices_;
using PointCloud = typename Filter<PointT>::PointCloud;
using PointCloudPtr = typename PointCloud::Ptr;
using PointCloudConstPtr = typename PointCloud::ConstPtr;
private:
struct he
{
he () = default;
int ix{0}, iy{0}, iz{0};
int count{0};
Eigen::VectorXf centroid;
};
public:
using Ptr = shared_ptr<ApproximateVoxelGrid<PointT> >;
using ConstPtr = shared_ptr<const ApproximateVoxelGrid<PointT> >;
/** \brief Empty constructor. */
ApproximateVoxelGrid () :
pcl::Filter<PointT> (),
leaf_size_ (Eigen::Vector3f::Ones ()),
inverse_leaf_size_ (Eigen::Array3f::Ones ()),
history_ (new he[histsize_])
{
filter_name_ = "ApproximateVoxelGrid";
}
/** \brief Copy constructor.
* \param[in] src the approximate voxel grid to copy into this.
*/
ApproximateVoxelGrid (const ApproximateVoxelGrid &src) :
pcl::Filter<PointT> (),
leaf_size_ (src.leaf_size_),
inverse_leaf_size_ (src.inverse_leaf_size_),
downsample_all_data_ (src.downsample_all_data_),
histsize_ (src.histsize_),
history_ ()
{
history_ = new he[histsize_];
for (std::size_t i = 0; i < histsize_; i++)
history_[i] = src.history_[i];
}
/** \brief Destructor.
*/
~ApproximateVoxelGrid () override
{
delete [] history_;
}
/** \brief Copy operator.
* \param[in] src the approximate voxel grid to copy into this.
*/
inline ApproximateVoxelGrid&
operator = (const ApproximateVoxelGrid &src)
{
leaf_size_ = src.leaf_size_;
inverse_leaf_size_ = src.inverse_leaf_size_;
downsample_all_data_ = src.downsample_all_data_;
histsize_ = src.histsize_;
history_ = new he[histsize_];
for (std::size_t i = 0; i < histsize_; i++)
history_[i] = src.history_[i];
return (*this);
}
/** \brief Set the voxel grid leaf size.
* \param[in] leaf_size the voxel grid leaf size
*/
inline void
setLeafSize (const Eigen::Vector3f &leaf_size)
{
leaf_size_ = leaf_size;
inverse_leaf_size_ = Eigen::Array3f::Ones () / leaf_size_.array ();
}
/** \brief Set the voxel grid leaf size.
* \param[in] lx the leaf size for X
* \param[in] ly the leaf size for Y
* \param[in] lz the leaf size for Z
*/
inline void
setLeafSize (float lx, float ly, float lz)
{
setLeafSize (Eigen::Vector3f (lx, ly, lz));
}
/** \brief Get the voxel grid leaf size. */
inline Eigen::Vector3f
getLeafSize () const { return (leaf_size_); }
/** \brief Set to true if all fields need to be downsampled, or false if just XYZ.
* \param downsample the new value (true/false)
*/
inline void
setDownsampleAllData (bool downsample) { downsample_all_data_ = downsample; }
/** \brief Get the state of the internal downsampling parameter (true if
* all fields need to be downsampled, false if just XYZ).
*/
inline bool
getDownsampleAllData () const { return (downsample_all_data_); }
protected:
/** \brief The size of a leaf. */
Eigen::Vector3f leaf_size_;
/** \brief Compute 1/leaf_size_ to avoid division later */
Eigen::Array3f inverse_leaf_size_;
/** \brief Set to true if all fields need to be downsampled, or false if just XYZ. */
bool downsample_all_data_{true};
/** \brief history buffer size, power of 2 */
std::size_t histsize_{512};
/** \brief history buffer */
struct he* history_;
using FieldList = typename pcl::traits::fieldList<PointT>::type;
/** \brief Downsample a Point Cloud using a voxelized grid approach
* \param output the resultant point cloud message
*/
void
applyFilter (PointCloud &output) override;
/** \brief Write a single point from the hash to the output cloud
*/
void
flush (PointCloud &output, std::size_t op, he *hhe, int rgba_index, int centroid_size);
};
}
#ifdef PCL_NO_PRECOMPILE
#include <pcl/filters/impl/approximate_voxel_grid.hpp>
#endif
pcl::ApproximateVoxelGrid
ApproximateVoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the...
Definition approximate_voxel_grid.h:101
pcl::ApproximateVoxelGrid::operator=
ApproximateVoxelGrid & operator=(const ApproximateVoxelGrid &src)
Copy operator.
Definition approximate_voxel_grid.h:166
pcl::ApproximateVoxelGrid::downsample_all_data_
bool downsample_all_data_
Set to true if all fields need to be downsampled, or false if just XYZ.
Definition approximate_voxel_grid.h:223
pcl::ApproximateVoxelGrid::inverse_leaf_size_
Eigen::Array3f inverse_leaf_size_
Compute 1/leaf_size_ to avoid division later.
Definition approximate_voxel_grid.h:220
pcl::ApproximateVoxelGrid::Ptr
shared_ptr< ApproximateVoxelGrid< PointT > > Ptr
Definition approximate_voxel_grid.h:122
pcl::ApproximateVoxelGrid::FieldList
typename pcl::traits::fieldList< PointT >::type FieldList
Definition approximate_voxel_grid.h:231
pcl::ApproximateVoxelGrid::histsize_
std::size_t histsize_
history buffer size, power of 2
Definition approximate_voxel_grid.h:226
pcl::ApproximateVoxelGrid::ApproximateVoxelGrid
ApproximateVoxelGrid()
Empty constructor.
Definition approximate_voxel_grid.h:127
pcl::ApproximateVoxelGrid::applyFilter
void applyFilter(PointCloud &output) override
Downsample a Point Cloud using a voxelized grid approach.
Definition approximate_voxel_grid.hpp:65
pcl::ApproximateVoxelGrid::leaf_size_
Eigen::Vector3f leaf_size_
The size of a leaf.
Definition approximate_voxel_grid.h:217
pcl::ApproximateVoxelGrid::history_
struct he * history_
history buffer
Definition approximate_voxel_grid.h:229
pcl::ApproximateVoxelGrid::~ApproximateVoxelGrid
~ApproximateVoxelGrid() override
Destructor.
Definition approximate_voxel_grid.h:156
pcl::ApproximateVoxelGrid::setLeafSize
void setLeafSize(float lx, float ly, float lz)
Set the voxel grid leaf size.
Definition approximate_voxel_grid.h:194
pcl::ApproximateVoxelGrid::ApproximateVoxelGrid
ApproximateVoxelGrid(const ApproximateVoxelGrid &src)
Copy constructor.
Definition approximate_voxel_grid.h:140
pcl::ApproximateVoxelGrid::setDownsampleAllData
void setDownsampleAllData(bool downsample)
Set to true if all fields need to be downsampled, or false if just XYZ.
Definition approximate_voxel_grid.h:207
pcl::ApproximateVoxelGrid::flush
void flush(PointCloud &output, std::size_t op, he *hhe, int rgba_index, int centroid_size)
Write a single point from the hash to the output cloud.
Definition approximate_voxel_grid.hpp:47
pcl::ApproximateVoxelGrid::getDownsampleAllData
bool getDownsampleAllData() const
Get the state of the internal downsampling parameter (true if all fields need to be downsampled,...
Definition approximate_voxel_grid.h:213
pcl::ApproximateVoxelGrid::setLeafSize
void setLeafSize(const Eigen::Vector3f &leaf_size)
Set the voxel grid leaf size.
Definition approximate_voxel_grid.h:182
pcl::ApproximateVoxelGrid::getLeafSize
Eigen::Vector3f getLeafSize() const
Get the voxel grid leaf size.
Definition approximate_voxel_grid.h:201
pcl::ApproximateVoxelGrid::ConstPtr
shared_ptr< const ApproximateVoxelGrid< PointT > > ConstPtr
Definition approximate_voxel_grid.h:123
pcl::Filter
Filter represents the base filter class.
Definition filter.h:81
pcl::Filter::getClassName
const std::string & getClassName() const
Get a string representation of the name of this class.
Definition filter.h:174
pcl::Filter::filter_name_
std::string filter_name_
The filter name.
Definition filter.h:158
pcl::PCLBase::input_
PointCloudConstPtr input_
The input point cloud dataset.
Definition pcl_base.h:147
pcl::PCLBase::indices_
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition pcl_base.h:150
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:173
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition point_cloud.h:413
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition point_cloud.h:414
Eigen
Definition bfgs.h:10
pcl
Definition convolution.h:46
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition point_types.hpp:594
pcl::xNdCopyEigenPointFunctor
Helper functor structure for copying data between an Eigen::VectorXf and a PointT.
Definition approximate_voxel_grid.h:47
pcl::xNdCopyEigenPointFunctor::Pod
typename traits::POD< PointT >::type Pod
Definition approximate_voxel_grid.h:48
pcl::xNdCopyEigenPointFunctor::operator()
void operator()()
Definition approximate_voxel_grid.h:55
pcl::xNdCopyEigenPointFunctor::xNdCopyEigenPointFunctor
xNdCopyEigenPointFunctor(const Eigen::VectorXf &p1, PointT &p2)
Definition approximate_voxel_grid.h:50
pcl::xNdCopyPointEigenFunctor
Helper functor structure for copying data between an Eigen::VectorXf and a PointT.
Definition approximate_voxel_grid.h:72
pcl::xNdCopyPointEigenFunctor::operator()
void operator()()
Definition approximate_voxel_grid.h:78
pcl::xNdCopyPointEigenFunctor::xNdCopyPointEigenFunctor
xNdCopyPointEigenFunctor(const PointT &p1, Eigen::VectorXf &p2)
Definition approximate_voxel_grid.h:75
pcl::xNdCopyPointEigenFunctor::Pod
typename traits::POD< PointT >::type Pod
Definition approximate_voxel_grid.h:73