Lecture 13 Visual Inertial Fusion

Transcription

1 Lecture 13 Visual Inertial Fusion Davide Scaramuzza

2 Outline Introduction IMU model and Camera-IMU system Different paradigms Filtering Maximum a posteriori estimation Fix-lag smoothing 2

3 What is an IMU? Inertial Measurement Unit Angular velocity Linear Accelerations 3

4 What is an IMU? Different categories Mechanical Optical MEMS. For mobile robots: MEMS IMU Cheap Power efficient Light weight and solid state 4

5 MEMS Accelerometer A spring-like structure connects the device to a seismic mass vibrating in a capacity devider. A capacitive divider converts the displacement of the seismic mass into an electric signal. Damping is created by the gas sealed in the device. capacitive divider spring M a M a M

6 MEMS Gyroscopes MEMS gyroscopes measure the Coriolis forces acting on MEMS vibrating structures (tuning forks, vibrating wheels, or resonant solids) Their working principle is similar to the haltere of a fly Haltere are small structures of some two-winged insects, such as flies. They are flapped rapidly and function as gyroscopes, informing the insect about rotation of the body during flight. 6

7 Why IMU? Monocular vision is scale ambiguous. Pure vision is not robust enough Low texture High dynamic range High speed motion Robustness is a critical issue: Tesla accident The autopilot sensors on the Model S failed to distinguish a white tractor-trailer crossing the highway against a bright sky. 7

8 Why vision? Pure IMU integration will lead to large drift (especially cheap IMUs) Will see later mathematically Intuition - Integration of angular velocity to get orientation: error proportional to t - Double integration of acceleration to get position: if there is a bias in acceleration, the error of position is proportional to t 2 - Worse, the actually position error also depends on the error of orientation. Smartphone accelerometers 8

9 Why visual inertial fusion? Summary: IMU and vision are complementary Visual sensor Precise in case of non-aggressive motion Rich information for other purposes Inertial sensor Robust High output rate (~1,000 Hz) ᵡ ᵡ ᵡ Limited output rate (~100 Hz) Scale ambiguity in monocular setup. Lack of robustness ᵡ ᵡ Large relative uncertainty when at low acceleration/angular velocity Ambiguity in gravity / acceleration In common: state estimation based on visual or/and inertial sensor is dead-reckoning, which suffers from drifting over time. (solution: loop detection and loop closure) 9

10 Outline Introduction IMU model and Camera-IMU system Different paradigms Filtering Maximum a posteriori estimation Fix-lag smoothing 10

11 IMU model: Measurement Model Measures angular velocity and acceleration in the body frame: ω t ω t b t n t g g B WB B WB B WB BW W WB w a a a t R t a t g b t n t measurements noise where the superscript g stands for Gyroscope and a for Accelerometer Notations: Left subscript: reference frame in which the quantity is expressed Right subscript {Q}{Frame1}{Frame2}: Q of Frame2 with respect to Frame1 Noises are all in the body frame 11

12 IMU model: Noise Property Additive Gaussian white noise: E n t 0 2 E n t n t t t Bias: b g b() t bw t t, b a t i.e., the derivative of the bias is white Gaussian noise (so-called random walk) n Trawny, Nikolas, and Stergios I. Roumeliotis. "Indirect Kalman filter for 3D attitude estimation. g t, n a t d wk ~ N(0,1) n k w k d / t k k 1 k b b w bd w k b t ~ N(0,1) The biases are usually estimated with the other states can change every time the IMU is started can change due to temperature change, mechanical pressure, etc. bd 12

13 IMU model: Integration Per component: {t} stands for {B}ody frame at time t t2 Wt Wt 2 1 Wt Wt w d a p p t t v R t a t b t g t Depends on initial position and velocity The rotation R(t) is computed from the gyroscope t 1 2 Trawny, Nikolas, and Stergios I. Roumeliotis. "Indirect Kalman filter for 3D attitude estimation." 14

14 Camera-IMU System There can be multiple cameras. 15

15 Outline Introduction IMU model and Camera-IMU system Different paradigms Closed-form solution Filtering Fix-lag smoothing Maximum a posteriori estimation 16

