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/gpu/containers/device_array.h>
42 #include <pcl/gpu/kinfu/pixel_rgb.h>
43 #include <pcl/gpu/kinfu/tsdf_volume.h>
44 #include <pcl/gpu/kinfu/color_volume.h>
45 #include <pcl/gpu/kinfu/raycaster.h>
46 #include <pcl/point_types.h>
47 #include <pcl/point_cloud.h>
48 #include <Eigen/Core>
49 #include <vector>
50 
51 // Focal lengths of RGB camera
52 #define KINFU_DEFAULT_RGB_FOCAL_X 525.f
53 #define KINFU_DEFAULT_RGB_FOCAL_Y 525.f
54 
55 // Focal lengths of depth (i.e. NIR) camera
56 #define KINFU_DEFAULT_DEPTH_FOCAL_X 585.f
57 #define KINFU_DEFAULT_DEPTH_FOCAL_Y 585.f
58 
59 namespace pcl
60 {
61  namespace gpu
62  {
63  /** \brief KinfuTracker class encapsulates implementation of Microsoft Kinect Fusion algorithm
64  * \author Anatoly Baskeheev, Itseez Ltd, (myname.mysurname@mycompany.com)
65  */
67  {
68  public:
69  /** \brief Pixel type for rendered image. */
71 
74 
77 
78  /** \brief Constructor
79  * \param[in] rows height of depth image
80  * \param[in] cols width of depth image
81  */
82  KinfuTracker (int rows = 480, int cols = 640);
83 
84  /** \brief Sets Depth camera intrinsics
85  * \param[in] fx focal length x
86  * \param[in] fy focal length y
87  * \param[in] cx principal point x
88  * \param[in] cy principal point y
89  */
90  void
91  setDepthIntrinsics (float fx, float fy, float cx = -1, float cy = -1);
92 
93  /** \brief Get Depth camera intrinsics
94  * \param[out] fx focal length x
95  * \param[out] fy focal length y
96  * \param[out] cx principal point x
97  * \param[out] cy principal point y
98  */
99  void
100  getDepthIntrinsics (float& fx, float& fy, float& cx, float& cy);
101 
102 
103  /** \brief Sets initial camera pose relative to volume coordinate space
104  * \param[in] pose Initial camera pose
105  */
106  void
107  setInitalCameraPose (const Eigen::Affine3f& pose);
108 
109  /** \brief Sets truncation threshold for depth image for ICP step only! This helps
110  * to filter measurements that are outside tsdf volume. Pass zero to disable the truncation.
111  * \param[in] max_icp_distance Maximal distance, higher values are reset to zero (means no measurement).
112  */
113  void
114  setDepthTruncationForICP (float max_icp_distance = 0.f);
115 
116  /** \brief Sets ICP filtering parameters.
117  * \param[in] distThreshold distance.
118  * \param[in] sineOfAngle sine of angle between normals.
119  */
120  void
121  setIcpCorespFilteringParams (float distThreshold, float sineOfAngle);
122 
123  /** \brief Sets integration threshold. TSDF volume is integrated iff a camera movement metric exceedes the threshold value.
124  * The metric represents the following: M = (rodrigues(Rotation).norm() + alpha*translation.norm())/2, where alpha = 1.f (hardcoded constant)
125  * \param[in] threshold a value to compare with the metric. Suitable values are ~0.001
126  */
127  void
128  setCameraMovementThreshold(float threshold = 0.001f);
129 
130  /** \brief Performs initialization for color integration. Must be called before calling color integration.
131  * \param[in] max_weight max weighe for color integration. -1 means default weight.
132  */
133  void
134  initColorIntegration(int max_weight = -1);
135 
136  /** \brief Returns cols passed to ctor */
137  int
138  cols ();
139 
140  /** \brief Returns rows passed to ctor */
141  int
142  rows ();
143 
144  /** \brief Processes next frame.
145  * \param[in] depth next frame with values in millimeters
146  * \param hint
147  * \return true if can render 3D view.
148  */
149  bool operator() (const DepthMap& depth, Eigen::Affine3f* hint=nullptr);
150 
151  /** \brief Processes next frame (both depth and color integration). Please call initColorIntegration before invpoking this.
152  * \param[in] depth next depth frame with values in millimeters
153  * \param[in] colors next RGB frame
154  * \return true if can render 3D view.
155  */
156  bool operator() (const DepthMap& depth, const View& colors);
157 
158  /** \brief Returns camera pose at given time, default the last pose
159  * \param[in] time Index of frame for which camera pose is returned.
160  * \return camera pose
161  */
162  Eigen::Affine3f
163  getCameraPose (int time = -1) const;
164 
165  /** \brief Returns number of poses including initial */
166  std::size_t
167  getNumberOfPoses () const;
168 
169  /** \brief Returns TSDF volume storage */
170  const TsdfVolume& volume() const;
171 
172  /** \brief Returns TSDF volume storage */
173  TsdfVolume& volume();
174 
175  /** \brief Returns color volume storage */
176  const ColorVolume& colorVolume() const;
177 
178  /** \brief Returns color volume storage */
179  ColorVolume& colorVolume();
180 
181  /** \brief Renders 3D scene to display to human
182  * \param[out] view output array with image
183  */
184  void
185  getImage (View& view) const;
186 
187  /** \brief Returns point cloud abserved from last camera pose
188  * \param[out] cloud output array for points
189  */
190  void
191  getLastFrameCloud (DeviceArray2D<PointType>& cloud) const;
192 
193  /** \brief Returns point cloud abserved from last camera pose
194  * \param[out] normals output array for normals
195  */
196  void
197  getLastFrameNormals (DeviceArray2D<NormalType>& normals) const;
198 
199  /** \brief Disables ICP forever */
200  void disableIcp();
201 
202  private:
203 
204  /** \brief Number of pyramid levels */
205  enum { LEVELS = 3 };
206 
207  /** \brief ICP Correspondences map type */
208  using CorespMap = DeviceArray2D<int>;
209 
210  /** \brief Vertex or Normal Map type */
211  using MapArr = DeviceArray2D<float>;
212 
213  using Matrix3frm = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>;
214  using Vector3f = Eigen::Vector3f;
215 
216  /** \brief Height of input depth image. */
217  int rows_;
218  /** \brief Width of input depth image. */
219  int cols_;
220  /** \brief Frame counter */
221  int global_time_;
222 
223  /** \brief Truncation threshold for depth image for ICP step */
224  float max_icp_distance_;
225 
226  /** \brief Intrinsic parameters of depth camera. */
227  float fx_, fy_, cx_, cy_;
228 
229  /** \brief Tsdf volume container. */
230  TsdfVolume::Ptr tsdf_volume_;
231  ColorVolume::Ptr color_volume_;
232 
233  /** \brief Initial camera rotation in volume coo space. */
234  Matrix3frm init_Rcam_;
235 
236  /** \brief Initial camera position in volume coo space. */
237  Vector3f init_tcam_;
238 
239  /** \brief array with IPC iteration numbers for each pyramid level */
240  int icp_iterations_[LEVELS];
241  /** \brief distance threshold in correspondences filtering */
242  float distThres_;
243  /** \brief angle threshold in correspondences filtering. Represents max sine of angle between normals. */
244  float angleThres_;
245 
246  /** \brief Depth pyramid. */
247  std::vector<DepthMap> depths_curr_;
248  /** \brief Vertex maps pyramid for current frame in global coordinate space. */
249  std::vector<MapArr> vmaps_g_curr_;
250  /** \brief Normal maps pyramid for current frame in global coordinate space. */
251  std::vector<MapArr> nmaps_g_curr_;
252 
253  /** \brief Vertex maps pyramid for previous frame in global coordinate space. */
254  std::vector<MapArr> vmaps_g_prev_;
255  /** \brief Normal maps pyramid for previous frame in global coordinate space. */
256  std::vector<MapArr> nmaps_g_prev_;
257 
258  /** \brief Vertex maps pyramid for current frame in current coordinate space. */
259  std::vector<MapArr> vmaps_curr_;
260  /** \brief Normal maps pyramid for current frame in current coordinate space. */
261  std::vector<MapArr> nmaps_curr_;
262 
263  /** \brief Array of buffers with ICP correspondences for each pyramid level. */
264  std::vector<CorespMap> coresps_;
265 
266  /** \brief Buffer for storing scaled depth image */
267  DeviceArray2D<float> depthRawScaled_;
268 
269  /** \brief Temporary buffer for ICP */
270  DeviceArray2D<double> gbuf_;
271  /** \brief Buffer to store MLS matrix. */
272  DeviceArray<double> sumbuf_;
273 
274  /** \brief Array of camera rotation matrices for each moment of time. */
275  std::vector<Matrix3frm> rmats_;
276 
277  /** \brief Array of camera translations for each moment of time. */
278  std::vector<Vector3f> tvecs_;
279 
280  /** \brief Camera movement threshold. TSDF is integrated iff a camera movement metric exceedes some value. */
281  float integration_metric_threshold_;
282 
283  /** \brief ICP step is completely disabled. Only integration now. */
284  bool disable_icp_;
285 
286  /** \brief Allocates all GPU internal buffers.
287  * \param[in] rows_arg
288  * \param[in] cols_arg
289  */
290  void
291  allocateBufffers (int rows_arg, int cols_arg);
292 
293  /** \brief Performs the tracker reset to initial state. It's used if case of camera tracking fail.
294  */
295  void
296  reset ();
297 
298 public:
300 
301  };
302  }
303 };
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::DeviceArray2D
DeviceArray2D class
Definition: device_array.h:153
pcl::gpu::KinfuTracker
KinfuTracker class encapsulates implementation of Microsoft Kinect Fusion algorithm.
Definition: kinfu.h:66
pcl::gpu::TsdfVolume
TsdfVolume class.
Definition: tsdf_volume.h:54
pcl::gpu::PixelRGB
Input/output pixel format for KinfuTracker.
Definition: pixel_rgb.h:46
pcl::PointXYZ
A point structure representing Euclidean xyz coordinates.
Definition: point_types.hpp:292
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::ColorVolume::Ptr
shared_ptr< ColorVolume > Ptr
Definition: color_volume.h:59
pcl::gpu::ColorVolume
ColorVolume class.
Definition: color_volume.h:55
PCL_EXPORTS
#define PCL_EXPORTS
Definition: pcl_macros.h:271