Point Cloud Library (PCL) 1.12.0
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
60namespace 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 exceedes 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
140
141 /** \brief Returns rows passed to ctor */
142 int
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 */
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 */
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 exceedes 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
299public:
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.
const ColorVolume & colorVolume() const
Returns color volume storage.
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...
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: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.
Input/output pixel format for KinfuTracker.
Definition: pixel_rgb.h:47