Dense Tracking and Mapping for Autonomous Quadrocopters. Jürgen Sturm

Transcription

1 Computer Vision Group Prof. Daniel Cremers Dense Tracking and Mapping for Autonomous Quadrocopters Jürgen Sturm Joint work with Frank Steinbrücker, Jakob Engel, Christian Kerl, Erik Bylow, and Daniel Cremers

2 Motivation Imagine you have a flying camera What would you use it for? Dense Tracking and Mapping for Autonomous Quadrocopters 2 Dr.

3 Motivation Imagine you have a flying camera What would you use it for? Dense Tracking and Mapping for Autonomous Quadrocopters 3 Dr.

4 Motivation Imagine you have a flying camera What would you use it for? Dense Tracking and Mapping for Autonomous Quadrocopters 4 Dr.

5 Motivation Imagine you have a flying camera What would you use it for? Dense Tracking and Mapping for Autonomous Quadrocopters 5 Dr.

6 Motivation Our research goal: Enable flying robots to operate autonomously in 3D environments using onboard cameras Use cameras because light weight and rich data Navigation, localization, mapping, exploration, people following,

7 Feature-based Visual Navigation [Engel, Sturm, Cremers, IROS 12]

8 Feature-based Visual Navigation [Engel, Sturm, Cremers, IROS 12] Architecture Quadrocopter Monocular SLAM Extended Kalman Filter PID Control Based on PTAM [Klein and Murray, ISMAR 07] Dense Tracking and Mapping for Autonomous Quadrocopters 8

9 Motivation Video feed from quadrocopter Dense Tracking and Mapping for Autonomous Quadrocopters 9

10 Motivation What PTAM actually sees Dense Tracking and Mapping for Autonomous Quadrocopters 10

11 Motivation Problem: Most approaches only use a small fraction of the available data Keypoint detection Visual features Question: How can we use most/all information to maximize the performance? Dense Tracking and Mapping for Autonomous Quadrocopters 11

12 Outline of the Talk Part 1: Dense tracking Part 2: Dense reconstruction Part 3: Evaluation and benchmarking Dense Tracking and Mapping for Autonomous Quadrocopters 12

13 Related Work on Dense Tracking Lucas and Kanade (IJCAI 81) Lovegrove et al. (IV 11) Newcombe et al. (ICCV 11) Comport/Tykkälä et al. (ICCV 11) Dense Tracking and Mapping for Autonomous Quadrocopters 13

14 Dense Tracking How can we exploit all data of an RGB-D image? Idea Photo-consistency constraint for all pixels Dense Tracking and Mapping for Autonomous Quadrocopters 14

15 How to deal with noise? Photo-consistency constraint will not perfectly hold Sensor noise Pose error Reflections, specular surfaces Dynamic objects (e.g., walking people) Residuals will be non-zero Residual distribution Dense Tracking and Mapping for Autonomous Quadrocopters 15

16 Probability p(r) Residual Distribution Zero-mean, peaked distribution Example: Correct camera pose 0,3 0,25 0,2 0,15 0,1 Correct camera pose 0, Residuals r Dense Tracking and Mapping for Autonomous Quadrocopters 16

17 Probability p(r) Residual Distribution Zero-mean, peaked distribution Example: Wrong camera pose 0,3 0,25 0,2 0,15 0,1 Wrong camera pose 0, Residuals r Dense Tracking and Mapping for Autonomous Quadrocopters 17

18 Probability p(r) Residual Distribution Goal: Find the camera pose that maximizes the observation likelihood 0,3 0,25 0,2 0,15 0,1 0,05 0 Wrong camera pose Correct camera pose Residuals r Dense Tracking and Mapping for Autonomous Quadrocopters 18

19 Motion Estimation Goal: Find the camera pose that maximizes the observation likelihood compute over all pixels Assume pixel-wise residuals are conditionally independent How can we solve this optimization problem? Dense Tracking and Mapping for Autonomous Quadrocopters 19

20 Approach Take negative logarithm Set derivative to zero Dense Tracking and Mapping for Autonomous Quadrocopters 20

21 Approach (cont.d) This can be rewritten as a weighted least squares problem with weights is non-linear in Need to linearize, solve, and iterate Dense Tracking and Mapping for Autonomous Quadrocopters 21

22 Iteratively Reweighted Least Squares Problem: Algorithm: 1. Compute weights 2. Linearize in the camera motion 3. Build and solve normal equations 4. Repeat until convergence Dense Tracking and Mapping for Autonomous Quadrocopters 22

