CVPR 2014 Visual SLAM Tutorial Efficient Inference

Transcription

1 CVPR 2014 Visual SLAM Tutorial Efficient Inference The Robotics Institute Carnegie Mellon University

2 The Mapping Problem (t=0) Robot Landmark Measurement Onboard sensors: Wheel odometry Inertial measurement unit (gyro, accelerometer) Sonar Laser range finder Camera RGB-D sensors Landmark 2 CVPR14: Visual SLAM Tutorial

3 The Mapping Problem (t=1) Odometry measurement Robot Landmark measurement Landmark 1 Landmark 2 3 CVPR14: Visual SLAM Tutorial

4 The Mapping Problem (t=n-1) Odometry measurement Robot Landmark measurement Landmark 1 Landmark 2 4 CVPR14: Visual SLAM Tutorial

5 The Mapping Problem (t=n) Odometry measurement Landmark measurement Mapping problem is incremental! 5 CVPR14: Visual SLAM Tutorial

6 Factor Graph Representation Odometry measurement Robot pose Landmark measurement Landmark position 6 CVPR14: Visual SLAM Tutorial

7 Factor Graph Representation Pose Point measurement Point 7 CVPR14: Visual SLAM Tutorial

8 Nonlinear Least-Squares Gaussian noise 2 argmin x h i x Ξ Repeatedly solve linearized system (GN) argmin x Ax b 2 i poses points h i x x A h i (x ) b 8 CVPR14: Visual SLAM Tutorial

9 Solving the Linear Least-Squares System Solve: argmin x Ax b 2 A Normal equations A T AA = A T b A T A Information matrix Measurement Jacobian 9 CVPR14: Visual SLAM Tutorial

10 Solving the Linear Least-Squares System Can we simply invert A T A to solve for x? Normal equations A T AA = A T b A T A Information matrix Yes, but we shouldn t The inverse of A T A is dense -> O(n^3) Can do much better by taking advantage of sparsity! 10 CVPR14: Visual SLAM Tutorial

11 Solving the Linear Least-Squares System Solve: argmin x Ax b 2 A Normal equations A T AA = A T b Matrix factorization A T A = R T R A T A Information matrix R Measurement Jacobian Square root information matrix 11 CVPR14: Visual SLAM Tutorial

12 Matrix Square Root Factorization QR on A: Numerically More Stable A QR R Cholesky on A T A: Faster A T A Cholesky R 12 CVPR14: Visual SLAM Tutorial

13 Solving by Backsubstitution After factorization: R T R x = A T b Forward substitution R T y = A T b, solve for y R T Backsubstition R x = y, solve for x R 13 CVPR14: Visual SLAM Tutorial

14 Full Bundle Adjustment From Strasdat et al, 2011 IVC Visual SLAM: Why filter? Graph grows with time: Have to solve a sequence of increasingly larger BA problems Will become too expensive even for sparse Cholesky F. Dellaert and M. Kaess, Square Root SAM: Simultaneous localization and mapping via square root information smoothing, IJRR CVPR14: Visual SLAM Tutorial

15 Keyframe Bundle Adjustment Drop subset of poses to reduce density/complexity Only retain keyframes necessary for good map Complexity still grows with time, just slower 15 CVPR14: Visual SLAM Tutorial

16 Filter Keyframe idea not applicable: map would fall apart Instead, marginalize out previous poses Extended Kalman Filter (EKF) Problems when used for Visual SLAM: All points become fully connected -> expensive Relinearization not possible -> inconsistent 16 CVPR14: Visual SLAM Tutorial

17 Incremental Solver Back to full BA and keyframes: New information is added to the graph Older information does not change Can be exploited to obtain an efficient solution! 17 CVPR14: Visual SLAM Tutorial

18 [Kaess et al., TRO 08] Incremental Smoothing and Mapping (isam) Solving a growing system: R factor from previous step How do we add new measurements? R New measurements -> Key idea: Append to existing matrix factorization Repair using Givens rotations R 18 CVPR14: Visual SLAM Tutorial

19 [Kaess et al., TRO 08] Incremental Smoothing and Mapping (isam) Update and solution are O(1) Are we done? BA is nonlinear isam requires periodic batch factorization to relinearize Not O(1), we need isam2! 19 CVPR14: Visual SLAM Tutorial

20 Fixed-lag Smoothing Marginalize out all but last n poses and connected landmarks Relinearization possible Linear case Nonlinear: need isam2 plus some fillin Cut here to marginalize Fixed lag 20 CVPR14: Visual SLAM Tutorial

21 Matrix vs. Graph A Measurement Jacobian Factor Graph x 0 x 1 x 2... x M l 1 l l N A T A Information Matrix Markov Random Field x 0 x 1 x 2... x M l 1 l l N R Square Root Inf. Matrix? 21 CVPR14: Visual SLAM Tutorial

