Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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
62namespace 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 */
71 class PCL_EXPORTS KinfuTracker
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
144
145 /** \brief Returns rows passed to ctor */
146 int
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 */
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
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 */
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
DeviceArray2D class
DeviceArray class
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.
DeviceArray2D< PixelRGB > View
Definition kinfu.h:78
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
const ColorVolume & colorVolume() const
Returns color volume storage.
ColorVolume & colorVolume()
Returns color volume storage.
void setCameraMovementThreshold(float threshold=0.001f)
Sets integration threshold.
Eigen::Affine3f getLastEstimatedPose() const
tsdf_buffer * getCyclicalBufferStructure()
Returns pointer to the cyclical buffer structure.
Definition kinfu.h:210
bool icpIsLost()
Returns true if ICP is currently lost.
Definition kinfu.h:222
const TsdfVolume & volume() const
Returns TSDF volume storage.
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.
void initColorIntegration(int max_weight=-1)
Performs initialization for color integration.
int cols()
Returns cols passed to ctor.
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.
TsdfVolume & volume()
Returns TSDF volume storage.
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.
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