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