22 [Kaess et al., IJRR 12] isam2: Variable Elimination Small Example Choose ordering: l 1, l 2, x 1, x 2, x 3 Eliminate one node at a time p(l1,x1,x2) l1 l2 x1 x2 x3 p(l1,x1,x2) = p(l1 x1,x2) p(x1,x2) 22 CVPR14: Visual SLAM Tutorial

23 [Kaess et al., IJRR 12] isam2: Variable Elimination Small Example Choose ordering: l 1, l 2, x 1, x 2, x 3 Eliminate one node at a time p(l1 x1,x2) l1 l2 x1 p(x1,x2) x2 x3 p(l1,x1,x2) = p(l1 x1,x2) p(x1,x2) 23 CVPR14: Visual SLAM Tutorial

24 [Kaess et al., IJRR 12] isam2: Variable Elimination Small Example Choose ordering: l 1, l 2, x 1, x 2, x 3 Eliminate one node at a time l1 l2 x1 x2 x3 p(l2,x3) = p(l2 x3) p(x3) 24 CVPR14: Visual SLAM Tutorial

25 [Kaess et al., IJRR 12] isam2: Variable Elimination Small Example Choose ordering: l 1, l 2, x 1, x 2, x 3 Eliminate one node at a time l1 l2 x1 x2 x3 p(x1,x2) = p(x1 x2) p(x2) 25 CVPR14: Visual SLAM Tutorial

26 [Kaess et al., IJRR 12] isam2: Variable Elimination Small Example Choose ordering: l 1, l 2, x 1, x 2, x 3 Eliminate one node at a time l1 l2 x1 x2 x3 p(x2,x3) = p(x2 x3) p(x3) 26 CVPR14: Visual SLAM Tutorial

27 [Kaess et al., IJRR 12] isam2: Variable Elimination Small Example Choose ordering: l 1, l 2, x 1, x 2, x 3 Eliminate one node at a time l1 l2 x1 x2 x3 p(x3) 27 CVPR14: Visual SLAM Tutorial

28 isam2: Bayes Tree Data Structure [Kaess et al., IJRR 12] Step 1 Step 2: Find cliques in reverse elimination order: 28 CVPR14: Visual SLAM Tutorial

29 isam2: Bayes Tree Data Structure [Kaess et al., IJRR 12] Step 1 Step 2: Find cliques in reverse elimination order: 29 CVPR14: Visual SLAM Tutorial

30 isam2: Bayes Tree Example [Kaess et al., IJRR 12] Manhattan dataset (Olson) How to update with new measurements / add variables? 30 CVPR14: Visual SLAM Tutorial

31 isam2: Updating the Bayes Tree [Kaess et al., IJRR 12] Add new factor between x 1 and x 3 31 CVPR14: Visual SLAM Tutorial

32 isam2: Updating the Bayes Tree [Kaess et al., IJRR 12] Add new factor between x 1 and x 3 32 CVPR14: Visual SLAM Tutorial

33 [Kaess et al., IJRR 12] isam2: Bayes Tree for Manhattan Sequence 33 CVPR14: Visual SLAM Tutorial

34

35 Backup slides 35 CVPR14: Visual SLAM Tutorial

36 Relevant Publications D. Rosen, M. Kaess, and J. Leonard, RISE: An incremental trust-region method for robust online sparse least-squares estimation, IEEE Trans. on Robotics (TRO), 2014, to appear. M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Dellaert, isam2: Incremental smoothing and mapping using the Bayes tree, Intl. J. of Robotics Research (IJRR), vol. 31, no. 2, pp , Feb M. Kaess, V. Ila, R. Roberts, and F. Dellaert, The Bayes tree: An algorithmic foundation for probabilistic robot mapping, in Intl. Workshop on the Algorithmic Foundations of Robotics (WAFR), Singapore, Dec. 2010, pp M. Kaess, A. Ranganathan, and F. Dellaert, isam: Incremental smoothing and mapping, IEEE Trans. on Robotics (TRO), vol. 24, no. 6, pp , Dec F. Dellaert and M. Kaess, Square Root SAM: Simultaneous localization and mapping via square root information smoothing, Intl. J. of Robotics Research (IJRR), vol. 25, no. 12, pp , Dec CVPR14: Visual SLAM Tutorial

37 Retaining Sparsity: Variable Ordering Fill-in depends on elimination order: A T A R [Dellaert and Kaess, IJRR 06] factor Default ordering (poses, landmarks) permute factor Ordering based on COLAMD heuristic [Davis04] (best order: NP hard)

38 Variable Reordering Constrained COLAMD Greedy approach Arbitrary placement of newest variable Constrained Ordering Newest variables forced to the end Number of affected variables: low high Much cheaper! 38 CVPR14: Visual SLAM Tutorial