Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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 */
62 class PCL_EXPORTS TsdfVolume
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
126 setTsdfTruncDist (float distance);
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.
shared_ptr< PointCloud< PointT > > Ptr
DeviceArray2D class
DeviceArray class
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.
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.
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.
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.
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.
Defines all the PCL and non-PCL macros used.
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