bundle’adjustmentkaess/vslam_cvpr14/media/vslam...toomuchfreedom!’ •...
TRANSCRIPT
![Page 1: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/1.jpg)
Bundle Adjustment
Frank Dellaert CVPR 2014 Visual SLAM Tutorial
![Page 2: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/2.jpg)
Mo@va@on
• VO: just two frames -‐> R,t using 5-‐pt or 3-‐pt
• Can we do beNer? SFM, SLAM -‐> VSLAM • Later: integrate IMU, other sensors
![Page 3: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/3.jpg)
Objec@ve Func@on
• refine VO by non-‐linear op@miza@on
pij
T1w T2
w
Pjw
![Page 4: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/4.jpg)
Two Views
• Unknowns: poses and points • Measurements pij: normalized (x,y), known K!
pij
T1w T2
w
Pjw
![Page 5: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/5.jpg)
If we lived in a Linear World:
![Page 6: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/6.jpg)
In a Linear World…
• Linear measurement func@on:
• …and objec@ve func@on:
• Linear least-‐squares !
• Note: = 6D, = = 3D
![Page 7: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/7.jpg)
Sparse MaNers
• Rewrite as where
![Page 8: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/8.jpg)
Normal Equa@ons
• Least-‐squares criterion
• Take deriva@ve, set to zero:
• Solve using cholmod, GTSAM… • In MATLAB: x=A\b
![Page 9: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/9.jpg)
Genera@ve Model
• Measurement Func@on, calibrated sedng!
Rigid 3D transform to camera frame
Projec@on to intrinsic image coordinates
![Page 10: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/10.jpg)
Taylor Expansion Epic Fail
• Taylor expansion?
![Page 11: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/11.jpg)
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)
![Page 12: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/12.jpg)
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
a==0
![Page 13: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/13.jpg)
Tangent Spaces
• Provides local coordinate frame for manifold
a
0
![Page 14: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/14.jpg)
SE(3): A Twist of Li(m)e
• Lie group = group + manifold – SE(3) is group! – SE(3) is 6DOF manifold embedded in R4*4
• For Lie groups, we have exponen@al maps:
• se<2> twist:
![Page 15: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/15.jpg)
Exponen@al Map for SE(3)
![Page 16: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/16.jpg)
Generators for SE(3)
Exponen@al map closed form:
![Page 17: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/17.jpg)
Generalized Taylor Expansion
• Define f’(a) to sa@sfy:
=[0.2 0.3]
![Page 18: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/18.jpg)
Taylor Expansion for Projec@on
• Projec@on: func@on of two variables,
2x6 2x3
)
![Page 19: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/19.jpg)
Taylor Expansion for Projec@on
• Projec@on: func@on of two variables,
2x6 2x3
![Page 20: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/20.jpg)
Gauss-‐Newton • Itera@vely Linearize, solve normal equa@ons on tangent space, update Lie group elements
![Page 21: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/21.jpg)
Too much Freedom!
• 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
SE(3) x SE(3) E
![Page 22: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/22.jpg)
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
![Page 23: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/23.jpg)
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
![Page 24: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/24.jpg)
Linearizing Re-‐projec@on Error
• Chain rule:
![Page 25: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/25.jpg)
Linearizing Re-‐projec@on Error
• Chain rule:
![Page 26: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/26.jpg)
Mul@ple frames = Full BA
• Simple to extend. Typically not fully connected:
• Factor graph representa@on:
![Page 27: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/27.jpg)
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:
![Page 28: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/28.jpg)
Stereo Camera Projec@on Model
b uL
v
XZ
l =
2
4XYZ
3
5
Projection equations: Due to rectification vL = vR = v
K =
2
4f c
x
f cy
1
3
5
v
uR
cx
cy
uL
= f X
Z
+ cx
uR
= f X�b
Z
+ cx
v = f YZ + cy
![Page 29: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/29.jpg)
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
29 Slide by Dr. Jürgen Sturm,
Computer Vision Group, TUM
![Page 30: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/30.jpg)
Schur Complement • Given: Linear system
• If D is inver@ble, then (using Gauss elimina@on)
• Reduced complexity, i.e., invert one and matrix instead of one matrix
Slide by Dr. Jürgen Sturm, Computer Vision Group, TUM
![Page 31: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/31.jpg)
Vision-‐based naviga?on:
Smart factors approach:
• A smart factor for each 3D landmark
• At each itera@on of nonlinear op@miza@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
• Fast(er), Robust
Camera poses Landmark posi@on
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!
Smart Factors
![Page 32: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/32.jpg)
3D reconstruc@on for crops monitoring
Smart Factors
![Page 33: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/33.jpg)
Lago Linear Approxima@on for Graph Op@miza@on
Smart Factors Mul@-‐threading
NEW RELEASE: GTSAM 3.1 Georgia Tech Smoothing And Mapping
Download: collab.cc.gatech.edu/borg/gtsam
Includes numerous performance improvements and features: • Mul@-‐threading with TBB (funded by DARPA) • Smart Projec@on Factors for SfM • LAGO Ini@aliza@on for planar SLAM (Luca Carlone et. al)
![Page 34: Bundle’Adjustmentkaess/vslam_cvpr14/media/VSLAM...ToomuchFreedom!’ • A’A’will’be’singular!’7DOF’gaugefreedom – Switch’to’5DOF’Essen@al’Manifold’ – Use’photogrammetry’“inner](https://reader035.vdocuments.site/reader035/viewer/2022070114/60853312e54b5a0f8b677ec5/html5/thumbnails/34.jpg)
Bibliography
• Hartley and Zisserman, 2004 • Murray, Li & Sastry, 1994 • Absil, Mahony & Sepulchre, 2007