Bundle Adjustment. Frank Dellaert CVPR 2014 Visual SLAM Tutorial

Transcription

1 Bundle Adjustment Frank Dellaert CVPR 2014 Visual SLAM Tutorial

2 VO: just two frames - > R,t using 5- pt or 3- pt Can we do bener? SFM, SLAM - > VSLAM Later: integrate IMU, other sensors

3 refine VO by non- linear T 1 w T 2 w p ij P j w

4 Two Views Unknowns: poses and points Measurements p ij : normalized (x,y), known K! T 1 w T 2 w p ij P j w

5 If we lived in a Linear World:

6 In a Linear World Linear measurement func@on: and objec@ve func@on: Linear least- squares! Note: = 6D, = = 3D

7 Rewrite as where Sparse MaNers

8 Normal Least- squares criterion Take set to zero: Solve using cholmod, GTSAM In MATLAB: x=a\b

9 Model Measurement calibrated sedng! Rigid 3D transform to camera frame to intrinsic image coordinates

10 Taylor Expansion Epic Fail Taylor expansion?

11 Taylor Expansion Epic Fail Taylor expansion? Oops:? T is a 4x4 matrix, but is over- parameterized! T in SE(3): only 6DOF (3 rota@on, 3 transla@on)

12 Tangent Spaces An incremental change on a manifold can be introduced via the no@on of an n- dimensional tangent space at a Sphere SO(2) a==0 a

13 Tangent Spaces Provides local coordinate frame for manifold a 0

14 SE(3): A Twist of Li(m)e Lie group = group + manifold SE(3) is group! SE(3) is 6DOF manifold embedded in R 4*4 For Lie groups, we have exponen@al maps: se<2> twist:

15 Map for SE(3)

16 map closed form: Generators for SE(3)

17 Generalized Taylor Expansion Define f (a) to sa@sfy: =[ ]

18 Taylor Expansion for of two variables, 2x6 2x3 )

19 Taylor Expansion for of two variables, 2x6 2x3

20 Gauss- Newton Linearize, solve normal on tangent space, update Lie group elements

21 Too much Freedom! SE(3) x SE(3) E A A will be singular! 7DOF gauge freedom Switch to 5DOF Essen@al Manifold Use photogrammetry inner constraints Add prior terms Fuse in other sensors, e.g., IMU/GPS

22 Levenberg- Marquardt Algorithm Idea: Add a damping factor What is the effect of this damping factor? Small? Large? Slide by Dr. Jürgen Sturm, Computer Vision Group, TUM

23 Levenberg- Marquardt Algorithm Idea: Add a damping factor What is the effect of this damping factor? Small à same as least squares Large à steepest descent (with small step size) Algorithm If error decreases, accept and reduce If error increases, reject and increase Slide by Dr. Jürgen Sturm, Computer Vision Group, TUM

24 Linearizing Re- Error Chain rule:

25 Linearizing Re- Error Chain rule:

26 frames = Full BA Simple to extend. Typically not fully connected: Factor graph representa@on:

27 SFM Packages SBA: pioneer Google Ceres: great at large- scale BA GTSAM (Georgia Tech Smoothing and Mapping) Has isam, isam2, ideal for sensor fusion Factor- graph based throughout:

28 Stereo Camera Model 2 3 l = 4 X Y Z 5 K = 2 4 f c x f c y Z X u L b u R v c y v Projection equations: c x u L = f X Z + c x u R = f X b Z + c x Due to rectification v L = v R = v v = f Y Z + c y

29 Primary Structure Insight: and are block- diagonal (because each constraint depends only on one camera and one point) This can be solved using the Schur Complement Slide by Dr. Jürgen Sturm, Computer Vision Group, TUM 29

30 Given: Linear system Schur Complement If D is inver@ble, then (using Gauss elimina@on) Reduced complexity, i.e., invert one matrix instead of one matrix Slide by Dr. Jürgen Sturm, Computer Vision Group, TUM and

31 Smart Factors Vision- based naviga?on: Smart factors approach: A smart factor for each 3D landmark At each itera@on of nonlinear op@miza@on Camera poses Landmark posi@on 1. SF triangulates the point, given camera poses 2. SF eliminates the point via Schur complement 3. One only needs to solve a small system including the camera poses We can easily manage degenerate instances.. e.g., if the triangula@on is degenerate the smart factor can generate a different poten@al (only on rota@ons).. and we can do outlier rejec@on inside each factor! Fast(er), Robust

32 3D for crops monitoring Smart Factors

33 NEW RELEASE: GTSAM 3.1 Georgia Tech Smoothing And Mapping Download: collab.cc.gatech.edu/borg/gtsam Includes numerous performance improvements and features: threading with TBB (funded by DARPA) Smart Factors for SfM LAGO for planar SLAM (Luca Carlone et. al) threading Smart Factors Lago Linear for Graph

34 Bibliography Hartley and Zisserman, 2004 Murray, Li & Sastry, 1994 Absil, Mahony & Sepulchre, 2007