1. Daniel Shepard and Todd Humphreys
High-Precision Globally-Referenced Position and Attitude via a Fusion of Visual SLAM, Carrier-Phase-Based GPS, and Inertial Measurements
Daniel Shepard and Todd Humphreys
2014 IEEE/ION PLANS Conference, Monterey, CA | May 8, 2014

2. Overview Globally-Referenced Visual SLAM
Motivating Application: Augmented Reality
Estimation Architecture
Bundle Adjustment (BA)
Simulation Results for BA

3. Stand-Alone Visual SLAM
Produces high-precision estimates of
Camera motion (with ambiguous scale for monocular SLAM)
A map of the environment
Limited in application due to lack of a global reference
[1] G. Klein and D. Murray, “Parallel tracking and mapping for small AR workspaces,” in 6th IEEE and ACM International Symposium on Mixed and Augmented Reality. IEEE, 2007, pp. 225–234.

4. Visual SLAM with Fiduciary Markers
Globally-referenced solution if fiduciary markers are globally-referenced
Requires substantial infrastructure and/or mapping effort
Microsoft’s augmented reality maps (TED2010[2])
[2] B. A. y Arcas, “Blaise Aguera y Arcas demos augmented-reality maps,” TED, Feb. 2010, aguera.html.

5. Can globally-referenced position and attitude (pose) be recovered from combining visual SLAM and GPS?

6. Observability of Visual SLAM + GPS
No GPS positions
Translation
Rotation
Scale 
1 GPS position
Translation
Rotation
Scale  
2 GPS positions
Translation
Rotation
Scale  ~
3 GPS positions
Translation
Rotation
Scale 

7. Combined Visual SLAM and CDGPS
CDGPS anchors visual SLAM to a global reference frame
Can add an IMU to improve dynamic performance (not required!)
Can be made inexpensive
Requires little infrastructure
Very Accurate!

8. Motivating Application: Augmented Reality
Augmenting a live view of the world with computer-generated sensory input to enhance one’s current perception of reality[3]
Current applications are limited by lack of accurate global pose
Potential uses in
Construction
Real-Estate
Gaming
Social Media
[3] Graham, M., Zook, M., and Boulton, A. "Augmented reality in urban places: contested content and the duplicity of code." Transactions of the Institute of British Geographers. .

9. Estimation Architecture Motivation
Sensors:
Camera
Two GPS antennas
(reference and mobile)
IMU
How can the information from these sensors best be combined to estimate the camera pose and a map of the environment?
Real-time operation
Computational burden vs. precision

10. Sensor Fusion Approach
Tighter coupling = higher precision, but increased computational burden
IMU
Visual SLAM
CDGPS
IMU
Visual SLAM
CDGPS
IMU
Visual SLAM
CDGPS
IMU
Visual SLAM
CDGPS

11. The Optimal Estimator

12. IMU only for Pose Propagation

13. Tightly-Coupled Architecture

14. Loosely-Coupled Architecture

15. Hybrid Batch/Sequential Estimator
Only geographically diverse frames (keyframes) in batch estimator

16. Bundle Adjustment State and Measurements
State Vector:
𝑿 𝐵𝐴 = 𝒄 𝒑 , 𝒄= … 𝒙 𝐺 𝐶 𝑖 𝑇 𝒒 𝐺 𝐶 𝑖 𝑇 … 𝑇 , 𝒑= … 𝒙 𝐺 𝑝 𝑗 𝑇 … 𝑇
Measurement Models:
CDGPS Positions:
𝒙 𝐺 𝐴 𝑖 = 𝒉 𝑥 𝒙 𝐺 𝐶 𝑖 , 𝒒 𝐺 𝐶 𝑖 + 𝒘 𝑥 𝑖 = 𝒙 𝐺 𝐶 𝑖 +𝑅 𝒒 𝐺 𝐶 𝑖 𝒙 𝐶 𝐴 + 𝒘 𝑥 𝑖
Image Feature Measurements:
𝒔 𝐼 𝑖 𝑝 𝑗 = 𝒉 𝑠 𝒙 𝐶 𝑖 𝑝 𝑗 + 𝒘 𝐼 𝑖 𝑝 𝑗 = 𝑥 𝐶 𝑖 𝑝 𝑗 𝑧 𝐶 𝑖 𝑝 𝑗 𝑦 𝐶 𝑖 𝑝 𝑗 𝑧 𝐶 𝑖 𝑝 𝑗 𝑇 + 𝒘 𝐼 𝑖 𝑝 𝑗
𝒙 𝐶 𝑖 𝑝 𝑗 = 𝑥 𝐶 𝑖 𝑝 𝑗 𝑦 𝐶 𝑖 𝑝 𝑗 𝑧 𝐶 𝑖 𝑝 𝑗 𝑇 = 𝑅 𝒒 𝐺 𝐶 𝑖 𝑇 ( 𝒙 𝐺 𝑝 𝑗 − 𝒙 𝐺 𝐶 𝑖 )

