Bi-objective Bundle Adjustment with Application to Multi-Sensor SLAM
Julien Michot Adrien Bartoli François Gaspard
Contact:
[email protected]
2
•
Context
Objective Real-time 3D localization in unknown environment (SLAM).
•
Monocular SLAM Localization & reconstruction with only one camera Localization up to a similarity
Main problems: drifts! on the trajectory (position/orientation) on the scale factor
•
•
Solutions? External data! Inertial (gyroscope, IMU,...), INS, GPS, Odometers, Laser range finder, GIS, CAD models, …
Approach
- Purple: Ground truth - Green: Monocular SLAM
Incorporate complementary data into an Incremental Structure-from-Motion SLAM ...directly in the optimization process (Bundle Adjustment) Julien Michot - 3DPVT - 05/18/2010
3
Presentation Outline
•
Background
•
Bi-objective Bundle Adjustment Weighted Bundle Adjustment Automatic Weight Selection
•
Application to Multi-sensor SLAM
•
Experimental Results on Multi-sensor SLAM Hand-held camera application Vehicle application
•
Conclusion
Julien Michot - 3DPVT - 05/18/2010
4
Background
Julien Michot - 3DPVT - 05/18/2010
5
Background •
Incremental Structure-from-Motion SLAM[Mouragnon06, Engles06, Klein07] Aim: real-time reconstruction & localization
Tracking
Features detection (salient corners) Features description (surf,...) Features matching Camera localization (linear/nonlinear) Keyframe selection
Mapping
3D reconstruction of points Bundle Adjustment
Julien Michot - 3DPVT - 05/18/2010
6
Background •
Bundle Adjustment[Triggs99] Optimize camera poses and scene reconstruction. Based on a MAP estimator + Gaussian probability functions → NLLS.
Julien Michot - 3DPVT - 05/18/2010
7
Background •
Bundle Adjustment[Triggs99] Optimize camera poses and scene reconstruction. Based on a MAP estimator + Gaussian probability functions → NLLS. Reprojection error:
with
Global Bundle Adjustment
...with Levenberg-Marquardt. Julien Michot - 3DPVT - 05/18/2010
8
Bi-objective Bundle Adjustment
Julien Michot - 3DPVT - 05/18/2010
9
Bi-objective Bundle Adjustment •
Objective Integrates external data directly in the BA.
•
How? Constraints each pose Camera position constraints
Camera orientation constraints
Trajectory scale factor constraint
...
New BA cost function:
Select a “good” weighting factor... When variances are unknown/untrusted... Julien Michot - 3DPVT - 05/18/2010
10
Bi-objective Bundle Adjustment •
Selecting an efficient weighting factor? Incremental approach: one by one. Aim: Choose a weight that minimizes both objectives → bi-objective optimization.
Julien Michot - 3DPVT - 05/18/2010
11
Bi-objective Bundle Adjustment •
Selecting an efficient weighting factor? Incremental approach: one by one. Aim: Choose a weight that minimizes both objectives → bi-objective optimization.
Julien Michot - 3DPVT - 05/18/2010
12
Bi-objective Bundle Adjustment •
Selecting an efficient weighting factor? Incremental approach: one by one. Aim: Choose a weight that minimizes both objectives → bi-objective optimization.
We propose two methods: Machine learning Criterion Julien Michot - 3DPVT - 05/18/2010
13
Methods for selecting the weighting factor
Julien Michot - 3DPVT - 05/18/2010
14
Bi-objective Bundle Adjustment •
Selecting an efficient weighting factor? Learning method: Cross Validation (leave-one-out) A good λ should predict well missing observations.
Observations set
Learning set
Test set
Find: Julien Michot - 3DPVT - 05/18/2010
15
Bi-objective Bundle Adjustment •
Selecting an efficient weighting factor? Criteria-based selection: L-Curve [Hansen01] Initially for regularization problems Principle: solve L NLLS with different value of λ l=1..L,
Log-log plot of the trade-off of each objective (Pareto frontier):
(reprojections)
(sensor constraint)
Select the weighting factor with the higher curvature: near the corner.
with ' and '' first & second derivatives wrt λ.
Julien Michot - 3DPVT - 05/18/2010
16
Application to a Real-time Multi-sensor SLAM
Julien Michot - 3DPVT - 05/18/2010
17
Application to Multi-sensor SLAM •
Overview Sensors Calibrated pinhole camera One additional sensor (ex: gyroscope)
Incremental Approach Each new keyframe is constrained with an independent weighting factor
•
Building a constraint Predict the system pose (at the time of the keyframe) Previous measurements (buffer) Motion model (asynchronous measurements)
Choose a constraint Absolute or relative On the position, orientation, ...
•
When should we compute the lambda? In the LBA? Too long (multiple LBA: >seconds)
In a simplest problem
During the pose refinement (tracking) Julien Michot - 3DPVT - 05/18/2010
18
Application to Multi-sensor SLAM •
Selecting the weighting factor Solve L NLLS problems! → need to be real-time! Estimate the weighting factor in a reduced BA: the camera refinement.
Pose refinement Optimize only 6 parameters (position&orientation of the camera)
Bi-objective pose refinement 1. Add the constraint to the cost function
(reprojections)
(unknown weight) (constraint)
2. Estimate an efficient weighting factor (Cross Validation, L-Curve, …) for this sub-problem 3. Finally, optimize the camera pose with the selected weighting factor. → More stable: the scene is fixed (less local minima)
Julien Michot - 3DPVT - 05/18/2010
19
Application to Multi-sensor SLAM •
Local Bundle Adjustment Optimize only recent camera poses and parts of the scene reconstruction.
Local Bundle Adjustment[Mouragnon06] Cost function (reprojections)
Constrained Local Bundle Adjustment Add the constraint on every optimized camera poses with the selected weighting factor.
(reprojections)
(weighting factor) (constraints)
Julien Michot - 3DPVT - 05/18/2010
20
Application to Multi-sensor SLAM •
Constrained Structure-from-Motion SLAM Tracking Features detection (salient corners) Features description (surf,...) Features matching Camera localization (linear/nonlinear) Keyframe selection Build the constraint Learn the weighting factor
Mapping
3D reconstruction of points Constrained Bundle Adjustment
Julien Michot - 3DPVT - 05/18/2010
21
Experimental Results
Julien Michot - 3DPVT - 05/18/2010
22
Experimental Results on Multi-sensor SLAM •
Hand-held camera localization Visual SLAM with an IMU
Trajectory: indoor, 8m Ground truth: 3D laser tracker system Constraint: relative rotations Mean 3D RMS with GT - Visual SLAM only: - Visual+IMU SLAM with - Visual+IMU SLAM with - Visual+IMU SLAM with - Visual+IMU SLAM with
EKF: L-Curve: Cross Validation: LTN:
3.15cm 3.17cm 2.99cm 3.11cm 3.01cm
- Purple: Ground truth - Red: Visual + IMU SLAM and L-Curve
Julien Michot - 3DPVT - 05/18/2010
23
Experimental Results on Multi-sensor SLAM •
Vehicle localization Visual SLAM with an odometer Trajectory: outdoor, 400m Ground truth: INS+GPS RTK (cm) Constraint: scale factor Mean 3D RMS with GT - Visual SLAM only: - Visual+odometer SLAM with L-Curve: 1.79m - Visual+odometer SLAM with Cross Validation: - Visual+odometer SLAM with LTN:
3.67m 2.39m 3.27m
- Purple: Ground truth - Yellow: Visual SLAM only - Red: Visual with an odometer SLAM and L-Curve
Julien Michot - 3DPVT - 05/18/2010
24
Experimental Results + Future Work •
Vehicle localization on long distances Visual SLAM with an odometer Trajectory: outdoor, ~4km Ground truth: INS+GPS RTK (cm) Constraint: scale factor - Purple: Ground truth - Green: Visual SLAM only - Red: Visual with an odometer SLAM and L-Curve
The scale factor drift is reduced but: - the orientation drift is always here We also need a constraint on the orientation.
- a small drift on the scale factor remains Absolute data are needed for long distance visual based localization.
Julien Michot - 3DPVT - 05/18/2010
25
Conclusion •
We've presented A bi-objective bundle adjustment Bi-objective: no priors on the variance of the measurements/parameters A way to add constraints in BA with automatic weighting factor selection
Two methods that estimates the weighting factor a learning method (Leave-one-out Cross Validation) a criterion (L-Curve, variants)
Application: two multi-sensor real-time SLAM Monocular + IMU SLAM Monocular + odometer SLAM
•
Future work Integrate absolute data such as GPS Extend the method to multiple constraints (multiple weights)
Julien Michot - 3DPVT - 05/18/2010
26
Thank you
Julien Michot - 3DPVT - 05/18/2010
