Point Cloud Library (PCL)  1.10.1
kinfu.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/pcl_macros.h>
41 #include <pcl/point_types.h>
42 #include <pcl/point_cloud.h>
43 #include <pcl/io/ply_io.h>
44 
45 #include <Eigen/Core>
46 #include <vector>
47 //#include <boost/graph/buffer_concepts.hpp>
48 
49 #include <pcl/gpu/kinfu_large_scale/device.h>
50 
51 #include <pcl/gpu/kinfu_large_scale/float3_operations.h>
52 #include <pcl/gpu/containers/device_array.h>
53 #include <pcl/gpu/kinfu_large_scale/pixel_rgb.h>
54 #include <pcl/gpu/kinfu_large_scale/tsdf_volume.h>
55 #include <pcl/gpu/kinfu_large_scale/color_volume.h>
56 #include <pcl/gpu/kinfu_large_scale/raycaster.h>
57 
58 #include <pcl/gpu/kinfu_large_scale/cyclical_buffer.h>
59 //#include <pcl/gpu/kinfu_large_scale/standalone_marching_cubes.h>
60 
61 namespace pcl
62 {
63  namespace gpu
64  {
65  namespace kinfuLS
66  {
67  /** \brief KinfuTracker class encapsulates implementation of Microsoft Kinect Fusion algorithm
68  * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
69  */
71  {
72  public:
73 
74  /** \brief Pixel type for rendered image. */
76 
79 
82 
83  void
84  performLastScan (){perform_last_scan_ = true; PCL_WARN ("Kinfu will exit after next shift\n");}
85 
86  bool
87  isFinished (){return (finished_);}
88 
89  /** \brief Constructor
90  * \param[in] volumeSize physical size of the volume represented by the tdsf volume. In meters.
91  * \param[in] shiftingDistance when the camera target point is farther than shiftingDistance from the center of the volume, shiting occurs. In meters.
92  * \note The target point is located at (0, 0, 0.6*volumeSize) in camera coordinates.
93  * \param[in] rows height of depth image
94  * \param[in] cols width of depth image
95  */
96  KinfuTracker (const Eigen::Vector3f &volumeSize, const float shiftingDistance, int rows = 480, int cols = 640);
97 
98  /** \brief Sets Depth camera intrinsics
99  * \param[in] fx focal length x
100  * \param[in] fy focal length y
101  * \param[in] cx principal point x
102  * \param[in] cy principal point y
103  */
104  void
105  setDepthIntrinsics (float fx, float fy, float cx = -1, float cy = -1);
106 
107  /** \brief Sets initial camera pose relative to volume coordinate space
108  * \param[in] pose Initial camera pose
109  */
110  void
111  setInitialCameraPose (const Eigen::Affine3f& pose);
112 
113  /** \brief Sets truncation threshold for depth image for ICP step only! This helps
114  * to filter measurements that are outside tsdf volume. Pass zero to disable the truncation.
115  * \param[in] max_icp_distance Maximal distance, higher values are reset to zero (means no measurement).
116  */
117  void
118  setDepthTruncationForICP (float max_icp_distance = 0.f);
119 
120  /** \brief Sets ICP filtering parameters.
121  * \param[in] distThreshold distance.
122  * \param[in] sineOfAngle sine of angle between normals.
123  */
124  void
125  setIcpCorespFilteringParams (float distThreshold, float sineOfAngle);
126 
127  /** \brief Sets integration threshold. TSDF volume is integrated iff a camera movement metric exceedes the threshold value.
128  * The metric represents the following: M = (rodrigues(Rotation).norm() + alpha*translation.norm())/2, where alpha = 1.f (hardcoded constant)
129  * \param[in] threshold a value to compare with the metric. Suitable values are ~0.001
130  */
131  void
132  setCameraMovementThreshold(float threshold = 0.001f);
133 
134  /** \brief Performs initialization for color integration. Must be called before calling color integration.
135  * \param[in] max_weight max weighe for color integration. -1 means default weight.
136  */
137  void
138  initColorIntegration(int max_weight = -1);
139 
140  /** \brief Returns cols passed to ctor */
141  int
142  cols ();
143 
144  /** \brief Returns rows passed to ctor */
145  int
146  rows ();
147 
148  /** \brief Processes next frame.
149  * \param[in] depth next frame with values in millimeters
150  * \return true if can render 3D view.
151  */
152  bool operator() (const DepthMap& depth);
153 
154  /** \brief Processes next frame (both depth and color integration). Please call initColorIntegration before invpoking this.
155  * \param[in] depth next depth frame with values in millimeters
156  * \param[in] colors next RGB frame
157  * \return true if can render 3D view.
158  */
159  bool operator() (const DepthMap& depth, const View& colors);
160 
161  /** \brief Returns camera pose at given time, default the last pose
162  * \param[in] time Index of frame for which camera pose is returned.
163  * \return camera pose
164  */
165  Eigen::Affine3f
166  getCameraPose (int time = -1) const;
167 
168  Eigen::Affine3f
169  getLastEstimatedPose () const;
170 
171  /** \brief Returns number of poses including initial */
172  std::size_t
173  getNumberOfPoses () const;
174 
175  /** \brief Returns TSDF volume storage */
176  const TsdfVolume& volume() const;
177 
178  /** \brief Returns TSDF volume storage */
179  TsdfVolume& volume();
180 
181  /** \brief Returns color volume storage */
182  const ColorVolume& colorVolume() const;
183 
184  /** \brief Returns color volume storage */
185  ColorVolume& colorVolume();
186 
187  /** \brief Renders 3D scene to display to human
188  * \param[out] view output array with image
189  */
190  void
191  getImage (View& view) const;
192 
193  /** \brief Returns point cloud abserved from last camera pose
194  * \param[out] cloud output array for points
195  */
196  void
197  getLastFrameCloud (DeviceArray2D<PointType>& cloud) const;
198 
199  /** \brief Returns point cloud abserved from last camera pose
200  * \param[out] normals output array for normals
201  */
202  void
203  getLastFrameNormals (DeviceArray2D<NormalType>& normals) const;
204 
205 
206  /** \brief Returns pointer to the cyclical buffer structure
207  */
208  tsdf_buffer*
210  {
211  return (cyclical_.getBuffer ());
212  }
213 
214  /** \brief Extract the world and save it.
215  */
216  void
217  extractAndSaveWorld ();
218 
219  /** \brief Returns true if ICP is currently lost */
220  bool
222  {
223  return (lost_);
224  }
225 
226  /** \brief Performs the tracker reset to initial state. It's used if camera tracking fails. */
227  void
228  reset ();
229 
230  void
232  {
233  disable_icp_ = !disable_icp_;
234  PCL_WARN("ICP is %s\n", !disable_icp_?"ENABLED":"DISABLED");
235  }
236 
237  /** \brief Return whether the last update resulted in a shift */
238  inline bool
239  hasShifted () const
240  {
241  return (has_shifted_);
242  }
243 
244  private:
245 
246  /** \brief Allocates all GPU internal buffers.
247  * \param[in] rows_arg
248  * \param[in] cols_arg
249  */
250  void
251  allocateBufffers (int rows_arg, int cols_arg);
252 
253  /** \brief Number of pyramid levels */
254  enum { LEVELS = 3 };
255 
256  /** \brief ICP Correspondences map type */
257  using CorespMap = DeviceArray2D<int>;
258 
259  /** \brief Vertex or Normal Map type */
260  using MapArr = DeviceArray2D<float>;
261 
262  using Matrix3frm = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>;
263  using Vector3f = Eigen::Vector3f;
264 
265  /** \brief helper function that converts transforms from host to device types
266  * \param[in] transformIn1 first transform to convert
267  * \param[in] transformIn2 second transform to convert
268  * \param[in] translationIn1 first translation to convert
269  * \param[in] translationIn2 second translation to convert
270  * \param[out] transformOut1 result of first transform conversion
271  * \param[out] transformOut2 result of second transform conversion
272  * \param[out] translationOut1 result of first translation conversion
273  * \param[out] translationOut2 result of second translation conversion
274  */
275  inline void
276  convertTransforms (Matrix3frm& transform_in_1, Matrix3frm& transform_in_2, Eigen::Vector3f& translation_in_1, Eigen::Vector3f& translation_in_2,
277  pcl::device::kinfuLS::Mat33& transform_out_1, pcl::device::kinfuLS::Mat33& transform_out_2, float3& translation_out_1, float3& translation_out_2);
278 
279  /** \brief helper function that converts transforms from host to device types
280  * \param[in] transformIn1 first transform to convert
281  * \param[in] transformIn2 second transform to convert
282  * \param[in] translationIn translation to convert
283  * \param[out] transformOut1 result of first transform conversion
284  * \param[out] transformOut2 result of second transform conversion
285  * \param[out] translationOut result of translation conversion
286  */
287  inline void
288  convertTransforms (Matrix3frm& transform_in_1, Matrix3frm& transform_in_2, Eigen::Vector3f& translation_in,
289  pcl::device::kinfuLS::Mat33& transform_out_1, pcl::device::kinfuLS::Mat33& transform_out_2, float3& translation_out);
290 
291  /** \brief helper function that converts transforms from host to device types
292  * \param[in] transformIn transform to convert
293  * \param[in] translationIn translation to convert
294  * \param[out] transformOut result of transform conversion
295  * \param[out] translationOut result of translation conversion
296  */
297  inline void
298  convertTransforms (Matrix3frm& transform_in, Eigen::Vector3f& translation_in,
299  pcl::device::kinfuLS::Mat33& transform_out, float3& translation_out);
300 
301  /** \brief helper function that pre-process a raw detph map the kinect fusion algorithm.
302  * The raw depth map is first blurred, eventually truncated, and downsampled for each pyramid level.
303  * Then, vertex and normal maps are computed for each pyramid level.
304  * \param[in] depth_raw the raw depth map to process
305  * \param[in] cam_intrinsics intrinsics of the camera used to acquire the depth map
306  */
307  inline void
308  prepareMaps (const DepthMap& depth_raw, const pcl::device::kinfuLS::Intr& cam_intrinsics);
309 
310  /** \brief helper function that performs GPU-based ICP, using vertex and normal maps stored in v/nmaps_curr_ and v/nmaps_g_prev_
311  * The function requires the previous local camera pose (translation and inverted rotation) as well as camera intrinsics.
312  * It will return the newly computed pose found as global rotation and translation.
313  * \param[in] cam_intrinsics intrinsics of the camera
314  * \param[in] previous_global_rotation previous local rotation of the camera
315  * \param[in] previous_global_translation previous local translation of the camera
316  * \param[out] current_global_rotation computed global rotation
317  * \param[out] current_global_translation computed global translation
318  * \return true if ICP has converged.
319  */
320  inline bool
321  performICP(const pcl::device::kinfuLS::Intr& cam_intrinsics, Matrix3frm& previous_global_rotation, Vector3f& previous_global_translation, Matrix3frm& current_global_rotation, Vector3f& current_global_translation);
322 
323 
324  /** \brief helper function that performs GPU-based ICP, using the current and the previous depth-maps (i.e. not using the synthetic depth map generated from the tsdf-volume)
325  * The function requires camera intrinsics.
326  * It will return the transformation between the previous and the current depth map.
327  * \param[in] cam_intrinsics intrinsics of the camera
328  * \param[out] resulting_rotation computed global rotation
329  * \param[out] resulting_translation computed global translation
330  * \return true if ICP has converged.
331  */
332  inline bool
333  performPairWiseICP(const pcl::device::kinfuLS::Intr cam_intrinsics, Matrix3frm& resulting_rotation, Vector3f& resulting_translation);
334 
335  /** \brief Helper function that copies v_maps_curr and n_maps_curr to v_maps_prev_ and n_maps_prev_ */
336  inline void
337  saveCurrentMaps();
338 
339  /** \brief Cyclical buffer object */
340  CyclicalBuffer cyclical_;
341 
342  /** \brief Height of input depth image. */
343  int rows_;
344 
345  /** \brief Width of input depth image. */
346  int cols_;
347 
348  /** \brief Frame counter */
349  int global_time_;
350 
351  /** \brief Truncation threshold for depth image for ICP step */
352  float max_icp_distance_;
353 
354  /** \brief Intrinsic parameters of depth camera. */
355  float fx_, fy_, cx_, cy_;
356 
357  /** \brief Tsdf volume container. */
358  TsdfVolume::Ptr tsdf_volume_;
359 
360  /** \brief Color volume container. */
361  ColorVolume::Ptr color_volume_;
362 
363  /** \brief Initial camera rotation in volume coo space. */
364  Matrix3frm init_Rcam_;
365 
366  /** \brief Initial camera position in volume coo space. */
367  Vector3f init_tcam_;
368 
369  /** \brief array with IPC iteration numbers for each pyramid level */
370  int icp_iterations_[LEVELS];
371 
372  /** \brief distance threshold in correspondences filtering */
373  float distThres_;
374 
375  /** \brief angle threshold in correspondences filtering. Represents max sine of angle between normals. */
376  float angleThres_;
377 
378  /** \brief Depth pyramid. */
379  std::vector<DepthMap> depths_curr_;
380 
381  /** \brief Vertex maps pyramid for current frame in global coordinate space. */
382  std::vector<MapArr> vmaps_g_curr_;
383 
384  /** \brief Normal maps pyramid for current frame in global coordinate space. */
385  std::vector<MapArr> nmaps_g_curr_;
386 
387  /** \brief Vertex maps pyramid for previous frame in global coordinate space. */
388  std::vector<MapArr> vmaps_g_prev_;
389 
390  /** \brief Normal maps pyramid for previous frame in global coordinate space. */
391  std::vector<MapArr> nmaps_g_prev_;
392 
393  /** \brief Vertex maps pyramid for current frame in current coordinate space. */
394  std::vector<MapArr> vmaps_curr_;
395 
396  /** \brief Normal maps pyramid for current frame in current coordinate space. */
397  std::vector<MapArr> nmaps_curr_;
398 
399  /** \brief Vertex maps pyramid for previous frame in current coordinate space. */
400  std::vector<MapArr> vmaps_prev_;
401 
402  /** \brief Normal maps pyramid for previous frame in current coordinate space. */
403  std::vector<MapArr> nmaps_prev_;
404 
405  /** \brief Array of buffers with ICP correspondences for each pyramid level. */
406  std::vector<CorespMap> coresps_;
407 
408  /** \brief Buffer for storing scaled depth image */
409  DeviceArray2D<float> depthRawScaled_;
410 
411  /** \brief Temporary buffer for ICP */
412  DeviceArray2D<double> gbuf_;
413 
414  /** \brief Buffer to store MLS matrix. */
415  DeviceArray<double> sumbuf_;
416 
417  /** \brief Array of camera rotation matrices for each moment of time. */
418  std::vector<Matrix3frm> rmats_;
419 
420  /** \brief Array of camera translations for each moment of time. */
421  std::vector<Vector3f> tvecs_;
422 
423  /** \brief Camera movement threshold. TSDF is integrated iff a camera movement metric exceedes some value. */
424  float integration_metric_threshold_;
425 
426  /** \brief When set to true, KinFu will extract the whole world and mesh it. */
427  bool perform_last_scan_;
428 
429  /** \brief When set to true, KinFu notifies that it is finished scanning and can be stopped. */
430  bool finished_;
431 
432  /** \brief // when the camera target point is farther than DISTANCE_THRESHOLD from the current cube's center, shifting occurs. In meters . */
433  float shifting_distance_;
434 
435  /** \brief Size of the TSDF volume in meters. */
436  float volume_size_;
437 
438  /** \brief True if ICP is lost */
439  bool lost_;
440 
441  /** \brief Last estimated rotation (estimation is done via pairwise alignment when ICP is failing) */
442  Matrix3frm last_estimated_rotation_;
443 
444  /** \brief Last estimated translation (estimation is done via pairwise alignment when ICP is failing) */
445  Vector3f last_estimated_translation_;
446 
447 
448  bool disable_icp_;
449 
450  /** \brief True or false depending on if there was a shift in the last pose update */
451  bool has_shifted_;
452 
453  public:
455 
456  };
457  }
458  }
459 };
pcl::gpu::kinfuLS::KinfuTracker::hasShifted
bool hasShifted() const
Return whether the last update resulted in a shift.
Definition: kinfu.h:239
pcl::gpu::kinfuLS::KinfuTracker::isFinished
bool isFinished()
Definition: kinfu.h:87
pcl_macros.h
Defines all the PCL and non-PCL macros used.
pcl
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
point_types.h
pcl::Normal
A point structure representing normal coordinates and the surface curvature estimate.
Definition: point_types.hpp:804
pcl::gpu::kinfuLS::KinfuTracker
KinfuTracker class encapsulates implementation of Microsoft Kinect Fusion algorithm.
Definition: kinfu.h:70
pcl::gpu::kinfuLS::KinfuTracker::icpIsLost
bool icpIsLost()
Returns true if ICP is currently lost.
Definition: kinfu.h:221
pcl::gpu::DeviceArray2D
DeviceArray2D class
Definition: device_array.h:153
pcl::gpu::kinfuLS::KinfuTracker::getCyclicalBufferStructure
tsdf_buffer * getCyclicalBufferStructure()
Returns pointer to the cyclical buffer structure.
Definition: kinfu.h:209
pcl::device::kinfuLS::Intr
Camera intrinsics structure.
Definition: device.h:83
pcl::gpu::kinfuLS::PixelRGB
Input/output pixel format for KinfuTracker.
Definition: pixel_rgb.h:50
pcl::PointXYZ
A point structure representing Euclidean xyz coordinates.
Definition: point_types.hpp:292
pcl::gpu::kinfuLS::KinfuTracker::performLastScan
void performLastScan()
Definition: kinfu.h:84
pcl::gpu::TsdfVolume::Ptr
shared_ptr< TsdfVolume > Ptr
Definition: tsdf_volume.h:57
pcl::gpu::DeviceArray< double >
PCL_MAKE_ALIGNED_OPERATOR_NEW
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:389
pcl::gpu::kinfuLS::TsdfVolume
TsdfVolume class.
Definition: tsdf_volume.h:61
pcl::gpu::kinfuLS::KinfuTracker::setDisableICP
void setDisableICP()
Definition: kinfu.h:231
pcl::gpu::kinfuLS::ColorVolume
ColorVolume class.
Definition: color_volume.h:59
pcl::gpu::ColorVolume::Ptr
shared_ptr< ColorVolume > Ptr
Definition: color_volume.h:59
pcl::device::kinfuLS::Mat33
3x3 Matrix for device code
Definition: device.h:105
pcl::gpu::kinfuLS::tsdf_buffer
Structure to handle buffer addresses.
Definition: tsdf_buffer.h:50
PCL_EXPORTS
#define PCL_EXPORTS
Definition: pcl_macros.h:271