37 #ifndef TSDF_VOLUME_HPP_ 38 #define TSDF_VOLUME_HPP_ 40 #include "tsdf_volume.h" 45 template <
typename VoxelT,
typename WeightT>
bool 49 std::cout << std::flush;
51 std::ifstream file (filename.c_str());
58 file.read ((
char*) &header_,
sizeof (
Header));
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);
77 int num_elements = header_.getVolumeSize();
78 volume_->resize (num_elements);
79 weights_->resize (num_elements);
80 file.read ((
char*) &(*volume_)[0], num_elements *
sizeof(VoxelT));
81 file.read ((
char*) &(*weights_)[0], num_elements *
sizeof(WeightT));
96 const Eigen::Vector3i &res = this->gridResolution();
103 template <
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;
136 file <<
sizeof (VoxelT) <<
" " <<
sizeof(WeightT) << std::endl;
139 for (
typename std::vector<VoxelT>::const_iterator iter = volume_->begin(); iter != volume_->end(); ++iter)
140 file << *iter << std::endl;
157 template <
typename VoxelT,
typename WeightT>
void 160 int sx = header_.resolution(0);
161 int sy = header_.resolution(1);
162 int sz = header_.resolution(2);
165 const int cloud_size = header_.getVolumeSize() / (step*step*step);
168 cloud->
reserve (std::min (cloud_size/10, 500000));
170 int volume_idx = 0, cloud_idx = 0;
172 for (
int z = 0; z < sz; z+=step)
173 for (
int y = 0; y < sy; y+=step)
174 for (
int x = 0; x < sx; x+=step, ++cloud_idx)
176 volume_idx = sx*sy*z + sx*y + x;
179 if (weights_->at(volume_idx) == 0 || volume_->at(volume_idx) > 0.98 )
183 point.x = x; point.y = y; point.z = z;
184 point.
intensity = volume_->at(volume_idx);
193 template <
typename VoxelT,
typename WeightT>
template <
typename Po
intT>
void 196 static Eigen::Array3f voxel_size = voxelSize().array();
199 Eigen::Array3f point_coord (point.x, point.y, point.z);
200 Eigen::Array3f voxel_coord = (point_coord / voxel_size) - 0.5f;
201 coord(0) = round(voxel_coord(0));
202 coord(1) = round(voxel_coord(1));
203 coord(2) = round(voxel_coord(2));
208 template <
typename VoxelT,
typename WeightT>
template <
typename Po
intT>
void 210 Eigen::Vector3i &coord, Eigen::Vector3f &offset)
const 212 static Eigen::Array3f voxel_size = voxelSize().array();
215 Eigen::Array3f point_coord (point.x, point.y, point.z);
216 Eigen::Array3f voxel_coord = (point_coord / voxel_size) - 0.5f;
217 coord(0) = round(voxel_coord(0));
218 coord(1) = round(voxel_coord(1));
219 coord(2) = round(voxel_coord(2));
222 offset = (voxel_coord - coord.cast<
float>().array() * voxel_size).matrix();
226 template <
typename VoxelT,
typename WeightT>
bool 231 int shift = (neighborhood_size - 1) / 2;
232 Eigen::Vector3i min_index = voxel_coord.array() - shift;
233 Eigen::Vector3i max_index = voxel_coord.array() + shift;
236 if (getLinearVoxelIndex(min_index) < 0 || getLinearVoxelIndex(max_index) >= (
int)size())
238 pcl::console::print_info (
"[extractNeighborhood] Skipping voxel with coord (%d, %d, %d).\n", voxel_coord(0), voxel_coord(1), voxel_coord(2));
242 static const int descriptor_size = neighborhood_size*neighborhood_size*neighborhood_size;
243 neighborhood.resize (descriptor_size);
245 const Eigen::RowVector3i offset_vector (1, neighborhood_size, neighborhood_size*neighborhood_size);
248 #pragma omp parallel for 249 for (
int z = min_index(2); z <= max_index(2); ++z)
251 for (
int y = min_index(1); y <= max_index(1); ++y)
253 for (
int x = min_index(0); x <= max_index(0); ++x)
256 Eigen::Vector3i point (x,y,z);
257 int volume_idx = getLinearVoxelIndex (point);
258 int descr_idx = offset_vector * (point - min_index);
267 if (weights_->at (volume_idx) != 0)
268 neighborhood (descr_idx) = volume_->at (volume_idx);
270 neighborhood (descr_idx) = -1.0;
279 template <
typename VoxelT,
typename WeightT>
bool 281 const VoxelTVec &neighborhood, WeightT voxel_weight)
284 int shift = (neighborhood_size - 1) / 2;
285 Eigen::Vector3i min_index = voxel_coord.array() - shift;
286 Eigen::Vector3i max_index = voxel_coord.array() + shift;
289 if (getLinearVoxelIndex(min_index) < 0 || getLinearVoxelIndex(max_index) >= (
int)size())
291 pcl::console::print_info (
"[addNeighborhood] Skipping voxel with coord (%d, %d, %d).\n", voxel_coord(0), voxel_coord(1), voxel_coord(2));
296 const Eigen::RowVector3i offset_vector (1, neighborhood_size, neighborhood_size*neighborhood_size);
298 Eigen::Vector3i index = min_index;
300 #pragma omp parallel for 301 for (
int z = min_index(2); z <= max_index(2); ++z)
303 for (
int y = min_index(1); y <= max_index(1); ++y)
305 for (
int x = min_index(0); x <= max_index(0); ++x)
308 Eigen::Vector3i point (x,y,z);
309 int volume_idx = getLinearVoxelIndex (point);
310 int descr_idx = offset_vector * (point - min_index);
313 VoxelT &voxel = volume_->at (volume_idx);
314 WeightT &weight = weights_->at (volume_idx);
318 voxel += neighborhood (descr_idx);
321 weight += voxel_weight;
330 template <
typename VoxelT,
typename WeightT>
void 333 #pragma omp parallel for 334 for (
size_t i = 0; i < volume_->size(); ++i)
336 WeightT &w = weights_->at(i);
528 #define PCL_INSTANTIATE_TSDFVolume(VT,WT) template class PCL_EXPORTS pcl::reconstruction::TSDFVolume<VT,WT>;
PCL_EXPORTS void print_value(const char *format,...)
Print a value message on stream with colors.
bool addNeighborhood(const Eigen::Vector3i &voxel_coord, int neighborhood_size, const VoxelTVec &neighborhood, WeightT voxel_weight)
adds voxel values in local neighborhood
bool load(const std::string &filename, bool binary=true)
Loads volume from file.
PCL_EXPORTS void print_info(const char *format,...)
Print an info message on stream with colors.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
void averageValues()
averages voxel values by the weight value
boost::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 convertToTsdfCloud(pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud) const
Converts volume to cloud of TSDF values.
void clear()
Removes all points in a cloud and sets the width and height to 0.
void getVoxelCoordAndOffset(const PointT &point, Eigen::Vector3i &voxel_coord, Eigen::Vector3f &offset) const
Retunrs the 3D voxel coordinate and point offset wrt.
Eigen::VectorXf VoxelTVec
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool save(const std::string &filename="tsdf_volume.dat", bool binary=true) const
Saves volume to file.
PCL_EXPORTS void print_error(const char *format,...)
Print an error message on stream with colors.
void getVoxelCoord(const PointT &point, Eigen::Vector3i &voxel_coord) const
Converts the volume to a surface representation via a point cloud.