Point Cloud Library (PCL) 1.12.0
tsdf_volume.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2011, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37
38#pragma once
39
40#include <pcl/memory.h>
41#include <pcl/pcl_macros.h>
42#include <pcl/gpu/containers/device_array.h>
43#include <pcl/point_types.h>
44#include <pcl/point_cloud.h>
45#include <Eigen/Core>
46#include <vector>
47
48#include <pcl/gpu/kinfu_large_scale/tsdf_buffer.h>
49
50#include <pcl/gpu/kinfu_large_scale/point_intensity.h>
51
52
53namespace pcl
54{
55 namespace gpu
56 {
57 namespace kinfuLS
58 {
59 /** \brief TsdfVolume class
60 * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
61 */
63 {
64 public:
65 using Ptr = shared_ptr<TsdfVolume>;
66 using ConstPtr = shared_ptr<const TsdfVolume>;
67
68 /** \brief Supported Point Types */
71
72 /** \brief Structure storing voxel grid resolution, volume size (in mm) and element_size of data stored on host*/
73 struct Header
74 {
75 Eigen::Vector3i resolution;
76 Eigen::Vector3f volume_size;
77 int volume_element_size, weights_element_size;
78
80 : resolution (0,0,0),
81 volume_size (0,0,0),
82 volume_element_size (sizeof(float)),
83 weights_element_size (sizeof(short))
84 {};
85
86 Header (const Eigen::Vector3i &res, const Eigen::Vector3f &size)
87 : resolution (res),
88 volume_size (size),
89 volume_element_size (sizeof(float)),
90 weights_element_size (sizeof(short))
91 {};
92
93 /** \brief Get the size of data stored on host*/
94 inline std::size_t
95 getVolumeSize () const { return resolution[0] * resolution[1] * resolution[2]; };
96
97 friend inline std::ostream&
98 operator << (std::ostream& os, const Header& h)
99 {
100 os << "(resolution = " << h.resolution.transpose() << ", volume size = " << h.volume_size.transpose() << ")";
101 return (os);
102 }
103
104 public:
106 };
107
108 /** \brief Default buffer size for fetching cloud. It limits max number of points that can be extracted */
109 enum { DEFAULT_CLOUD_BUFFER_SIZE = 10 * 1000 * 1000 };
110
111 /** \brief Constructor
112 * \param[in] resolution volume resolution
113 */
114 TsdfVolume (const Eigen::Vector3i& resolution);
115
116 /** \brief Sets Tsdf volume size for each dimension
117 * \param[in] size size of tsdf volume in meters
118 */
119 void
120 setSize (const Eigen::Vector3f& size);
121
122 /** \brief Sets Tsdf truncation distance. Must be greater than 2 * volume_voxel_size
123 * \param[in] distance TSDF truncation distance
124 */
125 void
127
128 /** \brief Returns tsdf volume container that point to data in GPU memory */
130 data () const;
131
132 /** \brief Returns volume size in meters */
133 const Eigen::Vector3f&
134 getSize () const;
135
136 /** \brief Returns volume resolution */
137 const Eigen::Vector3i&
139
140 /** \brief Returns volume voxel size in meters */
141 const Eigen::Vector3f
142 getVoxelSize () const;
143
144 /** \brief Returns tsdf truncation distance in meters */
145 float
147
148 /** \brief Resets tsdf volume data to uninitialized state */
149 void
151
152 /** \brief Generates cloud using CPU (downloads volumetric representation to CPU memory)
153 * \param[out] cloud output array for cloud
154 * \param[in] connected26 If false point cloud is extracted using 6 neighbor, otherwise 26.
155 */
156 void
157 fetchCloudHost (PointCloud<PointType>& cloud, bool connected26 = false) const;
158
159 /** \brief Generates cloud using CPU (downloads volumetric representation to CPU memory)
160 * \param[out] cloud output array for cloud
161 * \param[in] connected26 If false point cloud is extracted using 6 neighbor, otherwise 26.
162 */
163 void
164 fetchCloudHost (PointCloud<PointXYZI>& cloud, bool connected26 = false) const;
165
166 /** \brief Generates cloud using GPU in connected6 mode only
167 * \param[out] cloud_buffer buffer to store point cloud
168 * \return DeviceArray with disabled reference counting that points to filled part of cloud_buffer.
169 */
171 fetchCloud (DeviceArray<PointType>& cloud_buffer) const;
172
173 /** \brief Push a point cloud of previously scanned tsdf slice to the TSDF volume
174 * \param[in] existing_data_cloud point cloud pointer to the existing data. This data will be pushed to the TSDf volume. The points with indices outside the range [0 ... VOLUME_X - 1][0 ... VOLUME_Y - 1][0 ... VOLUME_Z - 1] will not be added.
175 * \param buffer
176 */
177 void
178 pushSlice (const PointCloud<PointXYZI>::Ptr existing_data_cloud, const tsdf_buffer* buffer) const;
179
180 /** \brief Generates cloud using GPU in connected6 mode only
181 * \param[out] cloud_buffer_xyz buffer to store point cloud
182 * \param cloud_buffer_intensity
183 * \param[in] buffer Pointer to the buffer struct that contains information about memory addresses of the tsdf volume memory block, which are used for the cyclic buffer.
184 * \param[in] shiftX Offset in indices.
185 * \param[in] shiftY Offset in indices.
186 * \param[in] shiftZ Offset in indices.
187 * \return DeviceArray with disabled reference counting that points to filled part of cloud_buffer.
188 */
189 std::size_t
190 fetchSliceAsCloud (DeviceArray<PointType>& cloud_buffer_xyz, DeviceArray<float>& cloud_buffer_intensity, const tsdf_buffer* buffer, int shiftX, int shiftY, int shiftZ ) const;
191
192 /** \brief Computes normals as gradient of tsdf for given points
193 * \param[in] cloud Points where normals are computed.
194 * \param[out] normals array for normals
195 */
196
197 void
199
200 /** \brief Computes normals as gradient of tsdf for given points
201 * \param[in] cloud Points where normals are computed.
202 * \param[out] normals array for normals
203 */
204 void
206
207 /** \brief Downloads tsdf volume from GPU memory.
208 * \param[out] tsdf Array with tsdf values. if volume resolution is 512x512x512, so for voxel (x,y,z) tsdf value can be retrieved as volume[512*512*z + 512*y + x];
209 */
210 void
211 downloadTsdf (std::vector<float>& tsdf) const;
212
213 /** \brief Downloads tsdf volume from GPU memory to local CPU buffer*/
214 void
216
217 /** \brief Downloads TSDF volume and according voxel weights from GPU memory
218 * \param[out] tsdf Array with tsdf values. if volume resolution is 512x512x512, so for voxel (x,y,z) tsdf value can be retrieved as volume[512*512*z + 512*y + x];
219 * \param[out] weights Array with tsdf voxel weights. Same size and access index as for tsdf. A weight of 0 indicates the voxel was never used.
220 */
221 void
222 downloadTsdfAndWeights (std::vector<float>& tsdf, std::vector<short>& weights) const;
223
224 /** \brief Downloads TSDF volume and according voxel weights from GPU memory to local CPU buffers*/
225 void
227
228 /** \brief Releases tsdf buffer on GPU */
229 void releaseVolume () {volume_.release();}
230
231 void print_warn(const char* arg1, std::size_t size);
232
233 /** \brief Set the header for data stored on host directly. Useful if directly writing into volume and weights */
234 inline void
235 setHeader (const Eigen::Vector3i& resolution, const Eigen::Vector3f& volume_size) {
236 header_ = Header (resolution, volume_size);
237 if (volume_host_->size() != this->size())
238 pcl::console::print_warn ("[TSDFVolume::setHeader] Header volume size (%d) doesn't fit underlying data size (%d)", volume_host_->size(), size());
239 }
240
241 /** \brief Returns overall number of voxels in grid stored on host */
242 inline std::size_t
243 size () const {
244 return header_.getVolumeSize ();
245 }
246
247 /** \brief Converts volume stored on host to cloud of TSDF values
248 * \param[out] cloud - the output point cloud
249 * \param[in] step - the decimation step to use
250 */
251 void
253 const unsigned step = 2) const;
254
255 /** \brief Returns the voxel grid resolution */
256 inline const Eigen::Vector3i &
257 gridResolution () const { return header_.resolution; };
258
259 /** \brief Saves local volume buffer to file */
260 bool
261 save (const std::string &filename = "tsdf_volume.dat", bool binary = true) const;
262
263 /** \brief Loads local volume from file */
264 bool
265 load (const std::string &filename, bool binary = true);
266
267 private:
268 /** \brief tsdf volume size in meters */
269 Eigen::Vector3f size_;
270
271 /** \brief tsdf volume resolution */
272 Eigen::Vector3i resolution_;
273
274 /** \brief tsdf volume data container */
275 DeviceArray2D<int> volume_;
276
277 /** \brief tsdf truncation distance */
278 float tranc_dist_;
279
280 // The following member are resulting from the merge of TSDFVolume with TsdfVolume class.
281 using VolumePtr = shared_ptr<std::vector<float> >;
282 using WeightsPtr = shared_ptr<std::vector<short> >;
283
284 Header header_;
285 VolumePtr volume_host_;
286 WeightsPtr weights_host_;
287
288 public:
290 };
291 }
292 }
293}
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
void downloadTsdfAndWeights(std::vector< float > &tsdf, std::vector< short > &weights) const
Downloads TSDF volume and according voxel weights from GPU memory.
void releaseVolume()
Releases tsdf buffer on GPU.
Definition: tsdf_volume.h:229
void downloadTsdfLocal() const
Downloads tsdf volume from GPU memory to local CPU buffer.
void setHeader(const Eigen::Vector3i &resolution, const Eigen::Vector3f &volume_size)
Set the header for data stored on host directly.
Definition: tsdf_volume.h:235
void fetchNormals(const DeviceArray< PointType > &cloud, DeviceArray< NormalType > &normals) const
Computes normals as gradient of tsdf for given points.
void pushSlice(const PointCloud< PointXYZI >::Ptr existing_data_cloud, const tsdf_buffer *buffer) const
Push a point cloud of previously scanned tsdf slice to the TSDF volume.
void setSize(const Eigen::Vector3f &size)
Sets Tsdf volume size for each dimension.
void downloadTsdf(std::vector< float > &tsdf) const
Downloads tsdf volume from GPU memory.
DeviceArray2D< int > data() const
Returns tsdf volume container that point to data in GPU memory.
std::size_t size() const
Returns overall number of voxels in grid stored on host.
Definition: tsdf_volume.h:243
bool load(const std::string &filename, bool binary=true)
Loads local volume from file.
void convertToTsdfCloud(pcl::PointCloud< pcl::PointXYZI >::Ptr &cloud, const unsigned step=2) const
Converts volume stored on host to cloud of TSDF values.
DeviceArray< PointType > fetchCloud(DeviceArray< PointType > &cloud_buffer) const
Generates cloud using GPU in connected6 mode only.
shared_ptr< TsdfVolume > Ptr
Definition: tsdf_volume.h:65
const Eigen::Vector3i & gridResolution() const
Returns the voxel grid resolution.
Definition: tsdf_volume.h:257
void fetchCloudHost(PointCloud< PointType > &cloud, bool connected26=false) const
Generates cloud using CPU (downloads volumetric representation to CPU memory)
const Eigen::Vector3i & getResolution() const
Returns volume resolution.
void fetchCloudHost(PointCloud< PointXYZI > &cloud, bool connected26=false) const
Generates cloud using CPU (downloads volumetric representation to CPU memory)
shared_ptr< const TsdfVolume > ConstPtr
Definition: tsdf_volume.h:66
TsdfVolume(const Eigen::Vector3i &resolution)
Constructor.
void reset()
Resets tsdf volume data to uninitialized state.
void setTsdfTruncDist(float distance)
Sets Tsdf truncation distance.
void print_warn(const char *arg1, std::size_t size)
const Eigen::Vector3f & getSize() const
Returns volume size in meters.
const Eigen::Vector3f getVoxelSize() const
Returns volume voxel size in meters.
void downloadTsdfAndWeightsLocal() const
Downloads TSDF volume and according voxel weights from GPU memory to local CPU buffers.
std::size_t fetchSliceAsCloud(DeviceArray< PointType > &cloud_buffer_xyz, DeviceArray< float > &cloud_buffer_intensity, const tsdf_buffer *buffer, int shiftX, int shiftY, int shiftZ) const
Generates cloud using GPU in connected6 mode only.
void fetchNormals(const DeviceArray< PointType > &cloud, DeviceArray< PointType > &normals) const
Computes normals as gradient of tsdf for given points.
float getTsdfTruncDist() const
Returns tsdf truncation distance in meters.
bool save(const std::string &filename="tsdf_volume.dat", bool binary=true) const
Saves local volume buffer to file.
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.
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
std::ostream & operator<<(std::ostream &os, const float3 &v1)
Defines all the PCL and non-PCL macros used.
#define PCL_EXPORTS
Definition: pcl_macros.h:323
A point structure representing normal coordinates and the surface curvature estimate.
A point structure representing Euclidean xyz coordinates.
Structure storing voxel grid resolution, volume size (in mm) and element_size of data stored on host.
Definition: tsdf_volume.h:74
Header(const Eigen::Vector3i &res, const Eigen::Vector3f &size)
Definition: tsdf_volume.h:86
std::size_t getVolumeSize() const
Get the size of data stored on host.
Definition: tsdf_volume.h:95
Structure to handle buffer addresses.
Definition: tsdf_buffer.h:51