Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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
55namespace pcl
56{
57 template <typename VoxelT, typename WeightT>
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
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),
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
99public:
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_;
293public:
295
296 };
297
298}
shared_ptr< PointCloud< PointT > > Ptr
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.
std::size_t size() const
Returns overall number of voxels in grid.
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
const Header & header() const
Returns constant reference to header.
void getVoxelCoordAndOffset(const PointT &point, Eigen::Vector3i &voxel_coord, Eigen::Vector3f &offset) const
Returns the 3D voxel coordinate and point offset wrt.
const Eigen::Vector3f & volumeSize() const
Returns the volume size in mm.
void setHeader(const Eigen::Vector3i &resolution, const Eigen::Vector3f &volume_size)
Set the header directly.
Eigen::VectorXf VoxelTVec
Definition tsdf_volume.h:66
const Eigen::Vector3i & gridResolution() const
Returns the voxel grid resolution.
Eigen::Vector3f voxelSize() const
Returns the size of one voxel in mm.
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.
TSDFVolume(const std::string &filename)
Constructor loading data from file.
std::vector< WeightT > & weightsWriteable() const
Returns writebale(!) reference to volume.
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.
void averageValues()
averages voxel values by the weight value
const std::vector< VoxelT > & volume() const
Returns constant reference to the volume std::vector.
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.
std::vector< VoxelT > & volumeWriteable() const
Returns writebale(!) reference to volume.
bool load(const std::string &filename, bool binary=true)
Loads volume from file.
void resizeDefaultSize()
Resize internal storage and header to default sizes defined in tsdf_volume.h.
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.
TSDFVolume()
Default constructor.
const std::vector< WeightT > & weights() const
Returns constant reference to the weights std::vector.
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.
friend std::ostream & operator<<(std::ostream &os, const Intr &intr)
Intr operator()(int level_index) const
Intr(float fx_, float fy_, float cx_, float cy_)