23 Example First input image Second input image Residuals Dense Tracking and Mapping for Autonomous Quadrocopters 23 Image Jacobian for camera motion along x axis

24 What is a Good Model for the Residual Distribution? Dense Tracking and Mapping for Autonomous Quadrocopters 24

25 Weighted Error Dense Tracking and Mapping for Autonomous Quadrocopters 25

26 Example Weights Robust sensor model allows to down-weight outliers (dynamic objects, motion blur, reflections, ) Scene Residuals Weights Dense Tracking and Mapping for Autonomous Quadrocopters 26

27 Coarse-to-Fine Linearization only holds for small motions Coarse-to-fine scheme Image pyramids Dense Tracking and Mapping for Autonomous Quadrocopters 27

28 Dense Tracking: Results Dense Tracking and Mapping for Autonomous Quadrocopters 28

29 Summary: Dense tracking Direct matching of consecutive RGB-D images Pro Super fast, highly accurate (30 Hz on CPU) Low, constant memory consumption Con Accumulates drift over time, sometimes diverges How can we reduce/eliminate drift? Dense Tracking and Mapping for Autonomous Quadrocopters 29

30 Dense Tracking and Mapping Idea: Instead of tracking from frame-to-frame, track frame-to-model to eliminate drift t+1 t vs. Question: Where do we get the model from? Dense Tracking and Mapping for Autonomous Quadrocopters 30

31 Dense Tracking and Mapping Idea: Compute an iterative solution 1. Reconstruct model with known poses 2. Track camera with respect to known model Next question: How to represent the model? Dense Tracking and Mapping for Autonomous Quadrocopters 31

32 Representation of the 3D Model Idea: Instead of representing the cell occupancy, represent the distance of each cell to the surface Occupancy grid maps zero = free space Signed distance function (SDF) negative = outside obj. Dense Tracking and Mapping for Autonomous Quadrocopters 32 z = 1.8 x z = 1.8 x one = occupied x positive = inside obj. x

33 Dense Mapping: 2D Example Camera with known pose Dense Tracking and Mapping for Autonomous Quadrocopters 33

34 Dense Mapping: 2D Example Camera with known pose Grid with signed distance function Dense Tracking and Mapping for Autonomous Quadrocopters 34

35 Dense Mapping: 2D Example For each grid cell, compute its projective distance to the surface projective distance Dense Tracking and Mapping for Autonomous Quadrocopters 35

36 Dense Mapping: 3D Example Generalizes directly to 3D But: memory usage is cubic in side length Dense Tracking and Mapping for Autonomous Quadrocopters 36

37 Dense Tracking Given: SDF with projective distance to surface New depth image with unknown camera pose Wanted: Camera pose Our approach: Optimize camera pose so that the observed depth image minimizes the distance in the SDF Dense Tracking and Mapping for Autonomous Quadrocopters 37

38 Dense Tracking: 2D Example 3D model built from the first k frames Dense Tracking and Mapping for Autonomous Quadrocopters 38

39 Dense Tracking: 2D Example Minimize distance between depth image and SDF Dense Tracking and Mapping for Autonomous Quadrocopters 39

40 Dense Tracking: 2D Example Minimize distance between depth image and SDF Dense Tracking and Mapping for Autonomous Quadrocopters 40

41 Dense Tracking: 2D Example Minimize distance between depth image and SDF Dense Tracking and Mapping for Autonomous Quadrocopters 41

42 3D Example Given: A sequence of depth images Wanted: Accurate and dense 3D model Dense Tracking and Mapping for Autonomous Quadrocopters 42

43 Resulting 3D Model Dense Tracking and Mapping for Autonomous Quadrocopters 43

44 Can We Print These Models in 3D? Dense Tracking and Mapping for Autonomous Quadrocopters 44

45 Summary: Dense 3D Reconstruction Frame-to-model tracking Pro: Reduces drift significantly Outputs nice 3D models Con: Memory intensive (~2 GB for ) Computationally more expensive (~60 Hz on GPU) Dense Tracking and Mapping for Autonomous Quadrocopters 45

46 Evaluation and Benchmarking How can we evaluate such methods? What are good evaluation criteria? Dense Tracking and Mapping for Autonomous Quadrocopters 46

47 Existing Benchmarks Intel dataset: laser + odometry [Haehnel et al., 2004] New College dataset: stereo + omni-directional vision + laser + IMU [Smith et al., IJRR 2009] KITTI Vision benchmark: stereo [Geiger et al., CVPR 12] Our contribution: Dataset for RGB-D evaluation Intel New College KITTI RGB-D Dense Tracking and Mapping for Autonomous Quadrocopters 47