16 Closed-form Solution (intuitive idea) The absolute pose x is known up to a scale s, thus x = sx From the IMU x = x 0 + v 0 (t 1 t 0 ) + t 1 a t dt t 0 By equating them t 1 sx = x 0 + v 0 t 1 t 0 + a t dt t 0 As shown in [Martinelli 14], for 6DOF, both s and v 0 can be determined in closed form from a single feature observation and 3 views. x 0 can be set to 0. Martinelli, Closed-form solution of visual-inertial structure from motion, International Journal of Computer Vision,

17 Closed-form Solution t 1 sx 1 = v 0 t 1 t 0 + a t dt t 0 t 2 sx 2 = v 0 t 2 t 0 + a t dt t 0 t 0 t 1 t 2 L 1 x 1 (t 0 t 1 ) x 2 (t 0 t 2 ) s v 0 = t 1 a t dt t 0 2 t 0 a t dt Martinelli, Closed-form solution of visual-inertial structure from motion, International Journal of Computer Vision,

18 Different paradigms Loosely coupled: use the output of individual system Estimate the states individually from visual and inertial data Combine the separate states estimations Tightly coupled: use the internal states Make use of the raw measurements - Feature positions - IMU readings - Example: - Use IMU for guided feature matching - Minimizing reprojection error and IMU error together - More accurate More implementation effort. 19

19 Different paradigms images Feature Extraction Feature Matching Feature correspondence Motion Estimation position attitude IMU measurements IMU Integration position attitude velocity Loosely coupled Tightly coupled More accurate More implementation effort. 20

20 Different paradigms Filtering Fix-lag Smoothing Maximum-A-Posteriori (MAP) Estimation Filtering the most recent states (e.g., extended Kalman filter) Optimize window of states Marginalization Nonlinear least squares optimization Optimize all states Nonlinear Least squares optimization 1 Linearization Accumulation of linearization errors Gaussian approximation of marginalized states Faster Re-Linearize Accumulation of linearization errors Gaussian approximation of marginalized states Fast Re-Linearize Sparse Matrices Highest Accuracy Slow 21

21 Filtering: Kalman Filter in a Nutshell Assumptions: linear system, Gaussian noise System dynamics 1 uk 1 vk 1 x k A k x k z k H k x k w k x(k): state u(k): control input, can be 0 z(k): measurement 0 ~ 0, 0 ~ 0, ~ 0, x N x P v k N Q k w k N R k Kalman Filter x 0 x, P 0 P m 0 m 0 Prediction xˆ k A k x k u k p p 1 ˆm 1 1 T 1 m 1 1 Qk 1 P k A k P k A k Measurement update m T 1 p ˆ P k P k H k R k H k xˆ k x k m m p T 1 ˆ 1 P k H k R k z k H k x k Weight between the model prediction and measurement p 22

22 Filtering: Kalman Filter in a Nutshell Nonlinear system: linearization System dynamics k1 1, 1, 1, x k q x k u k v k k z k h x k w k Process and measurement noise and initial state are Gaussian. Key idea: Linearize around the estimated states A(k) L(k) H(k) M(k) are partial derivatives with respect to states and noise Extended Kalman Filter Prediction xˆ k q xˆ k 1, u k 1,0 1 T 1 m 1 1 T Lk 1Q k 1 L k 1 p k m P k A k P k A k p Measurement update T T p ( p T M k Rk M k K k P k H k H k P k H k xˆ k xˆ k K k z k h xˆ,0 m p k p m p P k I K k H k P k ) 23 1

23 Filtering: Visual Inertial Formulation System states: Tightly coupled: a g t t t t t X Wp ; qwb ; Wv ; b ; b ; WL1; WL2;...,; W L Loosely coupled: ; ; ; a g X p t q t v t b t; b t W WB W K Process Model: from IMU Integration of IMU states (rotation, position, velocity) Propagation of IMU noise needed for calculating the Kalman Filter gain 24

