Optimization in Factor Graphs for Fast and Scalable 3D Reconstruction and Mapping Frank Dellaert, Robotics & Intelligent Georgia Tech

Transcription

1 Optimization in Factor Graphs for Fast and Scalable 3D Reconstruction and Mapping Frank Dellaert, Robotics & Intelligent Georgia Tech

2 2014 2

3 Monte Carlo Localization Dellaert, Fox, Burgard & Thrun, ICRA 1999 Frank Dellaert

4 Monte Carlo Localization Dellaert, Fox, Burgard & Thrun, ICRA 1999 Frank Dellaert

5 SLAM = Simultaneous Localization and Mapping FastSLAM: Particle Filter on Trajectories Montemerlo, Thrun, Koller, & Wegbreit, AAAI 2002 Frank Dellaert 4

6 SLAM = Simultaneous Localization and Mapping FastSLAM: Particle Filter on Trajectories Montemerlo, Thrun, Koller, & Wegbreit, AAAI 2002 Frank Dellaert 4

7 2013: Full 3D LIDAR Mapping The Perceptual Robotics Laboratory at the University of Michigan Data/Movie by Nick Carlevaris-Bianco and Ryan Eustice (U. Michigan) Frank Dellaert 5

8 2013: Full 3D LIDAR Mapping The Perceptual Robotics Laboratory at the University of Michigan Data/Movie by Nick Carlevaris-Bianco and Ryan Eustice (U. Michigan) Frank Dellaert 5

9 Large-Scale Structure from Motion Photo Tourism Photosynth Iconic Scene Graphs [Snavely et al. SIGGRAPH 06] [Microsoft] [Li et al. ECCV 08] Build Rome in a Day Build Rome on a Cloudless Day Discrete-Continuous Optim. [Agarwal et al. ICCV 09] [Frahm et al. ECCV 10] [Crandall et al. CVPR 11] 6 2 / 40

10 3D Models from Community Databases E.g., Google image search on Dubrovnik 7

11 3D Models from Community Databases Agarwal, Snavely et al., University of Washington 5K images, 3.5M points, >10M factors 8

12 3D Models from Community Databases Agarwal, Snavely et al., University of Washington 5K images, 3.5M points, >10M factors 8

13 4D Reconstruction 4D Cities: 3D + Time 4D City Model Historical Image Collection Grant Schindler Frank Dellaert 9

14 4D Reconstruction 4D Cities: 3D + Time 4D City Model Historical Image Collection Grant Schindler Frank Dellaert 9

15 4D Reconstruction 4D Cities: 3D + Time 4D City Model Historical Image Collection Grant Schindler Frank Dellaert 9

16 3D Reconstruction Frank Dellaert Probabilistic Temporal Inference on Reconstructed 3D Scenes, G. Schindler and F. Dellaert, IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR),

17 3D Reconstruction Frank Dellaert Probabilistic Temporal Inference on Reconstructed 3D Scenes, G. Schindler and F. Dellaert, IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR),

18 4D Structure over Time Frank Dellaert 11

19 4D Structure over Time Frank Dellaert 11

20 Outline Intro Factor Graphs On Graphs & Matrices Graph-Inspired Methods Support Subgraphs Future Directions End 12

21 Acknowledgements Provided Materials: Michael Kaess & John Leonard (MIT), Ryan Eustice & Ed Olson (Michigan), Tim Barfoot (Toronto), Pratik Agarwal (Freiburg) Sponsors: NSF, DARPA, ONR, ARO/ARL, Microsoft Research, Samsung, Intel Noah Snavely, Sameer Agarwal, and Steve Seitz for cool data/videos 13

22 Outline Intro Factor Graphs On Graphs & Matrices Graph-Inspired Methods Support Subgraphs Future Directions End 14

23 Boolean Satisfiability 15

24 Boolean Satisfiability M I M I M I I H M A A H G A H H G 15

25 Boolean Satisfiability M I Variables M I M I I H M A A H G A H H G 15

26 Boolean Satisfiability M I Variables M M A M I I I H Factors A H G A H H G 15

27 Boolean Satisfiability M I Variables M M A M I I I H Factors A H G A H H G Factor Graph 15

28 Constraint Satisfaction Problems 16

29 Constraint Satisfaction Problems 16

30 Constraint Satisfaction Problems Ba A Z O C R Be G W T V 17

31 Graphical Models A S S T L T L E B E B X D X 18

