On the Use of Inverse Scaling in Monocular SLAM

Transcription

1 On the Use of Inverse Scaling in Monocular SLAM Daniele Marzorati 1, Matteo Matteucci 2, Davide Migliore 2, Domenico G. Sorrenti 1 1 Università degli Studi di Milano - Bicocca 2 Politecnico di Milano

2 SLAM using Single Camera Slide n 2

3 SLAM using Single Camera Slide n 2 Why is this challenging? v u u v z x y

4 SLAM using Single Camera Slide n 2 Why is this challenging? v u u v z x y Solutions: - (offline) - S. Soatto et al Structure from motion casually integrated over time - IEEE PAMI (online) - A. Davison Real-time Simultaneous Localization And Mapping with a Single Camera - ICCV 2003, PAMI 2007

5 SLAM using Single Camera Slide n 3

6 SLAM using Single Camera Slide n 3 Particle Filter approach (Delayed) - EKF based SLAM x k = x c m 1 m 2. P xx P xm1 P xm2, P = P m1 x P m1 m 1 P m1 m 2 P m2 x P m2 m 1 P m2 m x c = r WC q WC v W ω C r WC +(v W + V W ) t = q WC q((ω C + ω C )) t v W + V W, ω C + Ω C h C n = R CW (m W n r WC ), - 3 Parameters for Each Point - A. Davison et al. MonoSLAM: Real-Time Single Camera SLAM - PAMI 2007

7 SLAM using Single Camera Slide n 4

8 SLAM using Single Camera Slide n 4 Unified Inverse Depth Parametrization (Undelayed) y x X Z c Θ Φ d u v P Y Z X Y - J. Civera et al. Inverse Depth Parametrization for Monocular SLAM - TRO 2008

9 SLAM using Single Camera Slide n 4 Unified Inverse Depth Parametrization (Undelayed) x y X? c Z u Y v P Z d n = 1 ρ n m n = Y X m x,n m y,n = m z,n x n y n z n + 1 ρ n Γ(θ n, φ n ), Γ(θ n, φ n )= ( cosφ n sinθ n, sinφ n, cosφ n cosθ n ) T. - J. Civera et al. Inverse Depth Parametrization for Monocular SLAM - TRO 2008

10 SLAM using Single Camera Slide n 4 Unified Inverse Depth Parametrization (Undelayed) x y X Z c u Y v P Z d n = 1 ρ n m n = Y X m x,n m y,n = m z,n x n y n z n + 1 ρ n Γ(θ n, φ n ), Γ(θ n, φ n )= ( cosφ n sinθ n, sinφ n, cosφ n cosθ n ) T. - J. Civera et al. Inverse Depth Parametrization for Monocular SLAM - TRO 2008

11 SLAM using Single Camera Slide n 4 Unified Inverse Depth Parametrization (Undelayed) x y X Z c u Y v P Z d n = 1 ρ n m n = Y X m x,n m y,n = m z,n x n y n z n + 1 ρ n Γ(θ n, φ n ), Γ(θ n, φ n )= ( cosφ n sinθ n, sinφ n, cosφ n cosθ n ) T. - EKF based SLAM x n h C n = R CW ρ n y n r WC + Γ(θ n, φ n ). z n - 6 Parameters for Each Point - J. Civera et al. Inverse Depth Parametrization for Monocular SLAM - TRO 2008

12 UID Improvements Slide n 5 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

13 Inverse Scaling Parametrization Slide n 6

14 Inverse Scaling Parametrization Slide n 6 Idea: Y X X 1 X 2 C f P Z

15 Inverse Scaling Parametrization Slide n 6 Idea: Image Plane Viewing Ray Y X X 1 X 2 Camera Center C f P Z

16 Inverse Scaling Parametrization Slide n 6 Idea: Y X X X 1 x = (u,v) y x X 2 C X = X Y Z 1 f α i X α i Y α i Z 1 X X i 1 i = X i = α 1 X Y Z 1/α i

17 Inverse Scaling Parametrization Slide n 6 Idea: Y X X X 1 x = (u,v) y x X 2 C X = X Y Z 1 f α i X α i Y α i Z 1 X X i 1 i = X i = α 1 X Y Z 1/α i X = X Y Z 1 u v f 1/α u v f, ω

18 Inverse Scaling Parametrization Slide n 7 Uncertainty Modeling - Gaussian approximation

19 Inverse Scaling Parametrization Slide n 7 Uncertainty Modeling - Gaussian approximation MonteCarlo simulation Y C k C k+1 X

20 Inverse Scaling Parametrization Slide n 7 Uncertainty Modeling - Gaussian approximation MonteCarlo simulation Y C k C k+1 X X

21 Inverse Scaling Parametrization Slide n 7 Uncertainty Modeling - Gaussian approximation MonteCarlo simulation Y C k C k+1 X Y

22 Inverse Scaling Parametrization Slide n 7 Uncertainty Modeling - Gaussian approximation MonteCarlo simulation Inverse Scaling Representation

23 Inverse Scaling Parametrization Slide n 7 Uncertainty Modeling - Gaussian approximation MonteCarlo simulation Inverse Scaling Representation X

