41#include <pcl/point_cloud.h>
43#include <pcl/console/print.h>
46#define DEFAULT_GRID_RES_X 512
47#define DEFAULT_GRID_RES_Y 512
48#define DEFAULT_GRID_RES_Z 512
50#define DEFAULT_VOLUME_SIZE_X 3000
51#define DEFAULT_VOLUME_SIZE_Y 3000
52#define DEFAULT_VOLUME_SIZE_Z 3000
57 template <
typename VoxelT,
typename WeightT>
82 Header (
const Eigen::Vector3i &res,
const Eigen::Vector3f &
size)
92 friend inline std::ostream&
104 #define DEFAULT_TRANCATION_DISTANCE 30.0f
112 Intr (
float fx_,
float fy_,
float cx_,
float cy_)
113 :
fx(fx_),
fy(fy_),
cx(cx_),
cy(cy_) {};
121 friend inline std::ostream&
124 os <<
"([f = " <<
intr.fx <<
", " <<
intr.fy <<
"] [cp = " <<
intr.cx <<
", " <<
intr.cy <<
"])";
146 std::cout <<
"done [" <<
size() <<
"]" << std::endl;
148 std::cout <<
"error!" << std::endl;
153 setHeader (
const Eigen::Vector3i &resolution,
const Eigen::Vector3f &volume_size) {
154 header_ =
Header (resolution, volume_size);
155 if (volume_->size() !=
this->
size())
156 pcl::console::print_warn (
"[TSDFVolume::setHeader] Header volume size (%d) doesn't fit underlying data size (%d)", volume_->size(),
size());
161 resize (Eigen::Vector3i &
grid_resolution,
const Eigen::Vector3f& volume_size = Eigen::Vector3f (DEFAULT_VOLUME_SIZE_X, DEFAULT_VOLUME_SIZE_Y, DEFAULT_VOLUME_SIZE_Z)) {
171 resize (Eigen::Vector3i (DEFAULT_GRID_RES_X, DEFAULT_GRID_RES_Y, DEFAULT_GRID_RES_Z),
172 Eigen::Vector3f (DEFAULT_VOLUME_SIZE_X, DEFAULT_VOLUME_SIZE_Y, DEFAULT_VOLUME_SIZE_Z));
191 inline const Eigen::Vector3f &
195 inline Eigen::Vector3f
202 inline const Eigen::Vector3i &
206 inline const Header &
210 inline const std::vector<VoxelT> &
214 inline std::vector<VoxelT> &
218 inline const std::vector<WeightT> &
222 inline std::vector<WeightT> &
234 const unsigned step = 2)
const;
245 template <
typename Po
intT>
void
249 template <
typename Po
intT>
void
271 inline Eigen::VectorXi
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
shared_ptr< PointCloud< PointT > > Ptr
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.
std::size_t size() const
Returns overall number of voxels in grid.
bool extractNeighborhood(const Eigen::Vector3i &voxel_coord, int neighborhood_size, VoxelTVec &neighborhood) const
extracts voxels in neighborhood of given voxel
shared_ptr< TSDFVolume< VoxelT, WeightT > > Ptr
const Header & header() const
Returns constant reference to header.
void getVoxelCoordAndOffset(const PointT &point, Eigen::Vector3i &voxel_coord, Eigen::Vector3f &offset) const
Returns the 3D voxel coordinate and point offset wrt.
const Eigen::Vector3f & volumeSize() const
Returns the volume size in mm.
void setHeader(const Eigen::Vector3i &resolution, const Eigen::Vector3f &volume_size)
Set the header directly.
Eigen::VectorXf VoxelTVec
const Eigen::Vector3i & gridResolution() const
Returns the voxel grid resolution.
Eigen::Vector3f voxelSize() const
Returns the size of one voxel in mm.
bool addNeighborhood(const Eigen::Vector3i &voxel_coord, int neighborhood_size, const VoxelTVec &neighborhood, WeightT voxel_weight)
adds voxel values in local neighborhood
void convertToTsdfCloud(pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, const unsigned step=2) const
Converts volume to cloud of TSDF values.
TSDFVolume(const std::string &filename)
Constructor loading data from file.
std::vector< WeightT > & weightsWriteable() const
Returns writebale(!) reference to volume.
bool save(const std::string &filename="tsdf_volume.dat", bool binary=true) const
Saves volume to file.
void averageValues()
averages voxel values by the weight value
const std::vector< VoxelT > & volume() const
Returns constant reference to the volume std::vector.
void getVoxelCoord(const PointT &point, Eigen::Vector3i &voxel_coord) const
Converts the volume to a surface representation via a point cloud.
int getLinearVoxelIndex(const Eigen::Array3i &indices) const
Returns and index for linear access of the volume and weights.
std::vector< VoxelT > & volumeWriteable() const
Returns writebale(!) reference to volume.
bool load(const std::string &filename, bool binary=true)
Loads volume from file.
void resizeDefaultSize()
Resize internal storage and header to default sizes defined in tsdf_volume.h.
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.
TSDFVolume()
Default constructor.
const std::vector< WeightT > & weights() const
Returns constant reference to the weights std::vector.
Defines all the PCL implemented PointT point type structures.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Defines functions, macros and traits for allocating and using memory.
PCL_EXPORTS void print_warn(const char *format,...)
Print a warning message on stream with colors.
Defines all the PCL and non-PCL macros used.
A point structure representing Euclidean xyz coordinates, and the RGB color.
Camera intrinsics structure.
friend std::ostream & operator<<(std::ostream &os, const Intr &intr)
Intr operator()(int level_index) const
Intr(float fx_, float fy_, float cx_, float cy_)