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