Point Cloud Library (PCL)  1.14.0-dev
tsdf_volume.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id$
35  */
36 
37 #ifndef TSDF_VOLUME_HPP_
38 #define TSDF_VOLUME_HPP_
39 
40 #include "tsdf_volume.h"
41 
42 #include <fstream>
43 
44 
45 template <typename VoxelT, typename WeightT> bool
46 pcl::TSDFVolume<VoxelT, WeightT>::load (const std::string &filename, bool binary)
47 {
48  pcl::console::print_info ("Loading TSDF volume from "); pcl::console::print_value ("%s ... ", filename.c_str());
49  std::cout << std::flush;
50 
51  std::ifstream file (filename.c_str());
52 
53  if (file.is_open())
54  {
55  if (binary)
56  {
57  // read HEADER
58  file.read ((char*) &header_, sizeof (Header));
59  /* file.read (&header_.resolution, sizeof(Eigen::Array3i));
60  file.read (&header_.volume_size, sizeof(Eigen::Vector3f));
61  file.read (&header_.volume_element_size, sizeof(int));
62  file.read (&header_.weights_element_size, sizeof(int)); */
63 
64  // check if element size fits to data
65  if (header_.volume_element_size != sizeof(VoxelT))
66  {
67  pcl::console::print_error ("[TSDFVolume::load] Error: Given volume element size (%d) doesn't fit data (%d)", sizeof(VoxelT), header_.volume_element_size);
68  return false;
69  }
70  if ( header_.weights_element_size != sizeof(WeightT))
71  {
72  pcl::console::print_error ("[TSDFVolume::load] Error: Given weights element size (%d) doesn't fit data (%d)", sizeof(WeightT), header_.weights_element_size);
73  return false;
74  }
75 
76  // read DATA
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));
82  }
83  else
84  {
85  pcl::console::print_error ("[TSDFVolume::load] Error: ASCII loading not implemented.\n");
86  }
87 
88  file.close ();
89  }
90  else
91  {
92  pcl::console::print_error ("[TSDFVolume::load] Error: Cloudn't read file %s.\n", filename.c_str());
93  return false;
94  }
95 
96  const Eigen::Vector3i &res = this->gridResolution();
97  pcl::console::print_info ("done [%d voxels, res %dx%dx%d]\n", this->size(), res[0], res[1], res[2]);
98 
99  return true;
100 }
101 
102 
103 template <typename VoxelT, typename WeightT> bool
104 pcl::TSDFVolume<VoxelT, WeightT>::save (const std::string &filename, bool binary) const
105 {
106  pcl::console::print_info ("Saving TSDF volume to "); pcl::console::print_value ("%s ... ", filename.c_str());
107  std::cout << std::flush;
108 
109  std::ofstream file (filename.c_str(), binary ? std::ios_base::binary : std::ios_base::out);
110 
111  if (file.is_open())
112  {
113  if (binary)
114  {
115  // HEADER
116  // write resolution and size of volume
117  file.write ((char*) &header_, sizeof (Header));
118  /* file.write ((char*) &header_.resolution, sizeof(Eigen::Vector3i));
119  file.write ((char*) &header_.volume_size, sizeof(Eigen::Vector3f));
120  // write element size
121  int volume_element_size = sizeof(VolumeT);
122  file.write ((char*) &volume_element_size, sizeof(int));
123  int weights_element_size = sizeof(WeightT);
124  file.write ((char*) &weights_element_size, sizeof(int)); */
125 
126  // DATA
127  // write data
128  file.write ((char*) &(volume_->at(0)), volume_->size()*sizeof(VoxelT));
129  file.write ((char*) &(weights_->at(0)), weights_->size()*sizeof(WeightT));
130  }
131  else
132  {
133  // write resolution and size of volume and element size
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;
137 
138  // write data
139  for (typename std::vector<VoxelT>::const_iterator iter = volume_->begin(); iter != volume_->end(); ++iter)
140  file << *iter << std::endl;
141  }
142 
143  file.close();
144  }
145  else
146  {
147  pcl::console::print_error ("[saveTsdfVolume] Error: Couldn't open file %s.\n", filename.c_str());
148  return false;
149  }
150 
151  pcl::console::print_info ("done [%d voxels]\n", this->size());
152 
153  return true;
154 }
155 
156 
157 template <typename VoxelT, typename WeightT> void
159  const unsigned step) const
160 {
161  int sx = header_.resolution(0);
162  int sy = header_.resolution(1);
163  int sz = header_.resolution(2);
164 
165  const int cloud_size = header_.getVolumeSize() / (step*step*step);
166 
167  cloud->clear();
168  cloud->reserve (std::min (cloud_size/10, 500000));
169 
170  int volume_idx = 0, cloud_idx = 0;
171  for (int z = 0; z < sz; z+=step)
172  for (int y = 0; y < sy; y+=step)
173  for (int x = 0; x < sx; x+=step, ++cloud_idx)
174  {
175  volume_idx = sx*sy*z + sx*y + x;
176  // pcl::PointXYZI &point = (*cloud)[cloud_idx];
177 
178  if (weights_->at(volume_idx) == 0 || volume_->at(volume_idx) > 0.98 )
179  continue;
180 
181  pcl::PointXYZI point;
182  point.x = x; point.y = y; point.z = z;//*64;
183  point.intensity = volume_->at(volume_idx);
184  cloud->push_back (point);
185  }
186 
187  // cloud->width = cloud_size;
188  // cloud->height = 1;
189 }
190 
191 
192 template <typename VoxelT, typename WeightT> template <typename PointT> void
193 pcl::TSDFVolume<VoxelT, WeightT>::getVoxelCoord (const PointT &point, Eigen::Vector3i &coord) const
194 {
195  static Eigen::Array3f voxel_size = voxelSize().array();
196 
197  // point coordinates in world coordinate frame and voxel coordinates
198  Eigen::Array3f point_coord (point.x, point.y, point.z);
199  Eigen::Array3f voxel_coord = (point_coord / voxel_size) - 0.5f; // 0.5f offset due to voxel center vs grid
200  coord(0) = round(voxel_coord(0));
201  coord(1) = round(voxel_coord(1));
202  coord(2) = round(voxel_coord(2));
203 }
204 
205 
206 /** \brief Returns the 3D voxel coordinate and point offset wrt. to the voxel center (in mm) */
207 template <typename VoxelT, typename WeightT> template <typename PointT> void
209  Eigen::Vector3i &coord, Eigen::Vector3f &offset) const
210 {
211  static Eigen::Array3f voxel_size = voxelSize().array();
212 
213  // point coordinates in world coordinate frame and voxel coordinates
214  Eigen::Array3f point_coord (point.x, point.y, point.z);
215  Eigen::Array3f voxel_coord = (point_coord / voxel_size) - 0.5f; // 0.5f offset due to voxel center vs grid
216  coord(0) = round(voxel_coord(0));
217  coord(1) = round(voxel_coord(1));
218  coord(2) = round(voxel_coord(2));
219 
220  // offset of point wrt. to voxel center
221  offset = (voxel_coord - coord.cast<float>().array() * voxel_size).matrix();
222 }
223 
224 
225 template <typename VoxelT, typename WeightT> bool
226 pcl::TSDFVolume<VoxelT, WeightT>::extractNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size,
227  VoxelTVec &neighborhood) const
228 {
229  // point_index is at the center of a cube of scale_ x scale_ x scale_ voxels
230  int shift = (neighborhood_size - 1) / 2;
231  Eigen::Vector3i min_index = voxel_coord.array() - shift;
232  Eigen::Vector3i max_index = voxel_coord.array() + shift;
233 
234  // check that index is within range
235  if (getLinearVoxelIndex(min_index) < 0 || getLinearVoxelIndex(max_index) >= (int)size())
236  {
237  pcl::console::print_info ("[extractNeighborhood] Skipping voxel with coord (%d, %d, %d).\n", voxel_coord(0), voxel_coord(1), voxel_coord(2));
238  return false;
239  }
240 
241  static const int descriptor_size = neighborhood_size*neighborhood_size*neighborhood_size;
242  neighborhood.resize (descriptor_size);
243 
244  const Eigen::RowVector3i offset_vector (1, neighborhood_size, neighborhood_size*neighborhood_size);
245 
246  // loop over all voxels in 3D neighborhood
247  #pragma omp parallel for \
248  default(none)
249  for (int z = min_index(2); z <= max_index(2); ++z)
250  {
251  for (int y = min_index(1); y <= max_index(1); ++y)
252  {
253  for (int x = min_index(0); x <= max_index(0); ++x)
254  {
255  // linear voxel index in volume and index in descriptor vector
256  Eigen::Vector3i point (x,y,z);
257  int volume_idx = getLinearVoxelIndex (point);
258  int descr_idx = offset_vector * (point - min_index);
259 
260 /* std::cout << "linear index " << volume_idx << std::endl;
261  std::cout << "weight " << weights_->at (volume_idx) << std::endl;
262  std::cout << "volume " << volume_->at (volume_idx) << std::endl;
263  std::cout << "descr " << neighborhood.rows() << "x" << neighborhood.cols() << ", val = " << neighborhood << std::endl;
264  std::cout << "descr index = " << descr_idx << std::endl;
265 */
266  // get the TSDF value and store as descriptor entry
267  if (weights_->at (volume_idx) != 0)
268  neighborhood (descr_idx) = volume_->at (volume_idx);
269  else
270  neighborhood (descr_idx) = -1.0; // if never visited we assume inside of object (outside captured and thus filled with positive values)
271  }
272  }
273  }
274 
275  return true;
276 }
277 
278 
279 template <typename VoxelT, typename WeightT> bool
280 pcl::TSDFVolume<VoxelT, WeightT>::addNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size,
281  const VoxelTVec &neighborhood, WeightT voxel_weight)
282 {
283  // point_index is at the center of a cube of scale_ x scale_ x scale_ voxels
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;
287 
288  // check that index is within range
289  if (getLinearVoxelIndex(min_index) < 0 || getLinearVoxelIndex(max_index) >= (int)size())
290  {
291  pcl::console::print_info ("[addNeighborhood] Skipping voxel with coord (%d, %d, %d).\n", voxel_coord(0), voxel_coord(1), voxel_coord(2));
292  return false;
293  }
294 
295  // static const int descriptor_size = neighborhood_size*neighborhood_size*neighborhood_size;
296  const Eigen::RowVector3i offset_vector (1, neighborhood_size, neighborhood_size*neighborhood_size);
297 
298  Eigen::Vector3i index = min_index;
299  // loop over all voxels in 3D neighborhood
300  #pragma omp parallel for \
301  default(none)
302  for (int z = min_index(2); z <= max_index(2); ++z)
303  {
304  for (int y = min_index(1); y <= max_index(1); ++y)
305  {
306  for (int x = min_index(0); x <= max_index(0); ++x)
307  {
308  // linear voxel index in volume and index in descriptor vector
309  Eigen::Vector3i point (x,y,z);
310  int volume_idx = getLinearVoxelIndex (point);
311  int descr_idx = offset_vector * (point - min_index);
312 
313  // add the descriptor entry to the volume
314  VoxelT &voxel = volume_->at (volume_idx);
315  WeightT &weight = weights_->at (volume_idx);
316 
317  // TODO check that this simple lock works correctly!!
318  #pragma omp atomic
319  voxel += neighborhood (descr_idx);
320 
321  #pragma omp atomic
322  weight += voxel_weight;
323  }
324  }
325  }
326 
327  return true;
328 }
329 
330 template <typename VoxelT, typename WeightT> void
332 {
333  #pragma omp parallel for \
334  default(none)
335  for (std::size_t i = 0; i < volume_->size(); ++i)
336  {
337  WeightT &w = weights_->at(i);
338  if (w > 0.0)
339  {
340  volume_->at(i) /= w;
341  w = 1.0;
342  }
343  }
344 }
345 
346 #define PCL_INSTANTIATE_TSDFVolume(VT,WT) template class PCL_EXPORTS pcl::reconstruction::TSDFVolume<VT,WT>;
347 
348 #endif /* TSDF_VOLUME_HPP_ */
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:663
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:885
void reserve(std::size_t n)
Definition: point_cloud.h:445
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
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
Definition: tsdf_volume.h:66
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.
Definition: tsdf_volume.hpp:46
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.
Structure storing voxel grid resolution, volume size (in mm) and element_size of stored data.
Definition: tsdf_volume.h:70