24 Filtering: Visual Inertial Formulation Measurement Model: from camera Transform point to camera frame C x y R R L p p C z C CB BW W W CB Pinhole projection (without distortion) u v C x fx + cx C z C y fy cy C z H H H X proj H H H L B L proj pose Landmark H R R = R H Landmark CB BW CW H R R L pose CB BW B proj f 0 x 1 0 z f y f 1 f z x y x 2 z y 2 z Drop C for clarity 25

25 Filtering: ROVIO Use pixel intensities as measurements. Bloesch, Michael, et al. "Robust visual inertial odometry using a direct EKF-based approach." 26

26 Filtering: Potential Problems Wrong linearization point Linearization depends on the current estimates of states, which may be erroneous Linearization around different values of the same variable leads to estimator inconsistency (wrong observability/covariance estimation) Wrong covariance/initial states Intuitively, wrong weights for measurements and prediction May be overconfident/underconfident Explosion of number of states Roughly cubic of the number of the states Each 3D point: 3 variables 27

27 Filtering: Potential Problems Wrong linearization point Linearization depends on the current estimates of states, which may be erroneous Linearization around different values of the same variable leads to estimator inconsistency (wrong observability/covariance estimation) Wrong covariance/initial states Intuitively, wrong weights for measurements and prediction May be overconfident/underconfident Explosion of number of states Each 3D point: 3 variables 28

28 Filtering: MSCKF (Multi-State Constraint Kalman Filter): used in Google Tango Key idea: Keep a window of recent states incorporate visual observations without including point positions into the states Li, Mingyang, and Anastasios I. Mourikis. "High-precision, consistent EKF-based visual inertial odometry." 29

29 Filtering: Google Tango

30 Optimization-based Approaches (MAP, Fix-lag smoothing) Fusion solved as a non-linear optimization problem Increased accuracy over filtering methods * * { X, L } argmax P( X, L Z) { X, L} { X, L} N argmin f ( x ) x h( x, l ) z 2 k1 k i 1 k 1 k i j i k i M i 2 Forster, Carlone, Dellaert, Scaramuzza, IMU Preintegration on Manifold for efficient Visual-Inertial Maximum-a- Posteriori Estimation, Robotics Science and Systens 15, Best Paper Award Finalist

31 MAP: a nonlinear least squares problem Bayesian Theorem P( A B) P( B A) P( A) PB ( ) Applied to state estimation problem: X: states (position, attitude, velocity, and 3D point position) Z: measurements (feature positions, IMU readings) Max a Posteriori: given the observation, what is the optimal estimation of the states? Gaussian Property: for iid variables 1 2 x 2 1 i f ( x,..., x, ) e k Maximizing the probability is equivalent to minimizing the square root sum 32

32 MAP: a nonlinear least squares problem SLAM as a MAP problem x k f ( x ) k1 z h( x, l ) i i i k j X = {x 1, x N }: robot states L = {l 1, }: 3D points Z = {z i, z M }: feature positions P( X, L Z) P( Z X, L) P( X, L) M P( zi X, L) P( X ) i1 X L are independent, and no prior information about L Measurements are independent Markov process model M N P( x0) P( zi xi, l ) ( 1) k i P x j k x k i1 k2 33

33 MAP: a nonlinear least squares problem SLAM as a least squares problem (, ) ( ) M (, ) N P X L Z P x ( ) 0 P zi xi l k i P x j k x k 1 i1 k2 Without the prior, applying the property of Gaussian distribution: * * { X, L } argmax P( X, L Z) { X, L} Notes: { X, L} k N argmin f ( x ) x h( x, l ) z 2 k1 k i 1 k 1 k i j i i Normalize the residuals with the variance of process noise and measurement noise (so-called Mahalanobis distance) M i 2 34

34 MAP: Nonlinear optimization Gauss-Newton method Solve it iteratively M * 2 θ argmin fi( θ) zi θ i1 M * s 2 ε argmin fi( θ ε) zi i1 s1 s θ θ ε Applying first-order approximation: M * s 2 ε argmin fi ( θ ) zi Jiε i1 M s argmin r ( θ ) J ε i1 * T 1 T ε J J J r θ ( ) ( ) i i 2 J1 r1 J 2 r 2 J r JM rm 35

