On the Use of Inverse Scaling in Monocular SLAM
On the Use of Inverse Scaling in Monocular SLAM
Daniele Marzorati1, Matteo Matteucci2, Davide Migliore2, Domenico G. Sorrenti1 1 Università
Friday, 25 September 2009
degli Studi di Milano - Bicocca 2 Politecnico di Milano
SLAM using Single Camera
15/05/2009 Friday, 25 September 2009
Slide n° 2
ICRA 2009 - Kobe, Japan
SLAM using Single Camera
Slide n° 2
‣ Why is this challenging? u v u v
z y x
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
SLAM using Single Camera
Slide n° 2
‣ Why is this challenging? u v u v
z y x
‣ Solutions:
- (offline) - S. Soatto et al “Structure from motion casually integrated over time” -
IEEE PAMI 2002 (online) - A. Davison “Real-time Simultaneous Localization And Mapping with a Single Camera” - ICCV 2003, PAMI 2007
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
SLAM using Single Camera
15/05/2009 Friday, 25 September 2009
Slide n° 3
ICRA 2009 - Kobe, Japan
Slide n° 3
SLAM using Single Camera ‣ Particle Filter approach (Delayed)
- EKF based SLAM
Pxx xc Pm1 x m1 xk = m2 , P = Pm x 2 .. ··· .
Pxm1 Pm1 m1 Pm2 m1
Pxm2 Pm1 m2 Pm2 m2
···
···
··· · · · . · · · .. .
r + (v + V )∆t r W C W C q q × q((ω C + ω C ))∆t , xc = W = W W v +V v ωC ω C + ΩC WC
WC
W
W
CW WC hC (mW ), n =R n −r
- 3 Parameters for Each Point - A. Davison et al. “MonoSLAM: Real-Time Single Camera SLAM” - PAMI 2007 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
SLAM using Single Camera
15/05/2009 Friday, 25 September 2009
Slide n° 4
ICRA 2009 - Kobe, Japan
SLAM using Single Camera
Slide n° 4
‣ Unified Inverse Depth Parametrization (Undelayed) x y X Z c
Θ
u
Y v
Φ d
P
Z X Y
- J. Civera et al. “Inverse Depth Parametrization for Monocular SLAM” - TRO 2008 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Slide n° 4
SLAM using Single Camera ‣ Unified Inverse Depth Parametrization (Undelayed) x y X Z
?
c
u
Y v
P
Z X
mx,n xn 1 mn = my,n = yn + Γ(θn , φn ), ρn mz,n zn Y
1 dn = ρn
! Γ(θn , φn ) = cosφn sinθn ,
−sinφn ,
cosφn cosθn
- J. Civera et al. “Inverse Depth Parametrization for Monocular SLAM” - TRO 2008 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
"T
.
Slide n° 4
SLAM using Single Camera ‣ Unified Inverse Depth Parametrization (Undelayed) x y X Z c
u
Y v
P
Z X
mx,n xn 1 mn = my,n = yn + Γ(θn , φn ), ρn mz,n zn Y
1 dn = ρn
! Γ(θn , φn ) = cosφn sinθn ,
−sinφn ,
cosφn cosθn
- J. Civera et al. “Inverse Depth Parametrization for Monocular SLAM” - TRO 2008 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
"T
.
Slide n° 4
SLAM using Single Camera ‣ Unified Inverse Depth Parametrization (Undelayed) x y X Z c
u
Y v
P
Z X
mx,n xn 1 mn = my,n = yn + Γ(θn , φn ), ρn mz,n zn Y
1 dn = ρn
! Γ(θn , φn ) = cosφn sinθn ,
- EKF based SLAM CW hC n =R
−sinφn ,
cosφn cosθn
xn ρn yn − rW C + Γ(θn , φn ) . zn
- 6 Parameters for Each Point - J. Civera et al. “Inverse Depth Parametrization for Monocular SLAM” - TRO 2008 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
"T
.
Slide n° 5
UID Improvements ‣ J. Civera et al. - “Inverse depth to depth conversion for
Monocular
SLAM” - ICRA 2007
‣ M. Pupilli et al. - “Real Time Visual SLAM with resilience to Erratic Motion” - CVPR 2006
‣ A. P. Gee et al. - “Discovering Planes and Collapsing the State Space in Visual SLAM” - BMVC 2007
‣ E. Eade et al. - “Monocular SLAM as a Graph of Coalesced Observations” - ICCV 2007
‣ G. Klein et al. - “Parallel Tracking And Mapping for Small AR workspaces” - ISMAR 2007
‣ T. Pietzsch - “Efficient Feature Parameterisation for visual SLAM using Inverse Depth Bundles” - BMVC 2008
‣ E. Imre et al. - “Improved Inverse Depth Parameterization for Monocular SLAM” - ICRA 2009
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
15/05/2009 Friday, 25 September 2009
Slide n° 6
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 6
‣ Idea: Y
X1 X X2
C f
15/05/2009 Friday, 25 September 2009
P
Z
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization ‣ Idea:
Slide n° 6
Viewing Ray Image Plane Y
X1 X X2
Camera Center C f
15/05/2009 Friday, 25 September 2009
P
Z
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization ‣ Idea:
Slide n° 6
Y
X
X X2
y x = (u,v)
X1
x
C
f X Y X= Z 1
15/05/2009 Friday, 25 September 2009
αi X αi Y Xi = αi Z 1
X Y 1 Xi ≡ Xi = Z α1 1/αi
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization ‣ Idea:
Slide n° 6
Y
X
X X2
y x = (u,v)
X1
x
C
f X Y X= Z 1
αi X αi Y Xi = αi Z 1
X Y 1 Xi ≡ Xi = Z α1 1/αi
X u u Y v v ≡ ≡ , X= Z f f 1 1/α ω 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 7
‣ Uncertainty Modeling - Gaussian approximation
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Slide n° 7
Inverse Scaling Parametrization ‣ Uncertainty Modeling - Gaussian approximation MonteCarlo simulation
Y
Ck
15/05/2009 Friday, 25 September 2009
X
Ck+1
ICRA 2009 - Kobe, Japan
Slide n° 7
Inverse Scaling Parametrization ‣ Uncertainty Modeling - Gaussian approximation MonteCarlo simulation
Y
Ck
X
Ck+1
X
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Slide n° 7
Inverse Scaling Parametrization ‣ Uncertainty Modeling - Gaussian approximation MonteCarlo simulation
Y
Ck
X
Ck+1
Y
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 7
‣ Uncertainty Modeling - Gaussian approximation Inverse Scaling Representation MonteCarlo simulation
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 7
‣ Uncertainty Modeling - Gaussian approximation Inverse Scaling Representation MonteCarlo simulation
X
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 7
‣ Uncertainty Modeling - Gaussian approximation Inverse Scaling Representation MonteCarlo simulation
Y
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 7
‣ Uncertainty Modeling - Gaussian approximation Inverse Scaling Representation MonteCarlo simulation
ω
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 7
‣ Uncertainty Modeling - Gaussian approximation Inverse Scaling Representation MonteCarlo simulation
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
MonoSLAM with Inverse Scaling
15/05/2009 Friday, 25 September 2009
Slide n° 8
ICRA 2009 - Kobe, Japan
MonoSLAM with Inverse Scaling
Slide n° 8
‣ Extended Kalman Filter
Video Frame
xk =
15/05/2009 Friday, 25 September 2009
!
xW Ck
vkCk
xW F1 k
...
xW Fn k
...
xW FN k
"T
ICRA 2009 - Kobe, Japan
MonoSLAM with Inverse Scaling
Slide n° 8
‣ Extended Kalman Filter FD Feature Detection
Video Frame
xk =
15/05/2009 Friday, 25 September 2009
!
xW Ck
vkCk
xW F1 k
...
xW Fn k
...
xW FN k
"T
ICRA 2009 - Kobe, Japan
MonoSLAM with Inverse Scaling
Slide n° 8
‣ Extended Kalman Filter FD Feature Initialization
Feature Detection
Prediction
Video Frame
xk =
!
xW Ck
vkCk
xW F1 k
x u − ccx y v − ccy z = fc ω ω ˆ
...
xW Fn k
...
xW FN k
"T
Update
SLAM Filter
ω ˆ = f c/(2 ∗ mind )
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
MonoSLAM with Inverse Scaling
Slide n° 8
‣ Extended Kalman Filter FD Feature Initialization
Feature Detection
Prediction
Video Frame
xk =
!
ˆk = x
15/05/2009 Friday, 25 September 2009
xW Ck
vkCk C
k−1 xW Ck−1 ⊕ xCk vkCk xW F1 k−1 .. . W xFN k−1
xW F1 k
...
vkCk , C xCk−1 k
xW Fn k
...
xW FN k
"T
Update
SLAM Filter
C
k−1 = vk−1 + a · ∆t,
C
k−1 = vk−1 · ∆t.
ˆ k = J1 Pk−1 JT + J2 QJT P 1 2 ! " ! " J1 = Jx Jv . . . JFn , J2 = Jak
ICRA 2009 - Kobe, Japan
MonoSLAM with Inverse Scaling
Slide n° 8
‣ Extended Kalman Filter FD Feature Initialization
Feature Detection
Prediction
Data Association
Video Frame
xk =
!
ˆk = x
15/05/2009 Friday, 25 September 2009
xW Ck
vkCk C
k−1 xW Ck−1 ⊕ xCk vkCk xW F1 k−1 .. . W xFN k−1
DA
xW F1 k
...
vkCk , C xCk−1 k
xW Fn k
...
xW FN k
"T
Update
SLAM Filter
C
k−1 = vk−1 + a · ∆t,
C
k−1 = vk−1 · ∆t.
ˆ k = J1 Pk−1 JT + J2 QJT P 1 2 ! " ! " J1 = Jx Jv . . . JFn , J2 = Jak
ICRA 2009 - Kobe, Japan
MonoSLAM with Inverse Scaling
Slide n° 8
‣ Extended Kalman Filter FD Feature Initialization
Feature Detection
Prediction
Data Association
Video Frame
xk =
!
k hC n
vkCk
xW Ck
DA
xW F1 k
...
xW Fn k
...
xW Fn Ck W hk,n = yFn − ωFWn rW = M RW Ck zFWn
ˆ k HT + Wk Rk WT S = Hk P k k T −1 ˆ kH S K=P k ˆ k − KSKT Pk = P ˆ k + K (zk − hk ) xk = x
15/05/2009 Friday, 25 September 2009
xW FN k C
k hx,n C
hz k Ck hy,n C
hz k
"T
Update
SLAM Filter
.
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
15/05/2009 Friday, 25 September 2009
Slide n° 9
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 9
‣ Improvements w.r.t. Unified Inverse Depth: - Measurement model non-linearity d
P˜
y0
x0 x1
d0
1/#
"0
0
u0
#0 -
1
1/(
f=
d1 y1
D
!
P
#)
!1 u1
Camera 0
f=1 Camera 1
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Slide n° 9
Inverse Scaling Parametrization ‣ Improvements w.r.t. Unified Inverse Depth: - Measurement model non-linearity d
2
y0
x0
P˜
! ∂ f! ∂x2 !
! ! ! ! L=! ! !
x1
x
L
0.9
0.8
0.7
d0
D
!
P
! ! 2σx ! ! µx =0 ! !, ∂f ! ! ! ! ∂x µ =0
0.6
0.5
1/#
"0
0
α
1
1/( #
0.4
f=
d1 y1
u0 0
-#)
0.3
!1
0.2
u1
Camera 0
f=1 Camera 1
0.1
0
10
2 0.1
3 0.2
4 0.3
5 0.4
6 0.5
7 0.6
θ0 [rad]
- J. Civera et al. - “Inverse depth to depth conversion for monocular SLAM” ICRA 2007
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
8 0.7
Inverse Scaling Parametrization
15/05/2009 Friday, 25 September 2009
Slide n° 10
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 10
‣ Improvements w.r.t. Unified Inverse Depth:
- Uncertainty Modeling - Comparison with Inverse Depth Feature - 2.5m
Feature - 15m
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Feature - 2.5m
Inverse Scaling Parametrization
Slide n° 10
‣ Improvements w.r.t. Unified Inverse Depth:
- Uncertainty Modeling - Comparison with Inverse Depth Feature - 2.5m
Feature - 15m
X
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Feature - 2.5m
Inverse Scaling Parametrization
Slide n° 10
‣ Improvements w.r.t. Unified Inverse Depth:
- Uncertainty Modeling - Comparison with Inverse Depth Feature - 2.5m
Feature - 15m
Y
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Feature - 15m
Inverse Scaling Parametrization
Slide n° 10
‣ Improvements w.r.t. Unified Inverse Depth:
- Uncertainty Modeling - Comparison with Inverse Depth Feature - 2.5m
Feature - 15m
Y 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 10
‣ Improvements w.r.t. Unified Inverse Depth:
- Uncertainty Modeling - Comparison with Inverse Depth Feature - 2.5m
Feature - 15m
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Changing Reference Frame
15/05/2009 Friday, 25 September 2009
Slide n° 11
ICRA 2009 - Kobe, Japan
Changing Reference Frame
Slide n° 11
Without uncertainty into rototranslation
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Changing Reference Frame
15/05/2009 Friday, 25 September 2009
Slide n° 12
ICRA 2009 - Kobe, Japan
Changing Reference Frame
Slide n° 12
With uncertainty into rototranslation
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Experimental Results
15/05/2009 Friday, 25 September 2009
Slide n° 13
ICRA 2009 - Kobe, Japan
Experimental Results
Slide n° 13
‣ Simulated Dataset
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Experimental Results
15/05/2009 Friday, 25 September 2009
Slide n° 14
ICRA 2009 - Kobe, Japan
Experimental Results
15/05/2009 Friday, 25 September 2009
Slide n° 14
ICRA 2009 - Kobe, Japan
Experimental Results
15/05/2009 Friday, 25 September 2009
Slide n° 15
ICRA 2009 - Kobe, Japan
Experimental Results
15/05/2009 Friday, 25 September 2009
Slide n° 15
ICRA 2009 - Kobe, Japan
Conclusions
15/05/2009 Friday, 25 September 2009
Slide n° 16
ICRA 2009 - Kobe, Japan
Slide n° 16
Conclusions
‣ Presented a new parametrization for Monocular SLAM - Proper Uncertainty Modeling for both low and high parallax - More linear than Unified Inverse Depth - Only 4 parameters required - Undelayed initialization
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Slide n° 16
Conclusions
‣ Presented a new parametrization for Monocular SLAM - Proper Uncertainty Modeling for both low and high parallax - More linear than Unified Inverse Depth - Only 4 parameters required - Undelayed initialization
‣ Ongoing works:
- MonoSLAM with Bearing Only Tracking - Integration on Large Maps - Self calibration
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Slide n° 16
Conclusions
‣ Presented a new parametrization for Monocular SLAM - Proper Uncertainty Modeling for both low and high parallax - More linear than Unified Inverse Depth - Only 4 parameters required - Undelayed initialization
‣ Ongoing works:
- MonoSLAM with Bearing Only Tracking - Integration on Large Maps - Self calibration
Thanks! Any question? 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Daniele Marzorati1, Matteo Matteucci2, Davide Migliore2, Domenico G. Sorrenti1 1 Università
Friday, 25 September 2009
degli Studi di Milano - Bicocca 2 Politecnico di Milano
SLAM using Single Camera
15/05/2009 Friday, 25 September 2009
Slide n° 2
ICRA 2009 - Kobe, Japan
SLAM using Single Camera
Slide n° 2
‣ Why is this challenging? u v u v
z y x
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
SLAM using Single Camera
Slide n° 2
‣ Why is this challenging? u v u v
z y x
‣ Solutions:
- (offline) - S. Soatto et al “Structure from motion casually integrated over time” -
IEEE PAMI 2002 (online) - A. Davison “Real-time Simultaneous Localization And Mapping with a Single Camera” - ICCV 2003, PAMI 2007
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
SLAM using Single Camera
15/05/2009 Friday, 25 September 2009
Slide n° 3
ICRA 2009 - Kobe, Japan
Slide n° 3
SLAM using Single Camera ‣ Particle Filter approach (Delayed)
- EKF based SLAM
Pxx xc Pm1 x m1 xk = m2 , P = Pm x 2 .. ··· .
Pxm1 Pm1 m1 Pm2 m1
Pxm2 Pm1 m2 Pm2 m2
···
···
··· · · · . · · · .. .
r + (v + V )∆t r W C W C q q × q((ω C + ω C ))∆t , xc = W = W W v +V v ωC ω C + ΩC WC
WC
W
W
CW WC hC (mW ), n =R n −r
- 3 Parameters for Each Point - A. Davison et al. “MonoSLAM: Real-Time Single Camera SLAM” - PAMI 2007 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
SLAM using Single Camera
15/05/2009 Friday, 25 September 2009
Slide n° 4
ICRA 2009 - Kobe, Japan
SLAM using Single Camera
Slide n° 4
‣ Unified Inverse Depth Parametrization (Undelayed) x y X Z c
Θ
u
Y v
Φ d
P
Z X Y
- J. Civera et al. “Inverse Depth Parametrization for Monocular SLAM” - TRO 2008 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Slide n° 4
SLAM using Single Camera ‣ Unified Inverse Depth Parametrization (Undelayed) x y X Z
?
c
u
Y v
P
Z X
mx,n xn 1 mn = my,n = yn + Γ(θn , φn ), ρn mz,n zn Y
1 dn = ρn
! Γ(θn , φn ) = cosφn sinθn ,
−sinφn ,
cosφn cosθn
- J. Civera et al. “Inverse Depth Parametrization for Monocular SLAM” - TRO 2008 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
"T
.
Slide n° 4
SLAM using Single Camera ‣ Unified Inverse Depth Parametrization (Undelayed) x y X Z c
u
Y v
P
Z X
mx,n xn 1 mn = my,n = yn + Γ(θn , φn ), ρn mz,n zn Y
1 dn = ρn
! Γ(θn , φn ) = cosφn sinθn ,
−sinφn ,
cosφn cosθn
- J. Civera et al. “Inverse Depth Parametrization for Monocular SLAM” - TRO 2008 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
"T
.
Slide n° 4
SLAM using Single Camera ‣ Unified Inverse Depth Parametrization (Undelayed) x y X Z c
u
Y v
P
Z X
mx,n xn 1 mn = my,n = yn + Γ(θn , φn ), ρn mz,n zn Y
1 dn = ρn
! Γ(θn , φn ) = cosφn sinθn ,
- EKF based SLAM CW hC n =R
−sinφn ,
cosφn cosθn
xn ρn yn − rW C + Γ(θn , φn ) . zn
- 6 Parameters for Each Point - J. Civera et al. “Inverse Depth Parametrization for Monocular SLAM” - TRO 2008 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
"T
.
Slide n° 5
UID Improvements ‣ J. Civera et al. - “Inverse depth to depth conversion for
Monocular
SLAM” - ICRA 2007
‣ M. Pupilli et al. - “Real Time Visual SLAM with resilience to Erratic Motion” - CVPR 2006
‣ A. P. Gee et al. - “Discovering Planes and Collapsing the State Space in Visual SLAM” - BMVC 2007
‣ E. Eade et al. - “Monocular SLAM as a Graph of Coalesced Observations” - ICCV 2007
‣ G. Klein et al. - “Parallel Tracking And Mapping for Small AR workspaces” - ISMAR 2007
‣ T. Pietzsch - “Efficient Feature Parameterisation for visual SLAM using Inverse Depth Bundles” - BMVC 2008
‣ E. Imre et al. - “Improved Inverse Depth Parameterization for Monocular SLAM” - ICRA 2009
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
15/05/2009 Friday, 25 September 2009
Slide n° 6
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 6
‣ Idea: Y
X1 X X2
C f
15/05/2009 Friday, 25 September 2009
P
Z
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization ‣ Idea:
Slide n° 6
Viewing Ray Image Plane Y
X1 X X2
Camera Center C f
15/05/2009 Friday, 25 September 2009
P
Z
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization ‣ Idea:
Slide n° 6
Y
X
X X2
y x = (u,v)
X1
x
C
f X Y X= Z 1
15/05/2009 Friday, 25 September 2009
αi X αi Y Xi = αi Z 1
X Y 1 Xi ≡ Xi = Z α1 1/αi
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization ‣ Idea:
Slide n° 6
Y
X
X X2
y x = (u,v)
X1
x
C
f X Y X= Z 1
αi X αi Y Xi = αi Z 1
X Y 1 Xi ≡ Xi = Z α1 1/αi
X u u Y v v ≡ ≡ , X= Z f f 1 1/α ω 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 7
‣ Uncertainty Modeling - Gaussian approximation
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Slide n° 7
Inverse Scaling Parametrization ‣ Uncertainty Modeling - Gaussian approximation MonteCarlo simulation
Y
Ck
15/05/2009 Friday, 25 September 2009
X
Ck+1
ICRA 2009 - Kobe, Japan
Slide n° 7
Inverse Scaling Parametrization ‣ Uncertainty Modeling - Gaussian approximation MonteCarlo simulation
Y
Ck
X
Ck+1
X
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Slide n° 7
Inverse Scaling Parametrization ‣ Uncertainty Modeling - Gaussian approximation MonteCarlo simulation
Y
Ck
X
Ck+1
Y
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 7
‣ Uncertainty Modeling - Gaussian approximation Inverse Scaling Representation MonteCarlo simulation
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 7
‣ Uncertainty Modeling - Gaussian approximation Inverse Scaling Representation MonteCarlo simulation
X
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 7
‣ Uncertainty Modeling - Gaussian approximation Inverse Scaling Representation MonteCarlo simulation
Y
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 7
‣ Uncertainty Modeling - Gaussian approximation Inverse Scaling Representation MonteCarlo simulation
ω
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 7
‣ Uncertainty Modeling - Gaussian approximation Inverse Scaling Representation MonteCarlo simulation
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
MonoSLAM with Inverse Scaling
15/05/2009 Friday, 25 September 2009
Slide n° 8
ICRA 2009 - Kobe, Japan
MonoSLAM with Inverse Scaling
Slide n° 8
‣ Extended Kalman Filter
Video Frame
xk =
15/05/2009 Friday, 25 September 2009
!
xW Ck
vkCk
xW F1 k
...
xW Fn k
...
xW FN k
"T
ICRA 2009 - Kobe, Japan
MonoSLAM with Inverse Scaling
Slide n° 8
‣ Extended Kalman Filter FD Feature Detection
Video Frame
xk =
15/05/2009 Friday, 25 September 2009
!
xW Ck
vkCk
xW F1 k
...
xW Fn k
...
xW FN k
"T
ICRA 2009 - Kobe, Japan
MonoSLAM with Inverse Scaling
Slide n° 8
‣ Extended Kalman Filter FD Feature Initialization
Feature Detection
Prediction
Video Frame
xk =
!
xW Ck
vkCk
xW F1 k
x u − ccx y v − ccy z = fc ω ω ˆ
...
xW Fn k
...
xW FN k
"T
Update
SLAM Filter
ω ˆ = f c/(2 ∗ mind )
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
MonoSLAM with Inverse Scaling
Slide n° 8
‣ Extended Kalman Filter FD Feature Initialization
Feature Detection
Prediction
Video Frame
xk =
!
ˆk = x
15/05/2009 Friday, 25 September 2009
xW Ck
vkCk C
k−1 xW Ck−1 ⊕ xCk vkCk xW F1 k−1 .. . W xFN k−1
xW F1 k
...
vkCk , C xCk−1 k
xW Fn k
...
xW FN k
"T
Update
SLAM Filter
C
k−1 = vk−1 + a · ∆t,
C
k−1 = vk−1 · ∆t.
ˆ k = J1 Pk−1 JT + J2 QJT P 1 2 ! " ! " J1 = Jx Jv . . . JFn , J2 = Jak
ICRA 2009 - Kobe, Japan
MonoSLAM with Inverse Scaling
Slide n° 8
‣ Extended Kalman Filter FD Feature Initialization
Feature Detection
Prediction
Data Association
Video Frame
xk =
!
ˆk = x
15/05/2009 Friday, 25 September 2009
xW Ck
vkCk C
k−1 xW Ck−1 ⊕ xCk vkCk xW F1 k−1 .. . W xFN k−1
DA
xW F1 k
...
vkCk , C xCk−1 k
xW Fn k
...
xW FN k
"T
Update
SLAM Filter
C
k−1 = vk−1 + a · ∆t,
C
k−1 = vk−1 · ∆t.
ˆ k = J1 Pk−1 JT + J2 QJT P 1 2 ! " ! " J1 = Jx Jv . . . JFn , J2 = Jak
ICRA 2009 - Kobe, Japan
MonoSLAM with Inverse Scaling
Slide n° 8
‣ Extended Kalman Filter FD Feature Initialization
Feature Detection
Prediction
Data Association
Video Frame
xk =
!
k hC n
vkCk
xW Ck
DA
xW F1 k
...
xW Fn k
...
xW Fn Ck W hk,n = yFn − ωFWn rW = M RW Ck zFWn
ˆ k HT + Wk Rk WT S = Hk P k k T −1 ˆ kH S K=P k ˆ k − KSKT Pk = P ˆ k + K (zk − hk ) xk = x
15/05/2009 Friday, 25 September 2009
xW FN k C
k hx,n C
hz k Ck hy,n C
hz k
"T
Update
SLAM Filter
.
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
15/05/2009 Friday, 25 September 2009
Slide n° 9
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 9
‣ Improvements w.r.t. Unified Inverse Depth: - Measurement model non-linearity d
P˜
y0
x0 x1
d0
1/#
"0
0
u0
#0 -
1
1/(
f=
d1 y1
D
!
P
#)
!1 u1
Camera 0
f=1 Camera 1
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Slide n° 9
Inverse Scaling Parametrization ‣ Improvements w.r.t. Unified Inverse Depth: - Measurement model non-linearity d
2
y0
x0
P˜
! ∂ f! ∂x2 !
! ! ! ! L=! ! !
x1
x
L
0.9
0.8
0.7
d0
D
!
P
! ! 2σx ! ! µx =0 ! !, ∂f ! ! ! ! ∂x µ =0
0.6
0.5
1/#
"0
0
α
1
1/( #
0.4
f=
d1 y1
u0 0
-#)
0.3
!1
0.2
u1
Camera 0
f=1 Camera 1
0.1
0
10
2 0.1
3 0.2
4 0.3
5 0.4
6 0.5
7 0.6
θ0 [rad]
- J. Civera et al. - “Inverse depth to depth conversion for monocular SLAM” ICRA 2007
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
8 0.7
Inverse Scaling Parametrization
15/05/2009 Friday, 25 September 2009
Slide n° 10
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 10
‣ Improvements w.r.t. Unified Inverse Depth:
- Uncertainty Modeling - Comparison with Inverse Depth Feature - 2.5m
Feature - 15m
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Feature - 2.5m
Inverse Scaling Parametrization
Slide n° 10
‣ Improvements w.r.t. Unified Inverse Depth:
- Uncertainty Modeling - Comparison with Inverse Depth Feature - 2.5m
Feature - 15m
X
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Feature - 2.5m
Inverse Scaling Parametrization
Slide n° 10
‣ Improvements w.r.t. Unified Inverse Depth:
- Uncertainty Modeling - Comparison with Inverse Depth Feature - 2.5m
Feature - 15m
Y
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Feature - 15m
Inverse Scaling Parametrization
Slide n° 10
‣ Improvements w.r.t. Unified Inverse Depth:
- Uncertainty Modeling - Comparison with Inverse Depth Feature - 2.5m
Feature - 15m
Y 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Inverse Scaling Parametrization
Slide n° 10
‣ Improvements w.r.t. Unified Inverse Depth:
- Uncertainty Modeling - Comparison with Inverse Depth Feature - 2.5m
Feature - 15m
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Changing Reference Frame
15/05/2009 Friday, 25 September 2009
Slide n° 11
ICRA 2009 - Kobe, Japan
Changing Reference Frame
Slide n° 11
Without uncertainty into rototranslation
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Changing Reference Frame
15/05/2009 Friday, 25 September 2009
Slide n° 12
ICRA 2009 - Kobe, Japan
Changing Reference Frame
Slide n° 12
With uncertainty into rototranslation
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Experimental Results
15/05/2009 Friday, 25 September 2009
Slide n° 13
ICRA 2009 - Kobe, Japan
Experimental Results
Slide n° 13
‣ Simulated Dataset
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Experimental Results
15/05/2009 Friday, 25 September 2009
Slide n° 14
ICRA 2009 - Kobe, Japan
Experimental Results
15/05/2009 Friday, 25 September 2009
Slide n° 14
ICRA 2009 - Kobe, Japan
Experimental Results
15/05/2009 Friday, 25 September 2009
Slide n° 15
ICRA 2009 - Kobe, Japan
Experimental Results
15/05/2009 Friday, 25 September 2009
Slide n° 15
ICRA 2009 - Kobe, Japan
Conclusions
15/05/2009 Friday, 25 September 2009
Slide n° 16
ICRA 2009 - Kobe, Japan
Slide n° 16
Conclusions
‣ Presented a new parametrization for Monocular SLAM - Proper Uncertainty Modeling for both low and high parallax - More linear than Unified Inverse Depth - Only 4 parameters required - Undelayed initialization
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Slide n° 16
Conclusions
‣ Presented a new parametrization for Monocular SLAM - Proper Uncertainty Modeling for both low and high parallax - More linear than Unified Inverse Depth - Only 4 parameters required - Undelayed initialization
‣ Ongoing works:
- MonoSLAM with Bearing Only Tracking - Integration on Large Maps - Self calibration
15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
Slide n° 16
Conclusions
‣ Presented a new parametrization for Monocular SLAM - Proper Uncertainty Modeling for both low and high parallax - More linear than Unified Inverse Depth - Only 4 parameters required - Undelayed initialization
‣ Ongoing works:
- MonoSLAM with Bearing Only Tracking - Integration on Large Maps - Self calibration
Thanks! Any question? 15/05/2009 Friday, 25 September 2009
ICRA 2009 - Kobe, Japan
