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

miniSAM: A Flexible Factor Graph Non-linear Least Squares Optimization Framework

Jing Dong, Zhaoyang Lv

2019.11.1

Overall• Factor graph model in robotics

• Solve factor graph by solving non-linear least squares

• miniSAM library

• Basic miniSAM usage

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."


Vehicle variable

Landmark variable

Landmark measurement factor

Odometry measurement factor

Prior factor

x1 x2 x3

l1 l2



Vehicle variable

Landmark variable



Prior factor

x1 x2 x3

l1 l2



Vehicle variable

Landmark variable



Prior factor

x1 x2 x3

l1 l2



Vehicle variable

Landmark variable



Prior factor

x1 x2 x3

l1 l2


• 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

• 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

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.

Solving Non-linear Least Squares• Get the solution by an iterative method (e.g. Gauss-Newton)




Linearization• Each iteration solves a linear least square by Taylor expansion

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

A SLAM Example for Sparsity

x1 x2 x3 l1 l2x1 x2 x3

l1 l2

Vehicle variable

Landmark variable



Prior factor

x1 x2 x3 l1 l2

x1

x2

x3

l1

l2

x1 x2 x3 l1 l2x1 x2 x3 l1 l2

x1

x2

x3

l1

l2

x1 x2 x3

l1 l2

Vehicle variable

Landmark variable



Prior factor



x1 x2 x3 l1 l2x1 x2 x3 l1 l2

x1

x2

x3

l1

l2

x1 x2 x3

l1 l2

Vehicle variable

Landmark variable



Prior factor


x1 x2 x3 l1 l2x1 x2 x3 l1 l2

x1

x2

x3

l1

l2

x1 x2 x3

l1 l2

Vehicle variable

Landmark variable



Prior factor


x1 x2 x3 l1 l2x1 x2 x3 l1 l2

x1

x2

x3

l1

l2

x1 x2 x3

l1 l2

Vehicle variable

Landmark variable



Prior factor

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.

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

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

Comparison: Functionality

miniSAM Library: Get Started

1. Build a Factor Graph

Define the error distribution (measurements noise model)• Gaussian models given covariances• Robust models (Huber, Cauchy, etc.)



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

Insert a prior factor


Insert odometry/loop closure factor


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)

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 :

Results

Define Your New Factor

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

Comparison: Performance Benchmark

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!

https://github.com/dongjing3309/minisam

https://minisam.readthedocs.io/

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

Documents