Point Cloud Library (PCL)  1.14.1-dev
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>
52 namespace 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.
void resetBuffer(TsdfVolume::Ptr tsdf_volume)
Reset buffer structure.
tsdf_buffer * getBuffer()
get a pointer to the tsdf_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.
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.
pcl::kinfuLS::WorldModel< pcl::PointXYZI > * getWorldModel()
Return a pointer to the world model.
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