Point Cloud Library (PCL) 1.13.0
cyclical_buffer.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-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/point_types.h>
41#include <pcl/gpu/kinfu_large_scale/tsdf_volume.h>
42#include <pcl/gpu/kinfu_large_scale/tsdf_buffer.h>
43#include <Eigen/Core>
44//#include <boost/graph/buffer_concepts.hpp>
45#include <cuda_runtime.h>
46#include <pcl/gpu/kinfu_large_scale/point_intensity.h>
47
48#include <pcl/gpu/kinfu_large_scale/world_model.h>
49
50
51#include <pcl/io/pcd_io.h>
52namespace pcl
53{
54 namespace gpu
55 {
56 namespace kinfuLS
57 {
58
59 /** \brief CyclicalBuffer implements a cyclical TSDF buffer.
60 * The class offers a simple interface, by handling shifts and maintaining the world autonomously.
61 * \author Raphael Favier, Francisco Heredia
62 */
64 {
65 public:
66
67
68 /** \brief Constructor for a cubic CyclicalBuffer.
69 * \param[in] distance_threshold distance between cube center and target point at which we decide to shift.
70 * \param[in] cube_size physical size (in meters) of the volume (here, a cube) represented by the TSDF buffer.
71 * \param[in] nb_voxels_per_axis number of voxels per axis of the volume represented by the TSDF buffer.
72 */
73 CyclicalBuffer (const double distance_threshold, const double cube_size = 3.f, const int nb_voxels_per_axis = 512)
74 {
75 distance_threshold_ = distance_threshold;
76 buffer_.volume_size.x = cube_size;
77 buffer_.volume_size.y = cube_size;
78 buffer_.volume_size.z = cube_size;
79 buffer_.voxels_size.x = nb_voxels_per_axis;
80 buffer_.voxels_size.y = nb_voxels_per_axis;
81 buffer_.voxels_size.z = nb_voxels_per_axis;
82 }
83
84
85 /** \brief Constructor for a non-cubic CyclicalBuffer.
86 * \param[in] distance_threshold distance between cube center and target point at which we decide to shift.
87 * \param[in] volume_size_x physical size (in meters) of the volume, X axis.
88 * \param[in] volume_size_y physical size (in meters) of the volume, Y axis.
89 * \param[in] volume_size_z physical size (in meters) of the volume, Z axis.
90 * \param[in] nb_voxels_x number of voxels for X axis of the volume represented by the TSDF buffer.
91 * \param[in] nb_voxels_y number of voxels for Y axis of the volume represented by the TSDF buffer.
92 * \param[in] nb_voxels_z number of voxels for Z axis of the volume represented by the TSDF buffer.
93 */
94 CyclicalBuffer (const double distance_threshold, const double volume_size_x, const double volume_size_y, const double volume_size_z, const int nb_voxels_x, const int nb_voxels_y, const int nb_voxels_z)
95 {
96 distance_threshold_ = distance_threshold;
97 buffer_.volume_size.x = volume_size_x;
98 buffer_.volume_size.y = volume_size_y;
99 buffer_.volume_size.z = volume_size_z;
100 buffer_.voxels_size.x = nb_voxels_x;
101 buffer_.voxels_size.y = nb_voxels_y;
102 buffer_.voxels_size.z = nb_voxels_z;
103 }
104
105 /** \brief Check if shifting needs to be performed, returns true if so.
106 Shifting is considered needed if the target point is farther than distance_threshold_.
107 The target point is located at distance_camera_point on the local Z axis of the camera.
108 * \param[in] volume pointer to the TSDFVolume living in GPU
109 * \param[in] cam_pose global pose of the camera in the world
110 * \param[in] distance_camera_target distance from the camera's origin to the target point
111 * \param[in] perform_shift if set to false, shifting is not performed. The function will return true if shifting is needed.
112 * \param[in] last_shift if set to true, the whole cube will be shifted. This is used to push the whole cube to the world model.
113 * \param[in] force_shift if set to true, shifting is forced.
114 * \return true is the cube needs to be or has been shifted.
115 */
116 bool checkForShift (const TsdfVolume::Ptr volume, const Eigen::Affine3f &cam_pose, const double distance_camera_target, const bool perform_shift = true, const bool last_shift = false, const bool force_shift = false);
117
118 /** \brief Perform shifting operations:
119 Compute offsets.
120 Extract current slice from TSDF buffer.
121 Extract existing data from world.
122 Clear shifted slice in TSDF buffer.
123 Push existing data into TSDF buffer.
124 Update rolling buffer
125 Update world model.
126 * \param[in] volume pointer to the TSDFVolume living in GPU
127 * \param[in] target_point target point around which the new cube will be centered
128 * \param[in] last_shift if set to true, the whole cube will be shifted. This is used to push the whole cube to the world model.
129 */
130 void performShift (const TsdfVolume::Ptr volume, const pcl::PointXYZ &target_point, const bool last_shift = false);
131
132 /** \brief Sets the distance threshold between cube's center and target point that triggers a shift.
133 * \param[in] threshold the distance in meters at which to trigger shift.
134 */
135 void setDistanceThreshold (const double threshold)
136 {
137 distance_threshold_ = threshold;
138 // PCL_INFO ("Shifting threshold set to %f meters.\n", distance_threshold_);
139 }
140
141 /** \brief Returns the distance threshold between cube's center and target point that triggers a shift. */
142 float getDistanceThreshold () { return (distance_threshold_); }
143
144 /** \brief get a pointer to the tsdf_buffer structure.
145 * \return a pointer to the tsdf_buffer used by cyclical buffer object.
146 */
147 tsdf_buffer* getBuffer () { return (&buffer_); }
148
149 /** \brief Set the physical size represented by the default TSDF volume.
150 * \param[in] size_x size of the volume on X axis, in meters.
151 * \param[in] size_y size of the volume on Y axis, in meters.
152 * \param[in] size_z size of the volume on Z axis, in meters.
153 */
154 void setVolumeSize (const double size_x, const double size_y, const double size_z)
155 {
156 buffer_.volume_size.x = size_x;
157 buffer_.volume_size.y = size_y;
158 buffer_.volume_size.z = size_z;
159 }
160
161 /** \brief Set the physical size represented by the default TSDF volume.
162 * \param[in] size size of the volume on all axis, in meters.
163 */
164 void setVolumeSize (const double size)
165 {
166 buffer_.volume_size.x = size;
167 buffer_.volume_size.y = size;
168 buffer_.volume_size.z = size;
169 }
170
171 /** \brief Computes and set the origin of the new cube (relative to the world), centered around a the target point.
172 * \param[in] target_point the target point around which the new cube will be centered.
173 * \param[out] shiftX shift on X axis (in indices).
174 * \param[out] shiftY shift on Y axis (in indices).
175 * \param[out] shiftZ shift on Z axis (in indices).
176 */
177 void computeAndSetNewCubeMetricOrigin (const pcl::PointXYZ &target_point, int &shiftX, int &shiftY, int &shiftZ);
178
179 /** \brief Initializes memory pointers of the cyclical buffer (start, end, current origin)
180 * \param[in] tsdf_volume pointer to the TSDF volume managed by this cyclical buffer
181 */
182 void initBuffer (TsdfVolume::Ptr tsdf_volume)
183 {
184 PtrStep<short2> localVolume = tsdf_volume->data();
185
186 buffer_.tsdf_memory_start = &(localVolume.ptr (0)[0]);
187 buffer_.tsdf_memory_end = &(localVolume.ptr (buffer_.voxels_size.y * (buffer_.voxels_size.z - 1) + (buffer_.voxels_size.y - 1) )[buffer_.voxels_size.x - 1]);
188 buffer_.tsdf_rolling_buff_origin = buffer_.tsdf_memory_start;
189 }
190
191 /** \brief Reset buffer structure
192 * \param[in] tsdf_volume pointer to the TSDF volume managed by this cyclical buffer
193 */
194 void resetBuffer (TsdfVolume::Ptr tsdf_volume)
195 {
196 buffer_.origin_GRID.x = 0; buffer_.origin_GRID.y = 0; buffer_.origin_GRID.z = 0;
197 buffer_.origin_GRID_global.x = 0.f; buffer_.origin_GRID_global.y = 0.f; buffer_.origin_GRID_global.z = 0.f;
198 buffer_.origin_metric.x = 0.f; buffer_.origin_metric.y = 0.f; buffer_.origin_metric.z = 0.f;
199 initBuffer (tsdf_volume);
200 }
201
202 /** \brief Return a pointer to the world model
203 */
206 {
207 return (&world_model_);
208 }
209
210
211 private:
212
213 /** \brief buffer used to extract XYZ values from GPU */
214 DeviceArray<PointXYZ> cloud_buffer_device_xyz_;
215
216 /** \brief buffer used to extract Intensity values from GPU */
217 DeviceArray<float> cloud_buffer_device_intensities_;
218
219 /** \brief distance threshold (cube's center to target point) to trigger shift */
220 double distance_threshold_;
221
222 /** \brief world model object that maintains the known world */
224
225 /** \brief structure that contains all TSDF buffer's addresses */
226 tsdf_buffer buffer_;
227
228 /** \brief updates cyclical buffer origins given offsets on X, Y and Z
229 * \param[in] tsdf_volume pointer to the TSDF volume managed by this cyclical buffer
230 * \param[in] offset_x offset in indices on axis X
231 * \param[in] offset_y offset in indices on axis Y
232 * \param[in] offset_z offset in indices on axis Z
233 */
234 void shiftOrigin (TsdfVolume::Ptr tsdf_volume, const int offset_x, const int offset_y, const int offset_z)
235 {
236 // shift rolling origin (making sure they keep in [0 - NbVoxels[ )
237 buffer_.origin_GRID.x += offset_x;
238 if(buffer_.origin_GRID.x >= buffer_.voxels_size.x)
239 buffer_.origin_GRID.x -= buffer_.voxels_size.x;
240 else if(buffer_.origin_GRID.x < 0)
241 buffer_.origin_GRID.x += buffer_.voxels_size.x;
242
243 buffer_.origin_GRID.y += offset_y;
244 if(buffer_.origin_GRID.y >= buffer_.voxels_size.y)
245 buffer_.origin_GRID.y -= buffer_.voxels_size.y;
246 else if(buffer_.origin_GRID.y < 0)
247 buffer_.origin_GRID.y += buffer_.voxels_size.y;
248
249 buffer_.origin_GRID.z += offset_z;
250 if(buffer_.origin_GRID.z >= buffer_.voxels_size.z)
251 buffer_.origin_GRID.z -= buffer_.voxels_size.z;
252 else if(buffer_.origin_GRID.z < 0)
253 buffer_.origin_GRID.z += buffer_.voxels_size.z;
254
255 // update memory pointers
256 PtrStep<short2> localVolume = tsdf_volume->data();
257 buffer_.tsdf_memory_start = &(localVolume.ptr (0)[0]);
258 buffer_.tsdf_memory_end = &(localVolume.ptr (buffer_.voxels_size.y * (buffer_.voxels_size.z - 1) + (buffer_.voxels_size.y - 1) )[buffer_.voxels_size.x - 1]);
259 buffer_.tsdf_rolling_buff_origin = &(localVolume.ptr (buffer_.voxels_size.y * (buffer_.origin_GRID.z) + (buffer_.origin_GRID.y) )[buffer_.origin_GRID.x]);
260
261 // update global origin
262 buffer_.origin_GRID_global.x += offset_x;
263 buffer_.origin_GRID_global.y += offset_y;
264 buffer_.origin_GRID_global.z += offset_z;
265 }
266
267 };
268 }
269 }
270}
DeviceArray class
Definition: device_array.h:54
CyclicalBuffer implements a cyclical TSDF buffer.
void computeAndSetNewCubeMetricOrigin(const pcl::PointXYZ &target_point, int &shiftX, int &shiftY, int &shiftZ)
Computes and set the origin of the new cube (relative to the world), centered around a the target poi...
void setVolumeSize(const double size)
Set the physical size represented by the default TSDF volume.
pcl::kinfuLS::WorldModel< pcl::PointXYZI > * getWorldModel()
Return a pointer to the world model.
void resetBuffer(TsdfVolume::Ptr tsdf_volume)
Reset buffer structure.
void setDistanceThreshold(const double threshold)
Sets the distance threshold between cube's center and target point that triggers a shift.
bool checkForShift(const TsdfVolume::Ptr volume, const Eigen::Affine3f &cam_pose, const double distance_camera_target, const bool perform_shift=true, const bool last_shift=false, const bool force_shift=false)
Check if shifting needs to be performed, returns true if so.
CyclicalBuffer(const double distance_threshold, const double volume_size_x, const double volume_size_y, const double volume_size_z, const int nb_voxels_x, const int nb_voxels_y, const int nb_voxels_z)
Constructor for a non-cubic CyclicalBuffer.
float getDistanceThreshold()
Returns the distance threshold between cube's center and target point that triggers a shift.
tsdf_buffer * getBuffer()
get a pointer to the tsdf_buffer structure.
void initBuffer(TsdfVolume::Ptr tsdf_volume)
Initializes memory pointers of the cyclical buffer (start, end, current origin)
void performShift(const TsdfVolume::Ptr volume, const pcl::PointXYZ &target_point, const bool last_shift=false)
Perform shifting operations: Compute offsets.
void setVolumeSize(const double size_x, const double size_y, const double size_z)
Set the physical size represented by the default TSDF volume.
CyclicalBuffer(const double distance_threshold, const double cube_size=3.f, const int nb_voxels_per_axis=512)
Constructor for a cubic CyclicalBuffer.
shared_ptr< TsdfVolume > Ptr
Definition: tsdf_volume.h:65
Defines all the PCL implemented PointT point type structures.
#define PCL_EXPORTS
Definition: pcl_macros.h:323
A point structure representing Euclidean xyz coordinates.
__PCL_GPU_HOST_DEVICE__ T * ptr(int y=0)
Structure to handle buffer addresses.
Definition: tsdf_buffer.h:51
short2 * tsdf_memory_start
Address of the first element of the TSDF volume in memory.
Definition: tsdf_buffer.h:54
short2 * tsdf_memory_end
Address of the last element of the TSDF volume in memory.
Definition: tsdf_buffer.h:56
short2 * tsdf_rolling_buff_origin
Memory address of the origin of the rolling buffer.
Definition: tsdf_buffer.h:58
int3 voxels_size
Number of voxels in the volume, per axis.
Definition: tsdf_buffer.h:68
int3 origin_GRID
Internal cube origin for rollign buffer.
Definition: tsdf_buffer.h:60
float3 origin_GRID_global
Cube origin in world coordinates.
Definition: tsdf_buffer.h:62