32 Polynomial Equations Gim Hee Lee s thesis: multi-camera pose estimation 19

33 Polynomial Equations Gim Hee Lee s thesis: multi-camera pose estimation γ2 γ1 γ3 20

34 Continuous Probability Densities x 0 x 1 x 2... x M - Trajectory of Robot... - Measurements l 1 l 2... l N - Landmarks P(X,M) = k* 21

35 Simultaneous Localization and Mapping (SLAM) 22

36 Simultaneous Localization and Mapping (SLAM) 22

37 Factor Graph -> Smoothing and Mapping (SAM) 23

38 Factor Graph -> Smoothing and Mapping (SAM) Variables are multivariate Factors are non-linear 23

39 Factor Graph -> Smoothing and Mapping (SAM) Variables are multivariate Factors are non-linear Factors induce probability densities. -log P(x) = E(x) 2 23

40 Structure from Motion (Chicago, movie by Yong Dian Jian) 24

41 Structure from Motion (Chicago, movie by Yong Dian Jian) 24

42 Outline Intro Factor Graphs On Graphs & Matrices Graph-Inspired Methods Support Subgraphs Future Directions End 25

43 A Gaussian Factor Graph == mxn Matrix 26

44 Linear Least Squares A 1129x334, 5917 nnz 27

45 Linear Least Squares A A T A 334x334, 7992 nnz 1129x334, 5917 nnz 27

46 Linear Least Squares A A T A 334x334, 7992 nnz R 1129x334, 5917 nnz 334x334, 9606 nnz 27

47 Square Root Factorization A T A Cholesky R A QR R 28

48 Intel Dataset 910 poses, 4453 constraints, 45s or 49ms/step (Olson, RSS 07: iterative equation solver, same ballpark) 29

49 MIT Killian Court Dataset 1941 poses, 2190 constraints, 14.2s or 7.3ms/step 30

50 On Vesta and Ceres 31

51 On Vesta and Ceres 31

52 Variable Elimination Choose Ordering Eliminate one node at a time l 1 l 2 x 1 x 2 x 3 l1 l2 x1 x2 x3 x x x x x x x x x x x x x x Express node as function of adjacent nodes 32

53 Variable Elimination Choose Ordering Eliminate one node at a time l 1 l 2 x 1 x 2 x 3 P(l1,x1,x2) l1 l2 x1 x2 x3 x x x x x x x x x x x x x x Express node as function of adjacent nodes Basis = Chain Rule e.g. P(l1,x1,x2)=P(l1 x1,x2)p(x1,x2) 32

54 Small Example Choose Ordering Eliminate one node at a time x 1 x 2 x 3 P(x1,x2) P(l1 x1,x2) l 1 l 2 l1 l2 x1 x2 x3 x x x x x x x x x x x x x x Express node as function of adjacent nodes Basis = Chain Rule e.g. P(l1,x1,x2)=P(l1 x1,x2)p(x1,x2) 33

55 Small Example Choose Ordering Eliminate one node at a time l 1 l 2 x 1 x 2 x 3 l1 l2 x1 x2 x3 x x x x x x x x x x x x x x Express node as function of adjacent nodes 34

56 Small Example Choose Ordering Eliminate one node at a time l 1 l 2 x 1 x 2 x 3 l1 l2 x1 x2 x3 x x x x x x x x x x x Express node as function of adjacent nodes 35

57 Small Example Choose Ordering Eliminate one node at a time l 1 l 2 x 1 x 2 x 3 l1 l2 x1 x2 x3 x x x x x x x x x x x Express node as function of adjacent nodes 36

58 Small Example Choose Ordering Eliminate one node at a time l 1 l 2 x 1 x 2 x 3 l1 l2 x1 x2 x3 x x x x x x x x x x Express node as function of adjacent nodes 37

59 Small Example End-Result = Bayes Net l 1 l 2 l1 l2 x1 x2 x3 x x x x x x x x x x x 1 x 2 x 3 38

60 Polynomial Equations Gim Hee Lee s thesis: multi-camera pose estimation γ2 γ1 γ3 39

61 Polynomial Equations Three degree 2 polynomials, each in only 2 variables γ2 γ2 γ1 γ1 Degree 4 polynomial γ3 γ3 γ2 γ1 γ3 Degree 8 polynomial 40

62 Elimination at work 41

63 Elimination at work 41

64 Elimination at work 42

65 Elimination at work 42