Point Cloud Library (PCL)  1.9.1-dev
tsdf_volume.h
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: tsdf_volume.h 6459 2012-07-18 07:50:37Z dpb $
35  */
36 
37 #pragma once
38 
39 #include <pcl/pcl_macros.h>
40 #include <pcl/point_cloud.h>
41 #include <pcl/point_types.h>
42 #include <pcl/console/print.h>
43 
44 
45 #define DEFAULT_GRID_RES_X 512 // pcl::device::VOLUME_X ( and _Y, _Z)
46 #define DEFAULT_GRID_RES_Y 512
47 #define DEFAULT_GRID_RES_Z 512
48 
49 #define DEFAULT_VOLUME_SIZE_X 3000
50 #define DEFAULT_VOLUME_SIZE_Y 3000
51 #define DEFAULT_VOLUME_SIZE_Z 3000
52 
53 
54 namespace pcl
55 {
56  template <typename VoxelT, typename WeightT>
57  class TSDFVolume
58  {
59  public:
60 
61  using Ptr = boost::shared_ptr<TSDFVolume<VoxelT, WeightT> >;
62  using ConstPtr = boost::shared_ptr<const TSDFVolume<VoxelT, WeightT> >;
63 
64  // using VoxelTVec = Eigen::Matrix<VoxelT, Eigen::Dynamic, 1>;
65  using VoxelTVec = Eigen::VectorXf;
66 
67  /** \brief Structure storing voxel grid resolution, volume size (in mm) and element_size of stored data */
68  struct Header
69  {
70  Eigen::Vector3i resolution;
71  Eigen::Vector3f volume_size;
73 
74  Header ()
75  : resolution (0,0,0),
76  volume_size (0,0,0),
77  volume_element_size (sizeof(VoxelT)),
78  weights_element_size (sizeof(WeightT))
79  {};
80 
81  Header (const Eigen::Vector3i &res, const Eigen::Vector3f &size)
82  : resolution (res),
83  volume_size (size),
84  volume_element_size (sizeof(VoxelT)),
85  weights_element_size (sizeof(WeightT))
86  {};
87 
88  inline size_t
89  getVolumeSize () const { return resolution[0] * resolution[1] * resolution[2]; };
90 
91  friend inline std::ostream&
92  operator << (std::ostream& os, const Header& h)
93  {
94  os << "(resolution = " << h.resolution.transpose() << ", volume size = " << h.volume_size.transpose() << ")";
95  return (os);
96  }
97 
98 public:
100 
101  };
102 
103  #define DEFAULT_TRANCATION_DISTANCE 30.0f
104 
105  /** \brief Camera intrinsics structure
106  */
107  struct Intr
108  {
109  float fx, fy, cx, cy;
110  Intr () {};
111  Intr (float fx_, float fy_, float cx_, float cy_)
112  : fx(fx_), fy(fy_), cx(cx_), cy(cy_) {};
113 
114  Intr operator()(int level_index) const
115  {
116  int div = 1 << level_index;
117  return (Intr (fx / div, fy / div, cx / div, cy / div));
118  }
119 
120  friend inline std::ostream&
121  operator << (std::ostream& os, const Intr& intr)
122  {
123  os << "([f = " << intr.fx << ", " << intr.fy << "] [cp = " << intr.cx << ", " << intr.cy << "])";
124  return (os);
125  }
126 
127  };
128 
129 
130  ////////////////////////////////////////////////////////////////////////////////////////
131  // Constructors
132 
133  /** \brief Default constructor */
135  : volume_ (new std::vector<VoxelT>),
136  weights_ (new std::vector<WeightT>)
137  {};
138 
139  /** \brief Constructor loading data from file */
140  TSDFVolume (const std::string &filename)
141  : volume_ (new std::vector<VoxelT>),
142  weights_ (new std::vector<WeightT>)
143  {
144  if (load (filename))
145  std::cout << "done [" << size() << "]" << std::endl;
146  else
147  std::cout << "error!" << std::endl;
148  };
149 
150  /** \brief Set the header directly. Useful if directly writing into volume and weights */
151  inline void
152  setHeader (const Eigen::Vector3i &resolution, const Eigen::Vector3f &volume_size) {
153  header_ = Header (resolution, volume_size);
154  if (volume_->size() != this->size())
155  pcl::console::print_warn ("[TSDFVolume::setHeader] Header volume size (%d) doesn't fit underlying data size (%d)", volume_->size(), size());
156  };
157 
158  /** \brief Resizes the internal storage and updates the header accordingly */
159  inline void
160  resize (Eigen::Vector3i &grid_resolution, const Eigen::Vector3f& volume_size = Eigen::Vector3f (DEFAULT_VOLUME_SIZE_X, DEFAULT_VOLUME_SIZE_Y, DEFAULT_VOLUME_SIZE_Z)) {
161  int lin_size = grid_resolution[0] * grid_resolution[1] * grid_resolution[2];
162  volume_->resize (lin_size);
163  weights_->resize (lin_size);
164  setHeader (grid_resolution, volume_size);
165  };
166 
167  /** \brief Resize internal storage and header to default sizes defined in tsdf_volume.h */
168  inline void
170  resize (Eigen::Vector3i (DEFAULT_GRID_RES_X, DEFAULT_GRID_RES_Y, DEFAULT_GRID_RES_Z),
171  Eigen::Vector3f (DEFAULT_VOLUME_SIZE_X, DEFAULT_VOLUME_SIZE_Y, DEFAULT_VOLUME_SIZE_Z));
172  };
173 
174  ////////////////////////////////////////////////////////////////////////////////////////
175  // Storage and element access
176 
177  /** \brief Loads volume from file */
178  bool
179  load (const std::string &filename, bool binary = true);
180 
181  /** \brief Saves volume to file */
182  bool
183  save (const std::string &filename = "tsdf_volume.dat", bool binary = true) const;
184 
185  /** \brief Returns overall number of voxels in grid */
186  inline size_t
187  size () const { return header_.getVolumeSize(); };
188 
189  /** \brief Returns the volume size in mm */
190  inline const Eigen::Vector3f &
191  volumeSize () const { return header_.volume_size; };
192 
193  /** \brief Returns the size of one voxel in mm */
194  inline Eigen::Vector3f
195  voxelSize () const {
196  Eigen::Array3f res = header_.resolution.array().template cast<float>();
197  return header_.volume_size.array() / res;
198  };
199 
200  /** \brief Returns the voxel grid resolution */
201  inline const Eigen::Vector3i &
202  gridResolution() const { return header_.resolution; };
203 
204  /** \brief Returns constant reference to header */
205  inline const Header &
206  header () const { return header_; };
207 
208  /** \brief Returns constant reference to the volume std::vector */
209  inline const std::vector<VoxelT> &
210  volume () const { return *volume_; };
211 
212  /** \brief Returns writebale(!) reference to volume */
213  inline std::vector<VoxelT> &
214  volumeWriteable () const { return *volume_; };
215 
216  /** \brief Returns constant reference to the weights std::vector */
217  inline const std::vector<WeightT> &
218  weights () const { return *weights_; };
219 
220  /** \brief Returns writebale(!) reference to volume */
221  inline std::vector<WeightT> &
222  weightsWriteable () const { return *weights_; };
223 
224  ////////////////////////////////////////////////////////////////////////////////////////
225  // Functionality
226 
227  /** \brief Converts volume to cloud of TSDF values
228  * \param[ou] cloud - the output point cloud
229  * \param[in] step - the decimation step to use
230  */
231  void
233  const unsigned step = 2) const;
234 
235  /** \brief Converts the volume to a surface representation via a point cloud */
236  // void
237  // convertToCloud (pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) const;
238 
239  /** \brief Crate Volume from Point Cloud */
240  // template <typename PointT> void
241  // createFromCloud (const typename pcl::PointCloud<PointT>::ConstPtr &cloud, const Intr &intr);
242 
243  /** \brief Returns the 3D voxel coordinate */
244  template <typename PointT> void
245  getVoxelCoord (const PointT &point, Eigen::Vector3i &voxel_coord) const;
246 
247  /** \brief Returns the 3D voxel coordinate and point offset wrt. to the voxel center (in mm) */
248  template <typename PointT> void
249  getVoxelCoordAndOffset (const PointT &point, Eigen::Vector3i &voxel_coord, Eigen::Vector3f &offset) const;
250 
251  /** extracts voxels in neighborhood of given voxel */
252  bool
253  extractNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size, VoxelTVec &neighborhood) const;
254 
255  /** adds voxel values in local neighborhood */
256  bool
257  addNeighborhood (const Eigen::Vector3i &voxel_coord, int neighborhood_size, const VoxelTVec &neighborhood, WeightT voxel_weight);
258 
259  /** averages voxel values by the weight value */
260  void
261  averageValues ();
262 
263  /** \brief Returns and index for linear access of the volume and weights */
264  inline int
265  getLinearVoxelIndex (const Eigen::Array3i &indices) const {
266  return indices(0) + indices(1) * header_.resolution[0] + indices(2) * header_.resolution[0] * header_.resolution[1];
267  }
268 
269  /** \brief Returns a vector of linear indices for voxel coordinates given in 3xn matrix */
270  inline Eigen::VectorXi
271  getLinearVoxelIndinces (const Eigen::Matrix<int, 3, Eigen::Dynamic> &indices_matrix) const {
272  return (Eigen::RowVector3i (1, header_.resolution[0], header_.resolution[0] * header_.resolution[1]) * indices_matrix).transpose();
273  }
274 
275  private:
276 
277  ////////////////////////////////////////////////////////////////////////////////////////
278  // Private functions and members
279 
280  // void
281  // scaleDepth (const Eigen::MatrixXf &depth, Eigen::MatrixXf &depth_scaled, const Intr &intr) const;
282 
283  // void
284  // integrateVolume (const Eigen::MatrixXf &depth_scaled, float tranc_dist, const Eigen::Matrix3f &R_inv, const Eigen::Vector3f &t, const Intr &intr);
285 
286  using VolumePtr = boost::shared_ptr<std::vector<VoxelT> >;
287  using WeightsPtr = boost::shared_ptr<std::vector<WeightT> >;
288 
289  Header header_;
290  VolumePtr volume_;
291  WeightsPtr weights_;
292 public:
294 
295  };
296 
297 }
size_t getVolumeSize() const
Definition: tsdf_volume.h:89
size_t size() const
Returns overall number of voxels in grid.
Definition: tsdf_volume.h:187
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
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
void setHeader(const Eigen::Vector3i &resolution, const Eigen::Vector3f &volume_size)
Set the header directly.
Definition: tsdf_volume.h:152
std::vector< WeightT > & weightsWriteable() const
Returns writebale(!) reference to volume.
Definition: tsdf_volume.h:222
void convertToTsdfCloud(pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, const unsigned step=2) const
Converts volume to cloud of TSDF values.
const Eigen::Vector3i & gridResolution() const
Returns the voxel grid resolution.
Definition: tsdf_volume.h:202
friend std::ostream & operator<<(std::ostream &os, const Header &h)
Definition: tsdf_volume.h:92
const std::vector< VoxelT > & volume() const
Returns constant reference to the volume std::vector.
Definition: tsdf_volume.h:210
const Header & header() const
Returns constant reference to header.
Definition: tsdf_volume.h:206
Structure storing voxel grid resolution, volume size (in mm) and element_size of stored data...
Definition: tsdf_volume.h:68
Eigen::Vector3f voxelSize() const
Returns the size of one voxel in mm.
Definition: tsdf_volume.h:195
void resizeDefaultSize()
Resize internal storage and header to default sizes defined in tsdf_volume.h.
Definition: tsdf_volume.h:169
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:345
Header(const Eigen::Vector3i &res, const Eigen::Vector3f &size)
Definition: tsdf_volume.h:81
void averageValues()
averages voxel values by the weight value
const Eigen::Vector3f & volumeSize() const
Returns the volume size in mm.
Definition: tsdf_volume.h:191
Eigen::Vector3f volume_size
Definition: tsdf_volume.h:71
bool extractNeighborhood(const Eigen::Vector3i &voxel_coord, int neighborhood_size, VoxelTVec &neighborhood) const
extracts voxels in neighborhood of given voxel
Defines all the PCL implemented PointT point type structures.
std::vector< VoxelT > & volumeWriteable() const
Returns writebale(!) reference to volume.
Definition: tsdf_volume.h:214
Intr operator()(int level_index) const
Definition: tsdf_volume.h:114
const std::vector< WeightT > & weights() const
Returns constant reference to the weights std::vector.
Definition: tsdf_volume.h:218
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:444
void getVoxelCoordAndOffset(const PointT &point, Eigen::Vector3i &voxel_coord, Eigen::Vector3f &offset) const
Returns the 3D voxel coordinate and point offset wrt.
Eigen::Vector3i resolution
Definition: tsdf_volume.h:70
PCL_EXPORTS void print_warn(const char *format,...)
Print a warning message on stream with colors.
Eigen::VectorXf VoxelTVec
Definition: tsdf_volume.h:65
TSDFVolume(const std::string &filename)
Constructor loading data from file.
Definition: tsdf_volume.h:140
boost::shared_ptr< TSDFVolume< VoxelT, WeightT > > Ptr
Definition: tsdf_volume.h:61
boost::shared_ptr< const TSDFVolume< VoxelT, WeightT > > ConstPtr
Definition: tsdf_volume.h:62
A point structure representing Euclidean xyz coordinates, and the RGB color.
int getLinearVoxelIndex(const Eigen::Array3i &indices) const
Returns and index for linear access of the volume and weights.
Definition: tsdf_volume.h:265
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.
Definition: tsdf_volume.h:160
bool save(const std::string &filename="tsdf_volume.dat", bool binary=true) const
Saves volume to file.
Intr(float fx_, float fy_, float cx_, float cy_)
Definition: tsdf_volume.h:111
Defines all the PCL and non-PCL macros used.
Camera intrinsics structure.
Definition: tsdf_volume.h:107
TSDFVolume()
Default constructor.
Definition: tsdf_volume.h:134
void getVoxelCoord(const PointT &point, Eigen::Vector3i &voxel_coord) const
Converts the volume to a surface representation via a point cloud.
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.
Definition: tsdf_volume.h:271