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