Efficient Visual-Inertial Navigation using a Rolling-Shutter Camera with Inaccurate Timestamps

Transcription

1 Efficient Visua-Inertia Navigation using a Roing-Shutter Camera with Inaccurate Timestamps Chao X. Guo, Dimitrios G. Kottas, Ryan C. DuToit Ahmed Ahmed, Ruipeng Li and Stergios I. Roumeiotis Mutipe Autonomous Robotic Systems Laboratory Technica Report Number February 214 Repository Revision: Last Update: Dept. of Computer Science & Engineering University of Minnesota EE/CS Buiding 2 Union St. S.E. Minneapois, MN Te: ( Fax: ( URL:

2

3 Efficient Visua-Inertia Navigation using a Roing-Shutter Camera with Inaccurate Timestamps Chao X. Guo, Dimitrios G. Kottas, Ryan C. DuToit Ahmed Ahmed, Ruipeng Li and Stergios I. Roumeiotis Mutipe Autonomous Robotic Systems Laboratory, TR February 214 This technica report is structured as foows: In the first section, we introduce the expression for generating the position and orientation of an interpoated coordinate frame. In the second section, we derive the Jacobian matrices for the measurement mode deaing with both the time synchronization and roing shutter probems. The third and fourth sections, present the unobservabe directions of the inearized system, whie its non-inear counterpart is empoyed for proving the uniqueness of those directions. Finay, we provide a strategy for seecting the poses that comprise the estimator s optimization window, for two common motion profies: generic motion, that provides sufficient baseine between consecutive camera poses and hovering scenarios (i.e., static case or pure rotationa motions. A. Transation I. INTERPOLATION MODEL For generating the position of the interpoated frame at time φ, we use the foowing inear interpoation mode. p ψ = (1 λp k + λp k+1 (1 where λ is the interpoation parameter, p k and p k+1 are the camera position in the goba frame at times k and k+1 (with t k t φ t k+1, and p ψ is the camera position when capturing the feature measurement. B. Rotation For the orientation, we introduce the foowing sma ange-based interpoation mode: k+1 C k C I θ (2 ψ k C I λ θ = (1 λi + λ k+1 C k C (3 ψ G C = ψ k Ck C = (1 λ k C + λ k+1 C (4 where i jc denotes the orientation of j in frame i. When there is no superscript or subscript is used, this corresponds to the goba frame G (i.e., k+1 G C k+1 C. The interpoation parameter λ can be defined as the time deay caused by the time synchronization and the roing shutter. λ = t ts +t rs m (5 t n II. MEASUREMENT MODEL AND JACOBIANS The measurement mode can be written as z = h (x + n, where n is the measurement noise, and ] h (x = [ h(x1 h(x 3 h(x 2 h(x 3 (6

4 where h(x = ψ p f = ψ C(p f p ψ (7 ( (p = (1 λ k C + λ k+1 C f (1 λp k λp k+1 The measurement Jacobians can be computed using the chain rue, as where and h (x x = h (x h(x h(x x [ h 1 (x h(x = h(x h(x ] 1 3 (h(x h(x h(x 2 3 (h(x 3 2 (8 H pk (9 T 1 (1 λ k C + λ( k+1 Ĉ t 2 p f (1 λp k λp k+1 H θk =(1 λ k Ct 2 H θk+1 =λ k+1 Ct 2 H k =(λ 1T 1 H k+1 = λt 1 H p f =T 1 H λ =( k+1 C k Ct 2 + T 1 (p k p k+1 (1 where H θk and H θk+1 are the Jacobians of the orientation of the goba in the camera frames at times k and k+1, H k and H k+1 are the Jacobians of the position of the camera in goba frame at times k and k+1, H p f is the Jacobian of the feature position, and H λ is the Jacobian of the interpoation parameter λ. III. OBSERVABILITY ANALYSIS: LINEARIZED SYSTEM In our observabiity anaysis, we assume that the camera and IMU frames coincide so as to simpify notation. In practice, the extrinsic caibration parameters can be determined using the agorithm proposed in [1]. The system s state vector is: [ x k x k+1 ξ ], where x i, i = k,k + 1, is the IMU state: x i = [i q b g v i b a p i ] (11 corresponding, i q, the goba orientation in the IMU frame (in quaternion parametrization, b g, the gyroscope bias, v i, the IMU veocity in the goba frame, b a, the acceerometer bias, and p i, the IMU position in the goba frame respectivey. ξ is defined as: ξ = [ p f λ ] (12 where p f is the feature position in the goba frame, and λ is the interpoation parameter. Note that ony one point feature is necessary. A system s observabiity matrix [2], M, is defined as H 1 H 2 Φ 2,1 M =. H k Φ k,1 where Φ k,1 is the state transition matrix from time step 1 to k, and H k is the measurement Jacobian at time step k. As described in [2], a system s unobservabe directions, N, span the observabiity matrix s right nuspace. (13 MN = (14 TR RepRev 3

5 From now on, we wi use Φ and H with superscripts denote the state transition and measurement Jacobian matrices of the whoe system, whie Φ and H with subscripts denote the state transition and measurement Jacobian matrices corresponding to the IMU state x i, i =k,k+1. According to (1, the measurement Jacobian can be written as: [ ] H k = H pk H k R H k+1 R H p f H λ where for i = k, k + 1, H i R [ H θi H i ] (15 (16 Simiary, the system state transition matrix can be defined as: Φ k, Φ k,1 = Φ k+1, I ( where Φ i, j, (i, j = (k, 1, (k + 1, 2 is the state transition matrix between IMU states x i and x 1. As shown in [3], the structure of Φ i,1 is written as: i j C φ i 12 I Φ i, j = φ31 i φ32 i I φ34 i (18 I φ51 i φ52 i (i jδti φ54 i Then, substituting H k from (1, the bock coumns of the k-th bock row of the system s observabiity matrix, M, is written as: M k 1 = H p k ((1 λ k Ct 2 k C 1 + (λ 1T 1 Φ k 51 M k 2 = H p k ((1 λ k CT 2 Φ k 12 + (λ 1T 1Φ k 52 M k 3 = H p k ((k 1δt(λ 1T 1 M k 4 = H p k ((λ 1T 1 Φ k 54 M k 5 = H p k ((λ 1T 1 M k 6 = H p k (T 1 ( M k 7 = H p k λ k+1 CT 2 k+1 C 1 λt 1 Φ k+1 51 ( M k 8 = H p k λ k+1 CT 2 Φ k+1 12 λt 1Φ k+1 52 M k 9 = H p k ( kδtλt 1 ( M k 1 = H p k λt 1 Φ k+1 54 M k 11 = H p k ( λt 1 M k 12 = H p k (( k+1 C k Ct 2 + T 1 (p k p k+1 (19 In [3], it has been shown that with a time synchronized goba shutter camera, the vision-aided inertia navigation TR RepRev 4

6 system s unobservabe directions are: 1 Cg N goba v 1 g p 1 g I 3 3 p f g I 3 3 In our case, the system receives the same amount of information from the IMU and camera measurements, except there exists a time misaignment between the two sensors. Thus, we expect our system to have at east the same unobservabe directions. Lemma 1: The system with time misaigned IMU and camera measurements has at east the foowing four unobservabe directions: 1 Cg v 1 g p 1 g I 3 3 N 2 Cg = [ ] N g N p (21 v 2 g p 2 g I 3 3 p f g I Proof: First, it can be easiy seen that M k N p = H pk ( T 1 + T 1 =, for any k. Thus, MN p =. To verify N g is aso the system s unobservabe direction, we mutipy it to the system s observabiity matrix as: M k N g =H pk ((1 λ k C T 2 g + (λ 1T 1 p 1 p k (λ 1T 1 p 1 g T 1 p f g + H pk (λ k+1 C T 2 g λt 1 p 2 p k+1 g + λt 1 p 2 g =H pk ((1 λ k C T 2 g + (λ 1T 1 p k g T 1 p f + λ k+1 C T 2 gλt 1 p k+1 g = in which φ51 k is substituted with the foowing expression, as proved in [3]: φ k 51 = p 1 + (k 1δtv ((k 1δt2 g p k 1 C (23 (2 (22 IV. OBSERVABILITY ANALYSIS: NON-LINEAR SYSTEM In the ast section, we have proved N is the system s nuspace. In this section, we wi show that the time offset between the IMU and camera, corresponding to the interpoation ratio, is observabe for the VINS. Thus, there exists no other unobservabe directions other than N. Theorem 1: Consider a noninear, continuous-time system: { ẋ = i= f i (xu i z = h(x if a time deay τ is unobservabe, then where O is the system s observabiity matrix, as defined in [4]. (24 Oẋ = (25 Proof: For a system (24, given the contro inputs u i and the measurements z, if the time deay τ is unobservabe, TR RepRev 5

7 then: By appying Tayor series expansion on (26, we have: h(x(t = h(x(t + τ = h(x(t + τḣ τ2 ḧ τ3... h + ( = h(x(t + z = h(x(t = h(x(t + τ (26 τ L h τ2 i= L 1 f i hu i τ3 where ḣ, ḧ and ḧ are substituted with the foowing expressions: ḣ = h t = h x x t = L hẋ ḧ = ḣ t = ( L hẋ = ( i= L1 f i hu i t t = ( i= L 1 f i hu i ẋ i= j=... h = ḧ t = (( i= L1 f i hu i ẋ = ( i= j= L2 f i f j hu i u j t t = ( L 2 f i f j hu i u j + i= j= ẋ (27 L 2 f i f j hu i u j ẋ (28 Since (27 needs to be satisfied by any set of contro inputs u i, the foowing reation needs to hod: L h L 1 f i h Oẋ =, O = L 2 f i f j h L 3 f i f j f k h. where i, j,k =,...,, and the matrix O, is by definition [4], the observabiity matrix of system (1. (29 Lemma 2: For a Vision-aided Inertia Navigation System, Oẋ Proof: The VINS process mode can be written as: ṡ 2 1 v Db 1 g g C T 2 b a D C T ẋ = ṗ ṗ f = v + ω + a (3 ḃ a ḃ g }{{}}{{}}{{} f f 1 f 2 where 1 2 D θ. According to [5], the system s observabiity matrix can be spit into the product of two matrices: O = ΞB (31 where Ξ is a fu coumn rank matrix, and B is: ξ p f θ 3 C C I Cv θ C B = 3 3 I I I 3 3 Cg θ I I 3 (32 TR RepRev 6

8 where ξ is a 3 3 fu rank matrix. Then: Oẋ = ΞBẋ = ΞB( f + f 1 ω + f 2 a ξ C(p f p θ 3 C C Db g 3 I Cv θ C g C T b a = Ξ 3 3 I I 3 3 v I 3 3 Cg θ I 3 }{{} I 3 A C(p f p b g Cv + C(p f p ω Cv b g + Cg b a + Cv ω + a = A Cg b g + Cg ω 1 2 D C T ω + a Since A has fu coumn rank [5], Oẋ. Based on Theorem 1 and Lemma 2, the time deay τ is observabe. Therefore, there exists no unobservabe direction, that is not spanned by N. V. CLONING DURING HOVERING Given sufficient baseine, between consecutive camera poses, the proposed siding window estimator manages coned poses, using a first in, first out (FIFO scheme. Specificay, before coning the IMU pose corresponding to the new image, into the estimator s siding window, the odest pose is marginaized, such as the ength of the siding window remains constant. Assume that at time step k 1, the siding window comprises N poses, {x k N,...,x k 1 }. After processing feature tracks, firsty observed in image k N, the odest pose x k N is marginaized. At the next time step k, the new pose x k, is added to the estimator s optimization window. In short, the FIFO scheme sides the window of camera poses forward in time, which is the most commony used image management scheme empoyed by siding window fiters. Under sufficient motion, a FIFO coning strategy eads to poses uniformy distributed in space and time, which aows i the trianguation of features, for inearization purposes, and ii the representation of camera poses as an interpoation of consecutive IMU poses. Furthermore, under motion with sufficient baseine, the system has four unobservabe directions, equa in number to the system s theoretica minimum. However, if the sensor patform is hovering (i.e., the camera performs pure rotationa motion, or remains static, an aternative coning strategy is required such that no new unobservabe directions are introduced [6]. A simpe soution, is to use a ast in, first out (LIFO [6] coning strategy, during which a new (hovering IMU pose repaces the newest (hovering cone in the siding window. Such a scheme, maintains non-hovering camera frames, in the estimator s optimization window which provide sufficient baseine for visua observations. Consider a siding window comprising the poses {x k N,...x k 2, x k 1 }. At the arriva of image k, the patform enters a hovering state, and the management of coned poses switches to a LIFO scheme. After M images, the siding window wi comprise the poses {x k N,...x k 2, x k 1+M }. Ceary, the increasing time interva between the newest poses of the window, {x k 2, x k 1+M }, does not aow us to empoy the interpoation mode, for processing measurements originating from image k 1 + M. Instead, we propose an aternative coning strategy (Agorithm V. Specificay, instead of maintaining ony the most recent hovering pose, in the estimator s window, we keep the two most recent hovering poses (i.e., x k 2+M, x k 1+M. Such a strategy, aows us to use this pair of cones, for modeing measuremenets originating from the newest image k 1 + M, whie at the same time non-hovering poses remain in the estimator s window such as baseine is maintained, between the camera frames comprising the estimator s window. REFERENCES [1] F. M. Mirzaei and S. I. Roumeiotis, A kaman fiter-based agorithm for imu-camera caibration: Observabiity anaysis and performance evauation, IEEE Trans. on Robotics, vo. 24, no. 5, pp , Oct. 28. (33 TR RepRev 7

9 Agorithm 1 Proposed cone management at the arriva of image k, for a siding window of size N if Reguar Motion then Drop odest pose, {x k N } Add new pose, {x k } ese if Switched from Reguar Motion to Hovering then Drop the two most recent poses {x k 2, x k 1 } Add new pose, {x k } ese if Remained in Hovering then Drop second most recent pose, {x k 2 } Add new pose, {x k } end if [2] P. S. Maybeck, Stochastic modes, estimation and contro. New York, NY: Academic Press, 1979, vo. 1. [3] J. A. Hesch, D. G. Kottas, S. L. Bowman, and S. I. Roumeiotis, Observabiity-constrained vision-aided inertia navigation, University of Minnesota, Dept. of Comp. Sci. & Eng., MARS Lab, Tech. Rep., Feb [Onine]. Avaiabe: http: //www-users.cs.umn.edu/ dkottas/pdfs/vins tr winter 212.pdf [4] R. Hermann and A. J. Krener, Noninear controabiity and observabiity, IEEE Trans. on Automatic Contro, vo. 22, no. 5, pp , Oct [5] J. A. Hesch, D. G. Kottas, S. L. Bowman, and S. I. Roumeiotis, Camera-imu-based ocaization: Observabiity anaysis and consistency improvement, Internationa Journa of Robotics Research, vo. 33, no. 1, pp , Jan [6] D. G. Kottas, K. Wu, and S. I. Roumeiotis, Detecting and deaing with hovering maneuvers in vision-aided inertia navigation systems, in Proc. of the IEEE/RSJ Internationa Conference on Inteigent Robots and Systems, Tokyo, Japan, Nov , pp TR RepRev 8