1. A Monocular Pose Estimation System based on Infrared LEDs
Matthias Faessler, Elias Mueggler, Karl Schwabe, Davide Scaramuzza

2. The Problem Goal Mutual localization between robots
quadrotor & ground robot
collaboration
Monocular camera
Requirements
Cluttered scene
Illumination changes
Work indoors & outdoors

3. Approach Infrared LEDs with known locations on target object
Monocular camera with infrared-pass filter
Infrared LEDs
Easy to segment from image even in cluttered scene
Remain bright in image under illumination changes

4. Related Work
[Breitenmoser, 2011] «A monocular vision-based system for 6D relative robot localization»
Most similar work to our system
Passive & active markers
Problems:
Cluttered environments
Low & dynamic lighting conditions
[Censi, 2013] «Low-latency localization by active LED markers tracking using a dynamic vision sensor»
Event based camera
Track LEDs blinking at kiloherz frequencies
Low resolution (128 x 128 pixels)

5. Related Work ARTag [Fiala, 2007], AprilTag [Olson, 2011] Slow (20 Hz)
Restricted to a flat surface
Problems with illumination changes
Vicon & OptiTrack
Sub-millimeter accuracy
Expensive (~8k $)

6. P3P algorithm: Perspective from 3 Points
Given 3 object points & their image correspondences
Compute transformation of camera w.r.t. object frame
Returns 4 solutions
Need 4th point for disambiguation
OpenSource: (OpenGV) [Kneip&Scaramuzza, CVPR’11]
t = ?
[Kneip & Scaramuzza CVPR’11, “A novel parameterization of the Perspective-Three-Point Problem for a direct computation of the absolute camera position and orientation]

7. Correspondence search
Algorithm: Overview
LED configuration
LED detection
Correspondence search
Pose optimization
Pose with covariance
Image
Previous poses
Prediction

8. Algorithm: LED Detection
Original
Threshold
Gaussian blurring
Find blobs
𝐈 ′ 𝑢,𝑣 = 𝐈 𝑢,𝑣 , 0, if 𝐈 𝑢,𝑣 >threshold, otherwise
Method of Moments:
𝑀 𝑝𝑞 = 𝑢 𝑣 𝑢 𝑝 𝑣 𝑞 𝐈 ′ 𝑢,𝑣
𝑢 = 𝑀 10 / 𝑀 00 𝑣 = 𝑀 01 / 𝑀 00
Find blob centres
Exclude non-round blobs

9. Algorithm: Correspondence Search
Initially don’t know LED correspondences
Brute-force combinatorial approach
Every combination of 3 detections
Every permutation of 3 object points
Pose from P3P Solution
Reproject unused LEDs to check solutions 5 4 3 2 1
???
LEDs: A B C D

10. Algorithm: Correspondence Search
Combination: 1 2 3
Permutation: A B C
Project LED D
P3P Solutions P1 P2 P3 P4
No Correspondence
DP2 5
DP1
DP4 4 3 2 C
DP3 B 1 A

11. Algorithm: Correspondence Search
Combination: 2 3 4
Permutation: C D B
Project LED A
P3P Solutions P1 P2 P3 P4
Correct Correspondence
AP1 5
AP4
AP2 4 B 3
AP3 2 D C 1

12. Correspondence Histogram
Algorithm: Correspondence Search – LED-detection correspondence histogram
Combination: 2 3 4
Permutation: C D B
Project LED A
P3P Solutions P1 P2 P3 P4
Correspondence Histogram
Correct Correspondence L A B C D 1 2 3 4 5 5
AP2 4 B 3 2 D C 1

13. Correspondence Histogram
Algorithm: Correspondence Search – LED-detection correspondence histogram
Combination: 2 3 4
Permutation: C D B
Project LED A
P3P Solutions P1 P2 P3 P4
Correspondence Histogram
Correct Correspondence L A B C D 1 2 3 4 5 5
AP2 4 B 3 2 D C 1

14. Correspondence Histogram
Algorithm: Correspondence Search – LED-detection correspondence histogram
Combination: 2 4 5
Permutation: C B A
Project LED D
P3P Solutions P1 P2 P3 P4
Correspondence Histogram
Correct Correspondence L A B C D 1 2 3 4 5 5 A 4 B 3 2
DP1 C 1

15. Correspondence Histogram
Algorithm: Correspondence Search – LED-detection correspondence histogram
Combination: 1 2 3
Permutation: B C A
Project LED D
P3P Solutions P1 P2 P3 P4
Correspondence Histogram
Wrong Correspondence L A B C D 1 2 3 4 5 5 4
DP2 3 2 A C 1 B

16. Algorithm: Correspondences from Histogram
Search for highest number in each histogram column
Correspondence Histogram L A B C D 1 2 5 3 4 5 A 4 B 3 2 D C 1
Correspondences:
(A, 5), (B, 4), (C, 2), (D, 3)

