Bundle  Adjustment  

Frank  Dellaert  CVPR  2014  Visual  SLAM  Tutorial  

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  

Objec@ve  Func@on  

•  refine  VO  by  non-­‐linear  op@miza@on  

pij

T1w T2

w

Pjw

Two  Views  

•  Unknowns:  poses  and  points  •  Measurements  pij:  normalized  (x,y),  known  K!  

pij

T1w T2

w

Pjw

If  we  lived  in  a  Linear  World:  

In  a  Linear  World…  

•  Linear  measurement    func@on:  

•  …and  objec@ve  func@on:  

•  Linear  least-­‐squares  !  

•  Note:            =  6D,  =    =  3D  

Sparse  MaNers  

•  Rewrite  as  where  

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?  

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  

Tangent  Spaces  

•  Provides  local  coordinate  frame  for  manifold  

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:  

Exponen@al  Map  for  SE(3)  

Generators  for  SE(3)  

Exponen@al  map  closed  form:  

Generalized  Taylor  Expansion  

•  Define  f’(a)  to  sa@sfy:  

=[0.2  0.3]  

Taylor  Expansion  for  Projec@on  

•  Projec@on:  func@on  of  two  variables,  

2x6   2x3  

)  

Taylor  Expansion  for  Projec@on  

•  Projec@on:  func@on  of  two  variables,  

2x6   2x3  

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  

Linearizing  Re-­‐projec@on  Error  

•  Chain  rule:  

Linearizing  Re-­‐projec@on  Error  

•  Chain  rule:  

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  

3D  reconstruc@on  for    crops  monitoring  

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)    

Bibliography  

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