37#ifndef TSDF_VOLUME_HPP_
38#define TSDF_VOLUME_HPP_
40#include "tsdf_volume.h"
45template <
typename VoxelT,
typename WeightT>
bool
49 std::cout << std::flush;
65 if (header_.volume_element_size !=
sizeof(
VoxelT))
67 pcl::console::print_error (
"[TSDFVolume::load] Error: Given volume element size (%d) doesn't fit data (%d)",
sizeof(
VoxelT), header_.volume_element_size);
70 if ( header_.weights_element_size !=
sizeof(
WeightT))
72 pcl::console::print_error (
"[TSDFVolume::load] Error: Given weights element size (%d) doesn't fit data (%d)",
sizeof(
WeightT), header_.weights_element_size);
96 const Eigen::Vector3i &res = this->gridResolution();
103template <
typename VoxelT,
typename WeightT>
bool
107 std::cout << std::flush;
109 std::ofstream
file (
filename.c_str(),
binary ? std::ios_base::binary : std::ios_base::out);
117 file.write ((
char*) &header_,
sizeof (
Header));
128 file.write ((
char*) &(volume_->at(0)), volume_->size()*
sizeof(
VoxelT));
129 file.write ((
char*) &(weights_->at(0)), weights_->size()*
sizeof(
WeightT));
134 file << header_.resolution(0) <<
" " << header_.resolution(1) <<
" " << header_.resolution(2) << std::endl;
135 file << header_.volume_size(0) <<
" " << header_.volume_size(1) <<
" " << header_.volume_size(2) << std::endl;
139 for (
typename std::vector<VoxelT>::const_iterator iter = volume_->begin(); iter != volume_->end(); ++iter)
140 file << *iter << std::endl;
157template <
typename VoxelT,
typename WeightT>
void
159 const unsigned step)
const
161 int sx = header_.resolution(0);
162 int sy = header_.resolution(1);
163 int sz = header_.resolution(2);
165 const int cloud_size = header_.getVolumeSize() / (step*step*step);
168 cloud->reserve (std::min (cloud_size/10, 500000));
171 for (
int z = 0; z <
sz; z+=step)
172 for (
int y = 0; y <
sy; y+=step)
182 point.x = x; point.y = y; point.z = z;
184 cloud->push_back (point);
192template <
typename VoxelT,
typename WeightT>
template <
typename Po
intT>
void
195 static Eigen::Array3f
voxel_size = voxelSize().array();
198 Eigen::Array3f
point_coord (point.x, point.y, point.z);
207template <
typename VoxelT,
typename WeightT>
template <
typename Po
intT>
void
209 Eigen::Vector3i &
coord, Eigen::Vector3f &offset)
const
211 static Eigen::Array3f
voxel_size = voxelSize().array();
214 Eigen::Array3f
point_coord (point.x, point.y, point.z);
225template <
typename VoxelT,
typename WeightT>
bool
247 #pragma omp parallel for \
256 Eigen::Vector3i point (x,y,z);
279template <
typename VoxelT,
typename WeightT>
bool
300 #pragma omp parallel for \
309 Eigen::Vector3i point (x,y,z);
330template <
typename VoxelT,
typename WeightT>
void
333 #pragma omp parallel for \
335 for (std::size_t i = 0; i < volume_->size(); ++i)
346#define PCL_INSTANTIATE_TSDFVolume(VT,WT) template class PCL_EXPORTS pcl::reconstruction::TSDFVolume<VT,WT>;
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
std::size_t size() const
Size of the range the iterator is going through.
shared_ptr< PointCloud< PointT > > Ptr
bool extractNeighborhood(const Eigen::Vector3i &voxel_coord, int neighborhood_size, VoxelTVec &neighborhood) const
extracts voxels in neighborhood of given voxel
void getVoxelCoordAndOffset(const PointT &point, Eigen::Vector3i &voxel_coord, Eigen::Vector3f &offset) const
Returns the 3D voxel coordinate and point offset wrt.
Eigen::VectorXf VoxelTVec
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.
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
void getVoxelCoord(const PointT &point, Eigen::Vector3i &voxel_coord) const
Converts the volume to a surface representation via a point cloud.
bool load(const std::string &filename, bool binary=true)
Loads volume from file.
PCL_EXPORTS void print_error(const char *format,...)
Print an error message on stream with colors.
PCL_EXPORTS void print_info(const char *format,...)
Print an info message on stream with colors.
PCL_EXPORTS void print_value(const char *format,...)
Print a value message on stream with colors.
A point structure representing Euclidean xyz coordinates, and the RGB color.