Post on 09-Apr-2020
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
Two Views
• Unknowns: poses and points • Measurements pij: normalized (x,y), known K!
pij
T1w T2
w
Pjw
In a Linear World…
• Linear measurement func@on:
• …and objec@ve func@on:
• Linear least-‐squares !
• Note: = 6D, = = 3D
Normal Equa@ons
• Least-‐squares criterion
• Take deriva@ve, set to zero:
• Solve using cholmod, GTSAM… • In MATLAB: x=A\b
Genera@ve Model
• Measurement Func@on, calibrated sedng!
Rigid 3D transform to camera frame
Projec@on to intrinsic image coordinates
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)
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
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:
Gauss-‐Newton • Itera@vely Linearize, solve normal equa@ons on tangent space, update Lie group elements
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
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
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
Mul@ple frames = Full BA
• Simple to extend. Typically not fully connected:
• Factor graph representa@on:
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:
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
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
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
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
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)