48 Recorded Scenes Different environments (office, industrial hall, ) Large variations in camera speed, camera motion, illumination, number of features, dynamic objects, Handheld and robot-mounted sensor Dense Tracking and Mapping for Autonomous Quadrocopters 48

49 Dataset Acquisition Motion capture system Camera pose (100 Hz) Microsoft Kinect (later: Asus Xtion Pro Live) Color images (30 Hz) Depth images (30 Hz) External video camera (for documentation) Dense Tracking and Mapping for Autonomous Quadrocopters 49

50 Motion Capture System 9 high-speed cameras mounted in room Cameras have active illumination and pre-process image (thresholding) Cameras track positions of retro-reflective markers Dense Tracking and Mapping for Autonomous Quadrocopters 50

51 Calibration Calibration of the overall system is not trivial: 1. Intrinsic calibration (Mocap + Kinect) 2. Extrinsic calibration (Kinect vs. Mocap) 3. Time synchronization (Kinect vs. Mocap) Dense Tracking and Mapping for Autonomous Quadrocopters 51

52 Dense Tracking and Mapping for Autonomous Quadrocopters 52

53 Dense Tracking and Mapping for Autonomous Quadrocopters 53

54 Dense Tracking and Mapping for Autonomous Quadrocopters 54

55 File Formats In total: 69 sequences (33 training, 36 testing) One TGZ archive per sequence, containing Color and depth images (PNG) List of color images (timestamp filename) List of depth images (timestamp filename) List of camera poses (timestamp tx ty tz qx qy qz qw) Dense Tracking and Mapping for Autonomous Quadrocopters 55

56 What Is a Good Evaluation Metric? Visual odometry system outputs Camera trajectory (accumulated) Visual SLAM system outputs Camera trajectory 3D map Ground truth Camera trajectory Dense Tracking and Mapping for Autonomous Quadrocopters 56

57 What Is a Good Evaluation Metric? Trajectory comparison Ground truth trajectory Estimate camera trajectory Two evaluation metrics Drift per second Global consistency Ground truth trajectory Q 1:n Estimated camera trajectory P 1:n Evaluation function Scalar performance index Dense Tracking and Mapping for Autonomous Quadrocopters 57

58 Relative Pose Error (RPE) Measures the (relative) drift between the i-th frame and the (i+δ)-th frame Relative error True motion Estimated motion Ground truth Estimated traj. Relative error Dense Tracking and Mapping for Autonomous Quadrocopters 58

59 Relative Pose Error (RPE) How to choose the time delta Δ? For odometry methods: Δ=1: Drift per frame Δ=30: Drift per second For SLAM methods: Average over all possible deltas Measures the global consistency Dense Tracking and Mapping for Autonomous Quadrocopters 59

60 Absolute Trajectory Error (ATE) Alternative method to evaluate SLAM systems Requires pre-aligned trajectories Absolute error Groundtruth Alignment Estimated Absolute error Ground truth Pre-aligned estimated traj. Dense Tracking and Mapping for Autonomous Quadrocopters 60

61 Evaluation Tools Average over all time steps Evaluation scripts for both evaluation metrics available (Python) Output: RMSE, median, mean Plot to png/pdf (optional) Dense Tracking and Mapping for Autonomous Quadrocopters 61

62 Comparison of RPE and ATE RPE and ATE are strongly related RPE considers additionally rotational errors RPE ATE Dense Tracking and Mapping for Autonomous Quadrocopters 62

63 Example: Online Evaluation Dense Tracking and Mapping for Autonomous Quadrocopters 63

64 Dense Tracking and Mapping for Autonomous Quadrocopters 64

65 Summary TUM RGB-D Benchmark Dataset for the evaluation of RGB-D SLAM systems Ground-truth camera poses Evaluation metrics + tools available Since August 2011: > visitors >4.500 online trajectory evaluations >15 published research papers using the dataset Next steps: Possibility to upload own trajectories/publications Results page and automated ranking Dense Tracking and Mapping for Autonomous Quadrocopters 65

66 Conclusion Dense methods bear a large potential Dense camera tracking Dense 3D reconstruction Use benchmark for the comparison of alternative approaches Release as open-source in preparation Please contact us if you are interested in collaboration! Dense Tracking and Mapping for Autonomous Quadrocopters 66