17. Bundle Adjustment Cost Minimization
Weighted least-squares cost function
Employs robust weight functions to handle outliers
argmin 𝑿 𝐵𝐴 𝑖=1 𝑁 Δ 𝒙 𝐺 𝐴 𝑖 𝑗=1 𝑀 𝑤 𝑉 Δ 𝒔 𝐼 𝑖 𝑝 𝑗 Δ 𝒔 𝐼 𝑖 𝑝 𝑗 2
Δ 𝒙 𝐺 𝐴 𝑖 = 𝑅 𝒙 𝐺 𝐴 𝑖 −1/2 𝒙 𝐺 𝐴 𝑖 − 𝒙 𝐺 𝐴 𝑖 Δ 𝒔 𝐼 𝑖 𝑝 𝑗 = 𝑅 𝒔 𝐼 𝑖 𝑝 𝑗 −1/2 𝒔 𝐼 𝑖 𝑝 𝑗 − 𝒔 𝐼 𝑖 𝑝 𝑗
Sparse Levenberg-Marquart algorithm
Computational complexity linear in number of point features, but cubic in number of keyframes

18. Bundle Adjustment Initialization
Initialize BA based on stand-alone visual SLAM solution and CDGPS positions
Determine similarity transform relating coordinate systems
argmin 𝒙 𝐺 𝑉 , 𝒒 𝐺 𝑉 , 𝑠 𝑖=1 𝑁 𝒙 𝐺 𝐴 𝑖 − 𝒙 𝐺 𝑉 −𝑅 𝒒 𝐺 𝑉 𝑠 𝒙 𝑉 𝐶 𝑖 +𝑅 𝒒 𝑉 𝐶 𝑖 𝒙 𝐶 𝐴 2
Generalized form of Horn’s transform[4]
Rotation: Rotation that best aligns deviations from mean camera position
Scale: A ratio of metrics describing spread of camera positions
Translation: Difference in mean antenna position
[4] B. K. Horn, “Closed-form solution of absolute orientation using unit quaternions,” JOSA A, vol. 4, no. 4, pp. 629–642, 1987.

19. Simulation Scenario for BA
Simulations investigating estimability included in paper
Hallway Simulation:
Measurement errors:
2 cm std for CDGPS
1 pixel std for vision
Keyframes every 0.25 m
242 keyframes
1310 point features
Three scenarios:
GPS available
GPS lost when hallway entered
GPS reacquired when hallway exited A D ←C ←B

20. Simulation Results for BA

21. Summary
Hybrid batch/sequential estimator for loosely-coupled visual SLAM and CDGPS with IMU for state propagation
Compared to optimal estimator
Outlined algorithm for BA (batch)
Presented a novel technique for initialization of BA
BA simulations
Demonstrated positioning accuracy of ~1 cm and attitude accuracy of ~ 0.1 ∘ in areas of GPS availability
Attained slow drift during GPS unavailability (0.4% drift over 50 m)

22. Navigation Filter State Vector: Propagation Step:
𝑿 𝐹 = 𝒙 𝐺 𝐶 𝑇 𝒗 𝐺 𝐶 𝑇 𝒃 𝐵 𝑓 𝑇 𝒒 𝐺 𝐶 𝑇 𝒃 𝐵 𝜔 𝑇 𝑇
Propagation Step:
Standard EKF propagation step using accelerometer and gyro measurements
Accelerometer and gyro biases modeled as a first-order Gauss-Markov processes
More information in paper …

23. Navigation Filter (cont.)
Measurement Update Step:
Image feature measurements from all non-keyframes
Temporarily augment the state with point feature positions
Prior from map produced by BA
Must ignore cross-covariances ⇒ filter inconsistency
Similar block diagonal structure in the normal equations as BA
𝑈 𝐹 𝑊 𝐹 𝑊 𝐹 𝑇 𝑉 𝐹 𝛿 𝑿 𝐹 𝛿𝒑 = 𝝐 𝐹 𝝐 𝑝
⇒ 𝑈 𝐹 − 𝑊 𝐹 𝑉 𝐹 −1 𝑊 𝐹 𝑇 0 𝑊 𝐹 𝑇 𝑉 𝐹 𝛿𝒄 𝛿𝒑 = 𝐼 − 𝑊 𝐹 𝑉 𝐹 −1 0 𝐼 𝝐 𝐹 𝝐 𝑝

24. Simulation Results for BA (cont.)