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