Point Cloud Library (PCL)  1.9.1-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 //#pragma omp parallel for // if used, increment over idx not possible! use index calculation
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)
175  {
176  volume_idx = sx*sy*z + sx*y + x;
177  // pcl::PointXYZI &point = cloud->points[cloud_idx];
178 
179  if (weights_->at(volume_idx) == 0 || volume_->at(volume_idx) > 0.98 )
180  continue;
181 
182  pcl::PointXYZI point;
183  point.x = x; point.y = y; point.z = z;//*64;
184  point.intensity = volume_->at(volume_idx);
185  cloud->push_back (point);
186  }
187 
188  // cloud->width = cloud_size;
189  // cloud->height = 1;
190 }
191 
192 
193 template <typename VoxelT, typename WeightT> template <typename PointT> void
194 pcl::TSDFVolume<VoxelT, WeightT>::getVoxelCoord (const PointT &point, Eigen::Vector3i &coord) const
195 {
196  static Eigen::Array3f voxel_size = voxelSize().array();
197 
198  // point coordinates in world coordinate frame and voxel coordinates
199  Eigen::Array3f point_coord (point.x, point.y, point.z);
200  Eigen::Array3f voxel_coord = (point_coord / voxel_size) - 0.5f; // 0.5f offset due to voxel center vs grid
201  coord(0) = round(voxel_coord(0));
202  coord(1) = round(voxel_coord(1));
203  coord(2) = round(voxel_coord(2));
204 }
205 
206 
207 /** \brief Returns the 3D voxel coordinate and point offset wrt. to the voxel center (in mm) */
208 template <typename VoxelT, typename WeightT> template <typename PointT> void
210  Eigen::Vector3i &coord, Eigen::Vector3f &offset) const
211 {
212  static Eigen::Array3f voxel_size = voxelSize().array();
213 
214  // point coordinates in world coordinate frame and voxel coordinates
215  Eigen::Array3f point_coord (point.x, point.y, point.z);
216  Eigen::Array3f voxel_coord = (point_coord / voxel_size) - 0.5f; // 0.5f offset due to voxel center vs grid
217  coord(0) = round(voxel_coord(0));
218  coord(1) = round(voxel_coord(1));
219  coord(2) = round(voxel_coord(2));
220 
221  // offset of point wrt. to voxel center
222  offset = (voxel_coord - coord.cast<float>().array() * voxel_size).matrix();
223 }
224 
225 
226 template <typename VoxelT, typename WeightT> bool
227 pcl::TSDFVolume<VoxelT, WeightT>::extractNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size,
228  VoxelTVec &neighborhood) const
229 {
230  // point_index is at the center of a cube of scale_ x scale_ x scale_ voxels
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;
234 
235  // check that index is within range
236  if (getLinearVoxelIndex(min_index) < 0 || getLinearVoxelIndex(max_index) >= (int)size())
237  {
238  pcl::console::print_info ("[extractNeighborhood] Skipping voxel with coord (%d, %d, %d).\n", voxel_coord(0), voxel_coord(1), voxel_coord(2));
239  return false;
240  }
241 
242  static const int descriptor_size = neighborhood_size*neighborhood_size*neighborhood_size;
243  neighborhood.resize (descriptor_size);
244 
245  const Eigen::RowVector3i offset_vector (1, neighborhood_size, neighborhood_size*neighborhood_size);
246 
247  // loop over all voxels in 3D neighborhood
248  #pragma omp parallel for
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  for (int z = min_index(2); z <= max_index(2); ++z)
302  {
303  for (int y = min_index(1); y <= max_index(1); ++y)
304  {
305  for (int x = min_index(0); x <= max_index(0); ++x)
306  {
307  // linear voxel index in volume and index in descriptor vector
308  Eigen::Vector3i point (x,y,z);
309  int volume_idx = getLinearVoxelIndex (point);
310  int descr_idx = offset_vector * (point - min_index);
311 
312  // add the descriptor entry to the volume
313  VoxelT &voxel = volume_->at (volume_idx);
314  WeightT &weight = weights_->at (volume_idx);
315 
316  // TODO check that this simple lock works correctly!!
317  #pragma omp atomic
318  voxel += neighborhood (descr_idx);
319 
320  #pragma omp atomic
321  weight += voxel_weight;
322  }
323  }
324  }
325 
326  return true;
327 }
328 
329 template <typename VoxelT, typename WeightT> void
331 {
332  #pragma omp parallel for
333  for (size_t i = 0; i < volume_->size(); ++i)
334  {
335  WeightT &w = weights_->at(i);
336  if (w > 0.0)
337  {
338  volume_->at(i) /= w;
339  w = 1.0;
340  }
341  }
342 }
343 
344 #define PCL_INSTANTIATE_TSDFVolume(VT,WT) template class PCL_EXPORTS pcl::reconstruction::TSDFVolume<VT,WT>;
345 
346 #endif /* TSDF_VOLUME_HPP_ */
void reserve(size_t n)
Definition: point_cloud.h:462
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.
Definition: tsdf_volume.hpp:46
PCL_EXPORTS void print_info(const char *format,...)
Print an info message on stream with colors.
void convertToTsdfCloud(pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, const unsigned step=2) const
Converts volume to cloud of TSDF values.
Structure storing voxel grid resolution, volume size (in mm) and element_size of stored data...
Definition: tsdf_volume.h:68
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:493
void averageValues()
averages voxel values by the weight value
bool extractNeighborhood(const Eigen::Vector3i &voxel_coord, int neighborhood_size, VoxelTVec &neighborhood) const
extracts voxels in neighborhood of given voxel
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:441
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:618
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:65
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.