Least Squares

Bundle Adjustment (BA)

Bundle adjustment in SLAM is an optimization technique. It refines estimates of camera poses and 3D points in the map to minimize the re-projection error.

Bundle adjustment is a least squares solution for orienting images and estimating 3D point locations.

• SLAM: Creates a map of an unknown environment while also tracking the agent’s position within it.
• Bundle Adjustment: Optimizes the estimated camera poses and 3D points simultaneously.

Resources

• Bundle Adjustment - 5 Minutes with Cyrill
• The Basics about Bundle Adjustment (Cyrill Stachniss)
  • slides here
• The Numerics of Bundle Adjustment (Cyrill Stachniss)
  • slides here

Implementations

• https://github.com/fixstars/cuda-bundle-adjustment
• https://github.com/MegviiRobot/MegBA

Bundle Adjustment Statistically optimal solution.

Is BA the bottleneck of the SLAM pipeline?

Bottleneck is still the data association according to Cyrill.

Bundle “block” adjustment: multiple images are corrected “en bloc”

Key idea of BA

1. Start with an initial guess
2. Project the estimated 3D points into the estimated camera images
3. Compare locations of the projected 3D points with measured (2D) ones
4. Adjust to minimize error in the images

Notes from SLAM Textbook

Get a refresher from the SLAM Formalization.

We have the motion and observation equations $\{\begin{array}{l}{x}_k=f(x_{k-1},u_k)+w_k\\ z_{k,j}=h(y_j,x_k)+v_{k,j}\end{array}k=1\dots ,N,j=1,\dots ,M$

• motion data $u=\{u_1,\dots ,u_N\}$
• observation data $z=\{z_{1,1},\dots ,Z_{N,M}\}$
  • first subscript is for the timestamp, second subscript is for the $k$-th landmark
• state $x=\{x_1,\dots ,x_N\}$ (you don’t have this)
• landmarks $y=\{y_1,\dots ,y_M\}$ (you don’t have this)

What are the functions $f$ and $h$?

• Both are just transforms

Goal: Given $u,z$, find $x,y$ over time.

We want to figure out the ground truth poses and landmarks data. Assume that the variables follow a Normal Distribution, so our goal is to figure out the probability distribution of $x,y$ given the data: $P(x,y|z,u)$

This is a MLE problem, but I haven’t seen the multivariate version from my statistics class. We use Bayes Rule: $P(x,y|z,u)=\frac{P(z,u|x,y)P(x,y)}{P(z,u)}\propto P(z,u|x,y)P(x,y)$ The MLE problem is given by $(x,y{)}_{MLE}^{\ast }=argmaxP(z,u|x,y)$

“What state, it is most likely to produce the data currently observed?”

So how can we do this? The secrete lies in the fact that we can rewrite the prboably equationa gain:

P (z, ujx, y) = Y k P (ukjxk−1, xk) Y k,j P (zk,j jxk, yj)

Notes from Cyrill Stachniss

Why is the correction done for the 2D point, and not the 3D point?

??

Unknowns:

• 3D locations of new points
• 1D scale factor
• 6D exterior orientation
• 5D projection parameters (interior orientation)
• Non-linear distortion parameter

In the end, you don’t really need the scale factor.

Are the control points noisy or noise-free?

Initial Guess

Having a good initial guess is key. How are we supposed to obtain the initial guess?

• Compute Relative Orientation from the first image pair
• Compute orientation for subsequent images through P3P/RRS

Critical issues

• Gross error handling is essential
• Requires enough new points in each image overlap
• No singular/critical configurations

Gross Errors / Outliers

Reasons for gross errors

1. Wrong correspondences
2. Wrong point measurements

Observations per point

• At least 4 views to identify the observation with a gross error
• Observed points from 5 to 6 different views typically yield good estimates

Eliminating Gross Errors

• Decompose the problem into several small blocks of 3-6 images first
• Check for gross errors in the small blocks using statistical tests
• Only consider features that can be tracked consistently in all 3-6 images
• Relative orientation (5-Point algo.) combined with RANSAC
• Eliminate gross errors, then run full BA

Robust Kernels