24 Inverse Scaling Parametrization Slide n 7 Uncertainty Modeling - Gaussian approximation MonteCarlo simulation Inverse Scaling Representation Y

25 Inverse Scaling Parametrization Slide n 7 Uncertainty Modeling - Gaussian approximation MonteCarlo simulation Inverse Scaling Representation ω

26 Inverse Scaling Parametrization Slide n 7 Uncertainty Modeling - Gaussian approximation MonteCarlo simulation Inverse Scaling Representation

27 MonoSLAM with Inverse Scaling Slide n 8

28 MonoSLAM with Inverse Scaling Slide n 8 Extended Kalman Filter x k = Video Frame [ x W C k v C k k x W F 1 k... x W F n k... x W F N k ] T

29 MonoSLAM with Inverse Scaling Slide n 8 Extended Kalman Filter FD Feature Detection x k = Video Frame [ x W C k v C k k x W F 1 k... x W F n k... x W F N k ] T

30 MonoSLAM with Inverse Scaling Slide n 8 Extended Kalman Filter FD Feature Detection Feature Initialization Prediction x k = Video Frame [ x W C k x y z ω = v C k k x W F 1 k... x W F n k... x W F N k ] T u cc x v cc y fc ˆω Update SLAM Filter ˆω = fc/(2 min d )

31 MonoSLAM with Inverse Scaling Slide n 8 Extended Kalman Filter FD Feature Detection Feature Initialization Prediction x k = ˆx k = Video Frame [ x W C k x W C k 1 x C k 1 C k v C k k x W F 1 k 1. v C k k x W F 1 k... x W F n k... x W F N k ] T, v C k k = v C k 1 k 1 x C k 1 C k = v C k 1 k 1 + a t, t. Update SLAM Filter x W F N k 1 ˆP k = J 1 P k 1 J T 1 + J 2 QJ T 2 J 1 = [ ] J x J v... J Fn, J2 = [ ] J ak

32 MonoSLAM with Inverse Scaling Slide n 8 Extended Kalman Filter FD Feature Detection Feature Initialization Prediction Data Association x k = ˆx k = Video Frame [ x W C k x W C k 1 x C k 1 C k v C k k x W F 1 k 1. v C k k x W F 1 k... x W F n k... x W F N k ] T, DA v C k k = v C k 1 k 1 x C k 1 C k = v C k 1 k 1 + a t, t. Update SLAM Filter x W F N k 1 ˆP k = J 1 P k 1 J T 1 + J 2 QJ T 2 J 1 = [ ] J x J v... J Fn, J2 = [ ] J ak

33 MonoSLAM with Inverse Scaling Slide n 8 Extended Kalman Filter FD Feature Detection Feature Initialization Prediction Data Association x k = Video Frame [ x W C k DA v C k k x W F 1 k... x W F n k... x W F N k ] T Update h C k n = M R C k W x W F n y W F n z W F n ωf W n r W C k h k,n = h C k x,n h C k z h C k y,n h C k z. SLAM Filter S = H k ˆPk H T k + W kr k W T k K = ˆP k H T k S 1 P k = ˆP k KSK T x k = ˆx k + K (z k h k )

34 Inverse Scaling Parametrization Slide n 9

35 Inverse Scaling Parametrization Slide n 9 Improvements w.r.t. Unified Inverse Depth: - Measurement model non-linearity x 0 y 0 d d 0 P x 1 P! D 1/# 0 " 0 u 0 f = 1 d 1 y 1 1/(# 0 -#)! 1 u 1 Camera 0 f = 1 Camera 1

36 Inverse Scaling Parametrization Slide n 9 Improvements w.r.t. Unified Inverse Depth: - Measurement model non-linearity P x 0 P x 1 y 0 d d 0! L = D 2 f µx x 2σ 2 x =0 f, x µx =0 L d 1 y 1 1/# 0 " 0 u 0 f = α 1/(# 0 -#) 0.3! u 1 Camera f = 1 Camera θ 0 [rad] - J. Civera et al. - Inverse depth to depth conversion for monocular SLAM - ICRA 2007

37 Inverse Scaling Parametrization Slide n 10

38 Inverse Scaling Parametrization Slide n 10 Improvements w.r.t. Unified Inverse Depth: - Uncertainty Modeling - Comparison with Inverse Depth Feature - 2.5m Feature - 15m

39 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

40 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

41 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

42 Inverse Scaling Parametrization Slide n 10 Improvements w.r.t. Unified Inverse Depth: - Uncertainty Modeling - Comparison with Inverse Depth Feature - 2.5m Feature - 15m

43 Changing Reference Frame Slide n 11

44 Changing Reference Frame Slide n 11 Without uncertainty into rototranslation

45 Changing Reference Frame Slide n 12

46 Changing Reference Frame Slide n 12 With uncertainty into rototranslation

47 Experimental Results Slide n 13

48 Experimental Results Slide n 13 Simulated Dataset

49 Experimental Results Slide n 14

50 Experimental Results Slide n 14

51 Experimental Results Slide n 15

52 Experimental Results Slide n 15

53 Conclusions Slide n 16

54 Conclusions Slide n 16 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

55 Conclusions Slide n 16 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

56 Conclusions Slide n 16 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?