Bi-objective Bundle Adjustmentwith Application to Multi-Sensor SLAM
Contact: [email protected]
Julien MichotAdrien Bartoli
François Gaspard
Julien Michot - 3DPVT - 05/18/2010
2
Context
• 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,
…
• Objective Real-time 3D localization in unknown environment (SLAM).
- Purple: Ground truth- Green: Monocular SLAM
• Approach 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.
Observationsset
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
- Purple: Ground truth- Red: Visual + IMU SLAM and L-Curve
Trajectory: indoor, 8mGround truth: 3D laser tracker systemConstraint: relative rotations
Mean 3D RMS with GT- Visual SLAM only: 3.15cm- Visual+IMU SLAM with EKF: 3.17cm - Visual+IMU SLAM with L-Curve: 2.99cm- Visual+IMU SLAM with Cross Validation: 3.11cm- Visual+IMU SLAM with LTN: 3.01cm
Julien Michot - 3DPVT - 05/18/2010
23
Experimental Results on Multi-sensor SLAM
• Vehicle localization Visual SLAM with an odometer
Trajectory: outdoor, 400mGround truth: INS+GPS RTK (cm)Constraint: scale factor
Mean 3D RMS with GT- Visual SLAM only: 3.67m - Visual+odometer SLAM with L-Curve:1.79m- Visual+odometer SLAM with Cross Validation: 2.39m- Visual+odometer SLAM with LTN: 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, ~4kmGround truth: INS+GPS RTK (cm)Constraint: scale factor
The scale factor drift is reduced but:- the orientation drift is always here We also need a constraint on the orientation.
- Purple: Ground truth- Green: Visual SLAM only- Red: Visual with an odometer SLAM and L-Curve
- 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 Thank you