# IMU Preintegration on Manifold for Ecient.pdf

28

imu预积分，Forster, Christian, et al.Supplementary material to: IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation. Georgia Institute of Technology, 2015.
From(A3),(A 4), and(A.5 ), it follows that. that(A 2) can be written in iterative form 6dk+1=△Rk+10k+Jm△t Dvk+1=0ik-△Rk(ak-b)^b:△t+△R1kmx4△t 6pk+1=pk+6v△t-△Bhk(ak-b2)6k△ (A.6) for k=i, ...,, with initial conditions oi dvii=op Eq(A6)can be conveniently written in matrix form 0中 k:k+1 6中k Jk△t0 △R;k( ^△tI 0 +0 △R;△t (A.7) oPik+1 1△B;k(ak bg)^△+2I3x×3△tI3x3」Lp;k 03x3△Rk△t2 or morc simply: mik-1= Anak+ Bnk (A.8) where From the linear model(.s)and given the covariance 2 ∈R6×6 of the raw IMU measurements noise n it's now possible to compute the covariance iteratively ∑h+1=A∑+1A+B∑,B starting from initial conditionsΣ=09×9 Note that thc fact that thc covariance can be computcd itcrativcly is convcnicnt, computationally, as it means that we can easily update the covariance after integrating a new measurement, computationally, as it The same iterative computation is possible for the preintegrated measurements themselves △Rk+1=△ Rikexp(心k-b9)△) △vk+1=△vk+△k(ak-b2)△t △ik1=△p+△vM△t+△RA(ah-b2)△ which easily follows froIn Eys.(28)-(29)-(30) of the Nail docunent 1. 2 Bias Correction via First-Order Updates In this section we provide a complete derivation of the first-order bias correction proposed in Section V-C of 4. Let us start by recalling the expression of the preintegrated measurements ARi;, Avii, Apii, given in Eqs.(28)-(29)-(30)of the Main document △R-1Exp(ak=b)△ △v=∑△E;k(ak-b)△ △R(ak-b:)△t (A.11) Assume now that we have computed the preintegrated variables at a givell bias estimate b:=b, bl, ar nid let us denote the corresponding preintegrated measurements as ARii(bi), Avii (bi), Apii(bi). In this section we want to devise an cxpression to“ update”△R;/(b),△v;(b,),△p/(b) when our bias estimate changes Consider the case in which we get a new estimate bi<bi+obi, where obi is d snull correction w.r.t. the previous estimate bi. Using the new bias estimate b; in(A11) we get the updated preintegrated measurements △Rb)-IDP(-b9)2A)-ILp(k-b1-6b)△ k=s △v(b)=∑△Ak(b)(ak-b2)4=∑△Rx(,)(ak-b6-0b)△t ∑2△Rn(a-b:)△2=∑ 33(b1)(akb:-6b2)△t 12) 3 A naive solution would be to recompute the preintegrated measurements at the new bias est imate as pre scribed in(A 12). In this section, instead, we show how to update the preintegrated measurements wit. hout repeating the integration Let us start from the preintegrated rotation measurement ARii (bi). We assumed that the bias correction is sImall, hence we use the first-order approximation(7)for each tern in the product D )=∏[ Xp 心-b)△tE 6b9△ where we defined J.=JOk-b )(J, is the right Jacobian for SO(3) given in Eq (8)of the paper). W rearrange the terms in the product, by "moving"the terms including ob to the end, using the relation (11/ △E7(b)=△E;(b)I[EXp(-△R1(b)6b9At) (A.13) 二7 where we used the fact that ARi; (bi)=llk= Exp((Wk-b )At) by definition Repeated application of the first-order approximation(9)(recall that &b is small, hence the right Jacobians arc close to the identity) produces △()=△⑤E(∑△+的△0?)=△两(1Ep(0 (A.14) which corresponds to eq(36)in 4. The Jacobian alagi can be precomputed during preintegration. This can bc donc in analogy with Scction 1. 1 sincc thc structurc of OAR: j is essentially thc samc as the onc multiplying tho noise in(A.2 ). Using(A 14)we can update the previous preintegrated measurement ARij(bi)to get ARii(bi) Let us now focus on the preintegrated velocity Avii(bi). We substitute ARii (bi) back into(A 12) ∑ △E;(bEx/Q△Bb)(ak-b-8b2)At (A.15) h=? Recalling that the correction Sb, is sImall, we use the first-order approximation(4) △wnb)=2250(×.AR6b2))(ak-b2-b)△t Developing the previous expression and dropping higher-order terms △()∑△-6)△+∑△()(a)△+∑△1(△B1a0)^(a-b)△ K-n (A.17) recalling that△vi;(b)-∑=△1;(b)(ak-b:)△ t and using property(2 △vn}=△v(b)+∑-△B/()△b"-∑-△Bn/⑤)(a-b O△R; abs 4l Sb9 k=i △vz;(bz)+ 0△vi8b;+b9 O△v2 obs aba which corresponds to the second expression in Eq. 36)of the paper 4 Finally, repeating the same derivation for Apii (bi) p(b)=∑7△R(b1)(ak=b-0b2)△ (A.14) Rig(b: )Exp( 0△R 6b;)△ ∑2△(,(T+ △R abg (ak-b-6b2)△ △B1;b1)0b△2+∑2△R(b 0△ 2)△ △p(b)+∑-7△B/(b2b2+∑ O△Rz O △p;(b2.ba)+ mbb0△8b 0△p (A.19) which corresponds to the last expression in Eq- 36) To suInnarize, the Jacobialls used for a-posteriori bias update are(c/. eqs.(A 14)-(A.18-(A19)) O△R 0b9 ∑[△R4+1(b)小△ 0△v Ob ∑△B;/(b)△t 0△v ab k=(ak-1aAO△R△t △R Ob △pi ∑△R:()△t 0△p 3 0b9 △R2(bNb)O△R k=i Repeating the same derivation of Section 1.1, it is possible to show that(A 20) can be computed incrementally as new measurements arrive 2 IMU Factors: Residual errors and jacobians In this scction we providc analytic expressions for the Jacobian matrices of thc rcsidual crrors introduced in Section V-D of [4]. We start from the expression of the residual errors for the preintegrated IMU measurements 0△ △R;(b)Ex R; R 0△ r△v=R(v;v2g△ty)-△v/(b2b)+ 176b9 0△v8b db9 rAP÷R(p;-p-v~1 g△t2)-△p;(b9,b)+ 0△p △p; aba (A.21) Lifting"the cost function consists in substituting the following retraction P:∈p+Rp2,R;← Ri Exp(6中;) Pj←p;+R;5 ←B,Dxp(6中), (A.22) while, since velocity and biases already live in a vector space the corresponding retraction reduces to vvz+6v,v;←v;+6v;b←b8+b4;b←b2+6ba (A.23) The process of lifting makes the residual errors a function defined on a. vector space, on which it is easy to compute Jacobians. We derive the Jacobians w.r.t. the vectors dp dpi, dvi,dd, opj, dvi, abaa, dai in the folle g sections 5 2.1 acobians of r△py △ r△P/D2+R6p)=R(p,-p-Bp-V2△t-29442 )-|△py+P23+ 0△pij db (A.24) r△p;(P)+(-I3×1)6 r△P(P+R)=R!(P+R6p-D,v2△ 0△p 0△p ab 6b+ ab (A.25) =r△p(P)+(RR3)6p (v;+6v)=E(p;-p;-v△t-6v△t 0△ △ △ abg △Pb ab' (A.26) r△p(v)+(-R△t;)5v △t2 O△ r△p2(R,Ixp(6中)=(BExP(中)(p;-P;-v△ g △p;+ △pa 60:)B(p一p2-yN25 △px+ 8△pi6b 8△p18b ab △p(R3)+ △t:;-g△ i (we used ah (A.27) In summary, the jacobians of rAp are 中, g△2) Or△p 3×1 06v R!△t △p R:R △0A △p 6db。 ab 4 己△p 22 Jacobians of r△v 0△v 0△ △v(v+6v)=R(vv1-6v2-g4ty)-△vx+ (v)-R2 0△ 6△ r△v2(v+6v)=P(v;+6v g△t;) Sb:+ Obg A.29 (v)+B8 rAvi (Ri Exp(Sp))=(Ri Exp(opi))(v, )△ △v g△t b:+ ob ab (-6分)R(v一v-gAt1)-△+m6、→Nb Eq.(4) 0△ (A.30) r△√(R)+(R(v;-v;-g△t)6d;( we used a b 6 In summary, the jacobians of rA are (R}(v-v1-g△t) R 0 06中 0△v 6 ab 2.3 Jacobians oL1△h O△R (R, Exp(opi))=Log(ARi,(b) (RExp(6;)) △R L △B;/(b;)EXpD Sb Exp(-SPr;R 9 1(1) 0△R Los △R;(b2)Exp 2bg dbs R R;Exp(-R Ridi O△Rz T g((△E;(b)Fxp R. R 0b9 0△ 0g Rii(b)Exp R Ri(R RiSpi r△R(R1)-J1(r△R(R,)B: r△R,(R;Exp(6o,)=Iog 0△Rh2)n(,Fxp(6中) 0b9 r△R(E)+JF(r△(R,)60 r△+b)=Leg(25Esp O△R2 上Q.(7) O△R 0△R R;/(b) 0△ R.R 0△RiA\O△B 0△R Log Exp-j, db9 abs 6bz)(△B3(b)Exp R.R 0b92 b26b9)△R a△R; Log Exp ExP(r△(6b2) Eu(11) 0△R Log(Dxp(△m,(b?)Exp(-Dxp(r△,(b2) △R ob ob 9 ab g "r△,(0b)-J1(r△,、Gb)Exp(r△B.(b) O△R O△R abg (A.33) 7 In summary, the jacobians of rAR are (r△B(E)RB b61 (r△R(B3) r△,(6b)ExP(△(b)3(226b) 3 Vision factors: Schur Complement and Null Space Projection Let us start from the linearized vision fact Eg(44 of the document ∑∑FT;+E;!0P-b =1i() where STi=[&i pil e ko is a perturbation w.r.t. the linearization point of the pose at keyframe i and Spu linearization point. For brevity, we do not enter in the details of the Jacobians Fil E R2x6 Eil E R2x at the is a perturbation w r t. the linerization point of landmark l. The vector bil E R is the residual erre Now, as done in the main document, we denote with oTx( e rn the vector stacking the perturbations STi for each of the n cameras observing landmark l. With this notation(A34)can be written in matrix ∑‖16Tx(+E16m-b|2 where 0 0 ∈R2ntx6m,E E ∈R 2nt×3 b ∈R2. 0 02×6 02×6 (4.36) Since a landmark l appears inl a single terIn of the suIn(A35), for any given choice of the pose perturbatiOn STx(, the landmark perturbation Op! that minimizes the quadratic cost F1 oTx()+ El dp1-bill 6n=-(EE)-E(F10Tx(0)-b) Substituting (A 37) back into(A 35) we can eliminate the landmarks from the optimization problem L ∑‖-E(LE)E)(F261x(o)-by) (A.38) -1 which corresponds to eq(46)of the main document. The structureless factors(A.38) only involve poses and allow to perform the optimization disregarding landmark positions Computation can be further improved by the following linear algebra considerations. First, we note that MnpaaIrdtr-1el)E R2n: x2m is an orthogonal projector of E, Roughly speaking, Q projects any vector a to the null space of the matrix EL. Moreover, any basis EE R2n2X2n1-3 of the null space of E! satisfies the following relation 5 EL((EI)E+)(EN)=I-ELEIED-EL L.Substituting(A. 9)into(A. 38), and recalling that E is a unitary matrix, we obtain.' satisfies A basis for the null space can be easily computed from El using SVD. Such basis is unit ary, i.e ∑‖(E)(P26x(-b)2-∑((E)T(F:1x-b)(()(F26里:(-b) >(FLSTx(-bi) Et(E+TE(ET)(FLOTx(-bl >(FLSTx(0-bi)EL(E (FISTx()-bi=>(E!T(FLSTx()-bi) (A.40) which is an alternative representation of our vision factors(A. 38), and is usually preferable from a computationa standpoint. A similar null space projection is used in [6 within a Kalman filter architecture, while a factor graph view on Schur complement is given in 2 4 Angular Velocity and Right Jacobians for SO(3) The right Jacobian matrix J,(o)(also called body Jacobian 7) relates rates of change in the parameter vector o to the instantaneous body angular velocity BWwE BawB-Jr(中)中 (A.41) A closcd-form cxprcssion of the right Jacobian is givcn in 3 J()=I cS(|小)|中-smn(中)个2 I ol (A.42) Note that the acobian becomes the identity matrix for =0 Consider a direct cosine matrix rwb( So(3), that rotates a point from body coordinates B to world coordinates W, and that is parametrized by the rotatiOn vector b. The relation between angular velocity anld the derivative of a rotation matrix is 7 WB (A.43) Hence, using(A. 41) we can write the derivative of a rotation matrix at RwB(中)-Bw(d)(J()d) (A.44) Given a multiplicative perturbation Exp(S) on thc right hand sidc of an clcmcnt of thc group SO(3), wo may ask what is the equivalent additive perturbation in the tangent space o e so 3) that results in the same compound rotation Exp(φ)Exp(6v)=Exp(+0￠) (A.45) Computing the derivative with respect to the increments on both sides, using(A.44), and assuming that the increments are sinall. we finld v≈J(中)6中 leading to g Exp(φ+6)≈Exp(φ)ExpJ(φ)6d) (A.47) A similar first-order approximation holds for the log Log(EXp()EXD(6))≈φ+J(d)6 This property follows directly frol the Baker -Carnpbell-Hausdor/(BCH) formula under the assuMptioN that So is small [1]. An explicit expression for the inverse of the right Jacobian is given in 3 Jr()=l +co(|) + 小|22|小sin(||) eferences 1T. D. Barfoot and P. T Furgale. Associating uncertainty with three-dimensional poses for use in estinlatioll problems. IEEE Trans. Robotics, 30( 3): 679-693, 2014 2 L Carlone, P. F. Alcantarilla, H P. Chiu, K. Zsolt, and F. Dellaert. Mining structure fragments for smart bundle adjustment. In British Machine Vision Conf.(BMVC), 2014 Modern Applications(Applied and Numerical Harmonic Amalysis ). Birkhauser, 2012 Analytic Methods and 3 G. s. Chirikjian. Stochastic Models, Information Theory, and Lie groups, Volume 2 4 C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza. iMu preintegration on manifold for efficient visual- inertial maximum-a-posteriori estimation. In Robotics: Science and Systems(RSS), 2015 5 C. Meyer. Matri Analysis and Applied Linear Algebra. SIAM, 2000 6A.I. Mourikis alld S I. Rouneliotis. A multi-state constraint Kalal filter for visioIl-aided inertial navigation In IEEE Intl. Conf. on Robotics and Automation(ICRA), pages 3565-3572, April 2007 7 R. M. Murray, Z Li, and S Sastry. A Mullernatical InTroduction Lo Robotic Manipulation. CRC Press, 1994 10

...展开详情

• 技术圈认证

IMU Preintegration on Manifold for Ecient.pdf 10积分/C币 立即下载
1/10

10积分/C币 立即下载