17. Algorithm: Pose Optimization
Minimize reprojection error
𝑃 ∗ = arg min 𝑃 𝑘 π 𝒍 𝑘 ,𝑃 − 𝒅 𝑘 2
Use solution from P3P for initialization
Covariance
Σ 𝑃 = 𝐉 ⊤ 𝚺 𝒟 −1 𝐉 −1
where Σ 𝒟 = 𝐈 2×2 ⋅1 pixel 2 is a conservative guess for the covariance of the LED detections
𝐉∈ ℝ 2×6 is the Jacobian of the cost function with respect to the pose P on last iteration

18. Algorithm: Prediction
Use constant-velocity model
Angle-axis representation 𝜃 𝜔 for orientation
Predicted pose 𝑃 𝜔 𝜔 𝜃 𝜃 Δ𝑇 Δ𝑇
Pk-2
Pk-1 𝑃
𝐭 𝑘+1 = 𝒕 𝑘 +Δ𝑇 𝐭 𝑘 − 𝐭 k−1 𝜃 𝑘+1 = 𝜃 𝑘 +Δ𝑇 𝜃 𝑘 − 𝜃 k−1
Δ𝑇= 0, 𝑇 𝑘+1 − 𝑇 𝑘 / 𝑇 𝑘 − 𝑇 𝑘−1 , if 𝑛 𝑃 =1 if 𝑛 𝑃 ≥2
𝑃 =𝑓 𝜃 𝑘+1 𝜔 , 𝐭 𝑘+1

19. Algorithm: Estimating and Checking Correspondences
Predict image detections of LEDs
𝐝 𝑖 =π 𝒍 𝑖 , 𝑃
Initial correspondences determined by nearest neighbour search
Check projections for each combination of 3 correspondences
If initial correspondences incorrect, use brute-force correspondence search 1 2 3 4 5 B C D A
Positions of detected LEDs (red) and predicted positions of LEDs (green)

20. Experimental Setup mvBlueFOX-MLC200w camera with infrared-pass filter
752 x 480 pixels
90° FOV lens
Target object with 4 or 5 LEDs
Circumsphere radius of 10.9 cm
Benchmarked against
[Breitenmoser, 2011]
AprilTag with edge length of 23.8 cm
Ground truth from OptiTrack
Can track LEDs with OptiTrack directly

21. Results

22. Results AprilTags [Breitenmoser’11] Our system Mean Position Error
1.41
1.5
0.74 cm
Standard deviation
1.02
0.7
0.46
Max Position Error
11.2
12.1
3.28
Mean Orientation Error
1.53
1.2
0.79
Deg
Standard Deviation
1.61
0.4
0.41
Max Orientation Error
19.5
4.5
3.37

23. Quadrotor Stabilization
Used system to stabilize quadrotor

24. Results: Execution Time (ms)
nL= 4 LEDs
nL= 5 LEDs
Mean
Max
LED detection
2.7
5.1 ms
Correspondence search
1.4
6.0
5.2
14.4
Prediction
1.7 53
2.0 30 μs
Pose estimation 31 94 36 93
Total (without prediction)
5.0
10.1
9.0
17.6
Total (with prediction)
3.8
6.5
9.3
Cost of brute-force correspondence search very high
LED detection takes 71 % of time
Improve by restricting to region of interest

25. Discussion
Cheap, robust, accurate system for mutual robot localization
Works under strong illumination changes and in cluttered scenes
Robust to false LED detections and partial occlusions
Limited to line of sight operation
Max distance 6 m
Track larger LED balls instead
Open Source:
GitHub:
My lab webpage:

27. For Nd detections and NL LEDs on the object, we obtain N pose candidates

28. Future Work Tracking multiple targets
Use dynamic model of target object for prediction and filtering
Parallel computing
Setting the camera parameters for the ambient conditions automatically

29. Results

30. Self-Alignment L A B C D E 1 10 2 3 4 Before alignment
During alignment
After alignment L A B C D E 1 10 2 3 4

31. Occlusion L A B C D E 1 4 2 3 Before occlusion During occlusion
After occlusion L A B C D E 1 4 2 3

32. Minimize Reprojection Error
Minimize error between projected points and actual detections 1 2 3 4 5 B C D A 1 2 3 4 5 B C D A
Initialize with a P3P solution
Optimal solution

33. Pose Optimization Details
Minimize reprojection error
𝑃 ∗ = arg min 𝑃 𝑘 project 𝒍 𝑘 ,𝑃 − 𝒅 𝑘 2
Jacobian
𝐉= 𝑓 𝑥 𝑧 0 −𝑥∙ 𝑓 𝑥 𝑧 𝑓 𝑦 𝑧 −𝑦∙ 𝑓 𝑦 𝑧 −𝑥∙𝑦∙ 𝑓 𝑥 𝑧 𝑥 2 𝑧 2 𝑓 𝑥 −𝑦∙ 𝑓 𝑥 𝑧 − 1+ 𝑦 2 𝑧 2 𝑓 𝑦 𝑥∙𝑦∙ 𝑓 𝑦 𝑧 2 𝑥∙ 𝑓 𝑦 𝑧

34. Hardware Infrared LEDs Known positions Camera with IR-pass filter
Calibrate in OptiTrack
Non-symmetric
Not in a plane
Span large volume
Camera with IR-pass filter
Intrinsic parameters known
(1) Calibration stand, (2) LED test board, (3) AprilTag
(Approximate LED positions marked in green)