SWBAT: Graph dilations SWBAT: Determine the scale factor of a dilation SWBAT.
miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph...
Transcript of miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph...
![Page 1: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/1.jpg)
miniSAM: A Flexible Factor Graph Non-linear Least Squares Optimization Framework
Jing Dong, Zhaoyang Lv
2019.11.1
![Page 2: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/2.jpg)
Overall• Factor graph model in robotics
• Solve factor graph by solving non-linear least squares
• miniSAM library
• Basic miniSAM usage
![Page 3: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/3.jpg)
Modeling a SLAM Problem by a Factor Graph
t = 1 t = 2 t = 3
Landmark 1 Landmark 2
Vehicle poses
Landmarks
Landmark measurements
Odometry measurements
[1] Frank Dellaert and Michael Kaess. "Factor graphs for robot perception."
![Page 4: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/4.jpg)
Modeling a SLAM Problem by a Factor Graph
Vehicle variable
Landmark variable
Landmark measurement factor
Odometry measurement factor
Prior factor
x1 x2 x3
l1 l2
[1] Frank Dellaert and Michael Kaess. "Factor graphs for robot perception."
![Page 5: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/5.jpg)
Modeling a SLAM Problem by a Factor Graph
Vehicle variable
Landmark variable
Landmark measurement factor
Odometry measurement factor
Prior factor
x1 x2 x3
l1 l2
[1] Frank Dellaert and Michael Kaess. "Factor graphs for robot perception."
![Page 6: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/6.jpg)
Modeling a SLAM Problem by a Factor Graph
Vehicle variable
Landmark variable
Landmark measurement factor
Odometry measurement factor
Prior factor
x1 x2 x3
l1 l2
[1] Frank Dellaert and Michael Kaess. "Factor graphs for robot perception."
![Page 7: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/7.jpg)
Modeling a SLAM Problem by a Factor Graph
Vehicle variable
Landmark variable
Landmark measurement factor
Odometry measurement factor
Prior factor
x1 x2 x3
l1 l2
[1] Frank Dellaert and Michael Kaess. "Factor graphs for robot perception."
![Page 8: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/8.jpg)
• A factor has an error function of variables, and a measurement
• Prior factor:
• Odometry factor:
• Landmark factor:
Defining Factors: SLAM Example • A factor has an exponential form
![Page 9: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/9.jpg)
• A factor has an error function of variables, and a measurement
• Prior factor:
• Odometry factor:
• Landmark factor:
Defining Factors: SLAM Example • A factor has an exponential form
![Page 10: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/10.jpg)
Factor Graph Inference Nonlinear Least Square
• Solve log maximum likelihood
• Convert factor graph maximization to non-linear least square
[1] Frank Dellaert and Michael Kaess. "Factor graphs for robot perception.“[2] Frank Dellaert and Michael Kaess. "Square Root SAM: Simultaneous localization and mapping via square root information smoothing." The International Journal of Robotics Research 25.12 (2006): 1181-1203.
![Page 11: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/11.jpg)
Solving Non-linear Least Squares• Get the solution by an iterative method (e.g. Gauss-Newton)
![Page 12: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/12.jpg)
Solving Non-linear Least Squares• Get the solution by an iterative method (e.g. Gauss-Newton)
![Page 13: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/13.jpg)
Solving Non-linear Least Squares• Get the solution by an iterative method (e.g. Gauss-Newton)
![Page 14: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/14.jpg)
Solving Non-linear Least Squares• Get the solution by an iterative method (e.g. Gauss-Newton)
![Page 15: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/15.jpg)
Linearization• Each iteration solves a linear least square by Taylor expansion
![Page 16: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/16.jpg)
Solve the linear least square problem by
• Original from:
• Normal equation:
Solving Linear Least Squares
miniSAM built-in 3rd party sparse linear solvers:
• QR
• Cholesky
• Cholmod
• Preconditioned conjugate gradient
• CUDA Cholesky
• Schur complement
![Page 17: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/17.jpg)
A SLAM Example for Sparsity
x1 x2 x3 l1 l2x1 x2 x3
l1 l2
Vehicle variable
Landmark variable
Landmark measurement factor
Odometry measurement factor
Prior factor
x1 x2 x3 l1 l2
x1
x2
x3
l1
l2
![Page 18: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/18.jpg)
x1 x2 x3 l1 l2x1 x2 x3 l1 l2
x1
x2
x3
l1
l2
x1 x2 x3
l1 l2
Vehicle variable
Landmark variable
Landmark measurement factor
Odometry measurement factor
Prior factor
A SLAM Example for Sparsity
![Page 19: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/19.jpg)
A SLAM Example for Sparsity
x1 x2 x3 l1 l2x1 x2 x3 l1 l2
x1
x2
x3
l1
l2
x1 x2 x3
l1 l2
Vehicle variable
Landmark variable
Landmark measurement factor
Odometry measurement factor
Prior factor
![Page 20: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/20.jpg)
A SLAM Example for Sparsity
x1 x2 x3 l1 l2x1 x2 x3 l1 l2
x1
x2
x3
l1
l2
x1 x2 x3
l1 l2
Vehicle variable
Landmark variable
Landmark measurement factor
Odometry measurement factor
Prior factor
![Page 21: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/21.jpg)
A SLAM Example for Sparsity
x1 x2 x3 l1 l2x1 x2 x3 l1 l2
x1
x2
x3
l1
l2
x1 x2 x3
l1 l2
Vehicle variable
Landmark variable
Landmark measurement factor
Odometry measurement factor
Prior factor
![Page 22: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/22.jpg)
A Real-world SLAM Example
[1] Frank Dellaert and Michael Kaess. "Square Root SAM: Simultaneous localization and mapping via square root information smoothing." The International Journal of Robotics Research 25.12 (2006): 1181-1203.
![Page 23: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/23.jpg)
Factor Graph in Robotics
Motion Planning
Dong J. et al. "Motion Planning as Probabilistic Inference using Gaussian Processes and Factor Graphs." RSS 2016
3D/4D Reconstruction
Dong J. et al. "4D crop monitoring: Spatio-temporal reconstruction for agriculture.“ ICRA 2017
Scene Flow
Lv Z. et al. "A continuous optimization approach for efficient and accurate scene flow." ECCV 2016
Visual-Inertial Odometry
Forster C. et al. "On-Manifold Preintegration for Real-Time Visual--Inertial Odometry." IEEE Transactions on Robotics 2016
![Page 24: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/24.jpg)
miniSAM Library
• C++/Python library to solve factor graph/non-linear least squares
• Heavily inspired and influenced by GTSAM, but
• Full Python/NumPy API
• A wide list of 3rd party sparse linear solvers
• Super light weight, and minimal dependencies
![Page 25: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/25.jpg)
Comparison: Functionality
![Page 26: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/26.jpg)
miniSAM Library: Get Started
![Page 27: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/27.jpg)
1. Build a Factor Graph
![Page 28: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/28.jpg)
Define the error distribution (measurements noise model)• Gaussian models given covariances• Robust models (Huber, Cauchy, etc.)
1. Build a Factor Graph
![Page 29: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/29.jpg)
1. Build a Factor Graph
Insert a prior factor
Each factor has three inputs• Variable(s) connected to the factor, each variable by a key
(char + number style)• A measurement, for 2D pose is a SE(2)• An error distribution
![Page 30: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/30.jpg)
Insert a prior factor
1. Build a Factor Graph
![Page 31: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/31.jpg)
Insert odometry/loop closure factor
1. Build a Factor Graph
![Page 32: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/32.jpg)
2. Add Initial Values of Variables
Add each variable by• A name key (char + number style)• An initial value, for 2D pose is a SE(2)
![Page 33: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/33.jpg)
3. Optimize the Factor Graph
Choose a non-linear optimizer :
• QR
• Cholesky
• Cholmod
• PCG
• CUDA Cholesky
• Schur complement
• Gauss-Newton
• Levenberg-Marquardt
• Dogleg
Linear solvers :
![Page 34: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/34.jpg)
Results
![Page 35: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/35.jpg)
Define Your New Factor
![Page 36: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/36.jpg)
Define Your New Factor
To define a factor• Define error function in numpy• Give analytic Jacobian in numpy
• Analytic Jacobian is optional if use numerical Jacobian
![Page 37: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/37.jpg)
Comparison: Performance Benchmark
![Page 38: miniSAM: A Flexible Factor Graph Non-linear Least Squares … · 2019-11-04 · Factor Graph Inference Nonlinear Least Square • Solve log maximum likelihood • Convert factor graph](https://reader030.fdocuments.us/reader030/viewer/2022040223/5e558a3d4aa7641b870e142d/html5/thumbnails/38.jpg)
Summary• Factor graph model in robotics
• Solve factor graph by solving non-linear least squares
• miniSAM library introduction
Repo: github.com/dongjing3309/minisam
Document: minisam.readthedocs.io
Thank you!