35 MAP: visual inertial formulation States a g X k R p k WB, q k WB, vwb[k], b k, b k,,..., X X 1, X 2,..., X k, X X L L L L W1 W2 WL Combined: Dynamics Jacobians R R R L IMU integration w.r.t x k-1 Residual w.r.t. x k f x k1 x K Measurements Jacobians (same as filtering method) Feature position w.r.t. pose Feature position w.r.t. 3D coordinates h x, L z ik ij i 36

36 MAP: why it is slow Re-linearization Need to recalculate the Jacobian for each iteration But it is also an important reason why MAP is accurate The number of states is large Will see next: fix-lag smoothing and marginalization Re-integration of IMU measurements The integration from k to k+1 is related to the state estimation at time k Preintegration Lupton, Todd, and Salah Sukkarieh. "Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions." Forster, Christian, et al. "IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation." 37

37 MAP: IMU Preintegration {ω, a} ΔR, Δv, Δp R, p, v Standard: Evaluate error in global frame: e R = R ω, R k 1 T R k ev = v(ω, a, v k 1 ) v k e p = p (ω, a, p k 1 ) p k Predicted Estimate Repeat integration when previous state changes! Preintegration: Evaluate relative errors: e R = ΔR T ΔR ev = Δv Δv e p = Δp Δp Preintegration of IMU deltas possible with no initial condition required. 38

38 Optimization-based Approaches (MAP, Fix-lag smoothing) Fusion solved as a non-linear optimization problem Increased accuracy over filtering methods IMU residuals Reprojection residuals Forster, Carlone, Dellaert, Scaramuzza, IMU Preintegration on Manifold for efficient Visual-Inertial Maximum-a- Posteriori Estimation, Robotics Science and Systens 15, Best Paper Award Finalist 39

39 MAP: SVO + IMU Preintegration Open Source Accuracy: 0.1% of the travel distance Proposed Google Tango OKVIS Forster, Carlone, Dellaert, Scaramuzza, IMU Preintegration on Manifold for efficient Visual-Inertial Maximum-a- Posteriori Estimation, Robotics Science and Systens 15 and IEEE TRO 16, Best Paper Award Finalist 40

40 MAP: SVO + IMU Preintegration 41

41 Fix-lag smoothing: Basic Idea Recall MAP estimation T J J * T 1 T ε J J J r θ ( ) ( ) is also called the Hessian matrix. Hessian for full bundle adjustment: n x n, n number of all the states pose, velocity landmarks If only part of the states are of interest, can we think of a way for simplification? 42

42 Fix-lag smoothing: Marginalization Schur complement A B M C D 1 D D CA B 1 A A BD C Schur complement of A in M Schur complement of D in M Reduced linear system A B x1 b1 C D x b A B x1 1 0 b1 1 1 CA 1 C D 2 CA 1 x b2 A B b 1 0 D b 2 b b CA b We can then just solve for x 2, and (optionally) solve for x 1 by back substitution. 43

43 Fix-lag smoothing: Marginalization Generalized Schur complement Any principal submatrix: selecting n rows and n columns of the same index (i.e., select any states to marginalize) Nonsingular submatrix: use generalized inverse (e.g., Moore Penrose pseudoinverse) Special structure of SLAM Marginalization causes fillin, no longer maintaining the sparse structure. Inverse of diagonal matrix is very efficient to calculate. D 1 44

44 Fix-lag smoothing: Implementation States and formulations are similar to MAP estimation. Which states to marginalize? Old states: keep a window of recent frames Landmarks: structureless Marginalizing states vs. dropping the states Dropping the states: loss of information, not optimal Marginalization: optimal if there is no linearization error, but introduces fill-in, causing performance penalty Therefore, dropping states is also used to trade accuracy for speed. Leutenegger, Stefan, et al. "Keyframe-based visual inertial odometry using nonlinear optimization." 45

45 Fix-lag smoothing: OKVIS 46

46 Further topics Rotation parameterization Rotation is tricky to deal with Euler angle / Rotation matrix / Quaternion / SO(3) Consistency: filtering and fix-lag smoothing Linearization around different values of the same variable may lead to error 47