Loading...
Searching...
No Matches
tsdf_volume.h
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011, 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 Willow Garage, Inc. 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: tsdf_volume.h 6459 2012-07-18 07:50:37Z dpb $
*/
#pragma once
#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/console/print.h>
#define DEFAULT_GRID_RES_X 512 // pcl::device::VOLUME_X ( and _Y, _Z)
#define DEFAULT_GRID_RES_Y 512
#define DEFAULT_GRID_RES_Z 512
#define DEFAULT_VOLUME_SIZE_X 3000
#define DEFAULT_VOLUME_SIZE_Y 3000
#define DEFAULT_VOLUME_SIZE_Z 3000
namespace pcl
{
template <typename VoxelT, typename WeightT>
class TSDFVolume
{
public:
using Ptr = shared_ptr<TSDFVolume<VoxelT, WeightT> >;
using ConstPtr = shared_ptr<const TSDFVolume<VoxelT, WeightT> >;
// using VoxelTVec = Eigen::Matrix<VoxelT, Eigen::Dynamic, 1>;
using VoxelTVec = Eigen::VectorXf;
/** \brief Structure storing voxel grid resolution, volume size (in mm) and element_size of stored data */
struct Header
{
Eigen::Vector3i resolution;
Eigen::Vector3f volume_size;
int volume_element_size, weights_element_size;
Header ()
: resolution (0,0,0),
volume_size (0,0,0),
volume_element_size (sizeof(VoxelT)),
weights_element_size (sizeof(WeightT))
{};
Header (const Eigen::Vector3i &res, const Eigen::Vector3f &size)
: resolution (res),
volume_size (size),
volume_element_size (sizeof(VoxelT)),
weights_element_size (sizeof(WeightT))
{};
inline std::size_t
getVolumeSize () const { return resolution[0] * resolution[1] * resolution[2]; };
friend inline std::ostream&
operator << (std::ostream& os, const Header& h)
{
os << "(resolution = " << h.resolution.transpose() << ", volume size = " << h.volume_size.transpose() << ")";
return (os);
}
public:
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
#define DEFAULT_TRANCATION_DISTANCE 30.0f
/** \brief Camera intrinsics structure
*/
struct Intr
{
float fx, fy, cx, cy;
Intr () {};
Intr (float fx_, float fy_, float cx_, float cy_)
: fx(fx_), fy(fy_), cx(cx_), cy(cy_) {};
Intr operator()(int level_index) const
{
int div = 1 << level_index;
return (Intr (fx / div, fy / div, cx / div, cy / div));
}
friend inline std::ostream&
operator << (std::ostream& os, const Intr& intr)
{
os << "([f = " << intr.fx << ", " << intr.fy << "] [cp = " << intr.cx << ", " << intr.cy << "])";
return (os);
}
};
////////////////////////////////////////////////////////////////////////////////////////
// Constructors
/** \brief Default constructor */
TSDFVolume ()
: volume_ (new std::vector<VoxelT>),
weights_ (new std::vector<WeightT>)
{};
/** \brief Constructor loading data from file */
TSDFVolume (const std::string &filename)
: volume_ (new std::vector<VoxelT>),
weights_ (new std::vector<WeightT>)
{
if (load (filename))
std::cout << "done [" << size() << "]" << std::endl;
else
std::cout << "error!" << std::endl;
};
/** \brief Set the header directly. Useful if directly writing into volume and weights */
inline void
setHeader (const Eigen::Vector3i &resolution, const Eigen::Vector3f &volume_size) {
header_ = Header (resolution, volume_size);
if (volume_->size() != this->size())
pcl::console::print_warn ("[TSDFVolume::setHeader] Header volume size (%d) doesn't fit underlying data size (%d)", volume_->size(), size());
};
/** \brief Resizes the internal storage and updates the header accordingly */
inline void
resize (Eigen::Vector3i &grid_resolution, const Eigen::Vector3f& volume_size = Eigen::Vector3f (DEFAULT_VOLUME_SIZE_X, DEFAULT_VOLUME_SIZE_Y, DEFAULT_VOLUME_SIZE_Z)) {
int lin_size = grid_resolution[0] * grid_resolution[1] * grid_resolution[2];
volume_->resize (lin_size);
weights_->resize (lin_size);
setHeader (grid_resolution, volume_size);
};
/** \brief Resize internal storage and header to default sizes defined in tsdf_volume.h */
inline void
resizeDefaultSize () {
resize (Eigen::Vector3i (DEFAULT_GRID_RES_X, DEFAULT_GRID_RES_Y, DEFAULT_GRID_RES_Z),
Eigen::Vector3f (DEFAULT_VOLUME_SIZE_X, DEFAULT_VOLUME_SIZE_Y, DEFAULT_VOLUME_SIZE_Z));
};
////////////////////////////////////////////////////////////////////////////////////////
// Storage and element access
/** \brief Loads volume from file */
bool
load (const std::string &filename, bool binary = true);
/** \brief Saves volume to file */
bool
save (const std::string &filename = "tsdf_volume.dat", bool binary = true) const;
/** \brief Returns overall number of voxels in grid */
inline std::size_t
size () const { return header_.getVolumeSize(); };
/** \brief Returns the volume size in mm */
inline const Eigen::Vector3f &
volumeSize () const { return header_.volume_size; };
/** \brief Returns the size of one voxel in mm */
inline Eigen::Vector3f
voxelSize () const {
Eigen::Array3f res = header_.resolution.array().template cast<float>();
return header_.volume_size.array() / res;
};
/** \brief Returns the voxel grid resolution */
inline const Eigen::Vector3i &
gridResolution() const { return header_.resolution; };
/** \brief Returns constant reference to header */
inline const Header &
header () const { return header_; };
/** \brief Returns constant reference to the volume std::vector */
inline const std::vector<VoxelT> &
volume () const { return *volume_; };
/** \brief Returns writebale(!) reference to volume */
inline std::vector<VoxelT> &
volumeWriteable () const { return *volume_; };
/** \brief Returns constant reference to the weights std::vector */
inline const std::vector<WeightT> &
weights () const { return *weights_; };
/** \brief Returns writebale(!) reference to volume */
inline std::vector<WeightT> &
weightsWriteable () const { return *weights_; };
////////////////////////////////////////////////////////////////////////////////////////
// Functionality
/** \brief Converts volume to cloud of TSDF values
* \param[out] cloud - the output point cloud
* \param[in] step - the decimation step to use
*/
void
convertToTsdfCloud (pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud,
const unsigned step = 2) const;
/** \brief Converts the volume to a surface representation via a point cloud */
// void
// convertToCloud (pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) const;
/** \brief Create Volume from Point Cloud */
// template <typename PointT> void
// createFromCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, const Intr &intr);
/** \brief Returns the 3D voxel coordinate */
template <typename PointT> void
getVoxelCoord (const PointT &point, Eigen::Vector3i &voxel_coord) const;
/** \brief Returns the 3D voxel coordinate and point offset wrt. to the voxel center (in mm) */
template <typename PointT> void
getVoxelCoordAndOffset (const PointT &point, Eigen::Vector3i &voxel_coord, Eigen::Vector3f &offset) const;
/** extracts voxels in neighborhood of given voxel */
bool
extractNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size, VoxelTVec &neighborhood) const;
/** adds voxel values in local neighborhood */
bool
addNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size, const VoxelTVec &neighborhood, WeightT voxel_weight);
/** averages voxel values by the weight value */
void
averageValues ();
/** \brief Returns and index for linear access of the volume and weights */
inline int
getLinearVoxelIndex (const Eigen::Array3i &indices) const {
return indices(0) + indices(1) * header_.resolution[0] + indices(2) * header_.resolution[0] * header_.resolution[1];
}
/** \brief Returns a vector of linear indices for voxel coordinates given in 3xn matrix */
inline Eigen::VectorXi
getLinearVoxelIndinces (const Eigen::Matrix<int, 3, Eigen::Dynamic> &indices_matrix) const {
return (Eigen::RowVector3i (1, header_.resolution[0], header_.resolution[0] * header_.resolution[1]) * indices_matrix).transpose();
}
private:
////////////////////////////////////////////////////////////////////////////////////////
// Private functions and members
// void
// scaleDepth (const Eigen::MatrixXf &depth, Eigen::MatrixXf &depth_scaled, const Intr &intr) const;
// void
// integrateVolume (const Eigen::MatrixXf &depth_scaled, float tranc_dist, const Eigen::Matrix3f &R_inv, const Eigen::Vector3f &t, const Intr &intr);
using VolumePtr = shared_ptr<std::vector<VoxelT> >;
using WeightsPtr = shared_ptr<std::vector<WeightT> >;
Header header_;
VolumePtr volume_;
WeightsPtr weights_;
public:
PCL_MAKE_ALIGNED_OPERATOR_NEW
};
}
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition point_cloud.h:413
pcl::TSDFVolume
Definition tsdf_volume.h:59
pcl::TSDFVolume::resize
void resize(Eigen::Vector3i &grid_resolution, const Eigen::Vector3f &volume_size=Eigen::Vector3f(DEFAULT_VOLUME_SIZE_X, DEFAULT_VOLUME_SIZE_Y, DEFAULT_VOLUME_SIZE_Z))
Resizes the internal storage and updates the header accordingly.
Definition tsdf_volume.h:161
pcl::TSDFVolume::size
std::size_t size() const
Returns overall number of voxels in grid.
Definition tsdf_volume.h:188
pcl::TSDFVolume::extractNeighborhood
bool extractNeighborhood(const Eigen::Vector3i &voxel_coord, int neighborhood_size, VoxelTVec &neighborhood) const
extracts voxels in neighborhood of given voxel
Definition tsdf_volume.hpp:226
pcl::TSDFVolume::Ptr
shared_ptr< TSDFVolume< VoxelT, WeightT > > Ptr
Definition tsdf_volume.h:62
pcl::TSDFVolume::header
const Header & header() const
Returns constant reference to header.
Definition tsdf_volume.h:207
pcl::TSDFVolume::getVoxelCoordAndOffset
void getVoxelCoordAndOffset(const PointT &point, Eigen::Vector3i &voxel_coord, Eigen::Vector3f &offset) const
Returns the 3D voxel coordinate and point offset wrt.
Definition tsdf_volume.hpp:208
pcl::TSDFVolume::volumeSize
const Eigen::Vector3f & volumeSize() const
Returns the volume size in mm.
Definition tsdf_volume.h:192
pcl::TSDFVolume::setHeader
void setHeader(const Eigen::Vector3i &resolution, const Eigen::Vector3f &volume_size)
Set the header directly.
Definition tsdf_volume.h:153
pcl::TSDFVolume::VoxelTVec
Eigen::VectorXf VoxelTVec
Definition tsdf_volume.h:66
pcl::TSDFVolume::gridResolution
const Eigen::Vector3i & gridResolution() const
Returns the voxel grid resolution.
Definition tsdf_volume.h:203
pcl::TSDFVolume::voxelSize
Eigen::Vector3f voxelSize() const
Returns the size of one voxel in mm.
Definition tsdf_volume.h:196
pcl::TSDFVolume::addNeighborhood
bool addNeighborhood(const Eigen::Vector3i &voxel_coord, int neighborhood_size, const VoxelTVec &neighborhood, WeightT voxel_weight)
adds voxel values in local neighborhood
Definition tsdf_volume.hpp:280
pcl::TSDFVolume::convertToTsdfCloud
void convertToTsdfCloud(pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, const unsigned step=2) const
Converts volume to cloud of TSDF values.
Definition tsdf_volume.hpp:158
pcl::TSDFVolume::TSDFVolume
TSDFVolume(const std::string &filename)
Constructor loading data from file.
Definition tsdf_volume.h:141
pcl::TSDFVolume::weightsWriteable
std::vector< WeightT > & weightsWriteable() const
Returns writebale(!) reference to volume.
Definition tsdf_volume.h:223
pcl::TSDFVolume::ConstPtr
shared_ptr< const TSDFVolume< VoxelT, WeightT > > ConstPtr
Definition tsdf_volume.h:63
pcl::TSDFVolume::save
bool save(const std::string &filename="tsdf_volume.dat", bool binary=true) const
Saves volume to file.
Definition tsdf_volume.hpp:104
pcl::TSDFVolume::averageValues
void averageValues()
averages voxel values by the weight value
Definition tsdf_volume.hpp:331
pcl::TSDFVolume::volume
const std::vector< VoxelT > & volume() const
Returns constant reference to the volume std::vector.
Definition tsdf_volume.h:211
pcl::TSDFVolume::getVoxelCoord
void getVoxelCoord(const PointT &point, Eigen::Vector3i &voxel_coord) const
Converts the volume to a surface representation via a point cloud.
Definition tsdf_volume.hpp:193
pcl::TSDFVolume::getLinearVoxelIndex
int getLinearVoxelIndex(const Eigen::Array3i &indices) const
Returns and index for linear access of the volume and weights.
Definition tsdf_volume.h:266
pcl::TSDFVolume::volumeWriteable
std::vector< VoxelT > & volumeWriteable() const
Returns writebale(!) reference to volume.
Definition tsdf_volume.h:215
pcl::TSDFVolume::load
bool load(const std::string &filename, bool binary=true)
Loads volume from file.
Definition tsdf_volume.hpp:46
pcl::TSDFVolume::resizeDefaultSize
void resizeDefaultSize()
Resize internal storage and header to default sizes defined in tsdf_volume.h.
Definition tsdf_volume.h:170
pcl::TSDFVolume::getLinearVoxelIndinces
Eigen::VectorXi getLinearVoxelIndinces(const Eigen::Matrix< int, 3, Eigen::Dynamic > &indices_matrix) const
Returns a vector of linear indices for voxel coordinates given in 3xn matrix.
Definition tsdf_volume.h:272
pcl::TSDFVolume::TSDFVolume
TSDFVolume()
Default constructor.
Definition tsdf_volume.h:135
pcl::TSDFVolume::weights
const std::vector< WeightT > & weights() const
Returns constant reference to the weights std::vector.
Definition tsdf_volume.h:219
point_types.h
Defines all the PCL implemented PointT point type structures.
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition memory.h:86
memory.h
Defines functions, macros and traits for allocating and using memory.
pcl::console::print_warn
PCL_EXPORTS void print_warn(const char *format,...)
Print a warning message on stream with colors.
pcl
Definition convolution.h:46
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition point_types.hpp:594
pcl::TSDFVolume::Header
Structure storing voxel grid resolution, volume size (in mm) and element_size of stored data.
Definition tsdf_volume.h:70
pcl::TSDFVolume::Header::Header
Header()
Definition tsdf_volume.h:75
pcl::TSDFVolume::Header::Header
Header(const Eigen::Vector3i &res, const Eigen::Vector3f &size)
Definition tsdf_volume.h:82
pcl::TSDFVolume::Header::getVolumeSize
std::size_t getVolumeSize() const
Definition tsdf_volume.h:90
pcl::TSDFVolume::Header::volume_element_size
int volume_element_size
Definition tsdf_volume.h:73
pcl::TSDFVolume::Header::weights_element_size
int weights_element_size
Definition tsdf_volume.h:73
pcl::TSDFVolume::Header::operator<<
friend std::ostream & operator<<(std::ostream &os, const Header &h)
Definition tsdf_volume.h:93
pcl::TSDFVolume::Header::volume_size
Eigen::Vector3f volume_size
Definition tsdf_volume.h:72
pcl::TSDFVolume::Header::resolution
Eigen::Vector3i resolution
Definition tsdf_volume.h:71
pcl::TSDFVolume::Intr
Camera intrinsics structure.
Definition tsdf_volume.h:109
pcl::TSDFVolume::Intr::operator<<
friend std::ostream & operator<<(std::ostream &os, const Intr &intr)
Definition tsdf_volume.h:122
pcl::TSDFVolume::Intr::cx
float cx
Definition tsdf_volume.h:110
pcl::TSDFVolume::Intr::operator()
Intr operator()(int level_index) const
Definition tsdf_volume.h:115
pcl::TSDFVolume::Intr::Intr
Intr()
Definition tsdf_volume.h:111
pcl::TSDFVolume::Intr::fy
float fy
Definition tsdf_volume.h:110
pcl::TSDFVolume::Intr::fx
float fx
Definition tsdf_volume.h:110
pcl::TSDFVolume::Intr::cy
float cy
Definition tsdf_volume.h:110
pcl::TSDFVolume::Intr::Intr
Intr(float fx_, float fy_, float cx_, float cy_)
Definition tsdf_volume.h:112