3. Joint optimization

3.3 IMU preintegration

这篇论文中采用了IMU-LiDAR紧耦合的方法，其实主要参考了VINS的理论和方法，核心思想其实就是IMU预积分。应该说IMU预积分也不是什么很新的东西了，关于IMU预积分的博客也有很多。但笔者刚开始接触这个东西的时候觉得，很多博客只是照着VINS中的推导简单描述了一下，但是笔者刚开始看VINS论文的时候就觉得里面的理论推导省略的地方有点多，总有种隔靴搔让的感觉。为了更加透彻的理解IMU预积分方法，笔者参考VINS的论文重新自己推导了一遍。希望推导过程能尽可能详实，同时便于理解。水平所限，如有纰漏，还请各位小伙伴帮忙指正。

3.3.1 IMU integration

一般一个IMU包括三个正交的陀螺仪以及三个正交的加速度计，三个陀螺仪可以分别测量绕自身x,y,zx,y,zx,y,z轴的角速度wx,wy,wzw_x,w_y,w_zwx,wy,wz，而三个加速度计则可以分别测量沿着三轴的加速度ax,ay,aza_x,a_y,a_zax,ay,az，其测量模型一般可以建立为：
w~=w+ηw+bwa~=RT(a−g)+ηa+ba\tilde w=w+\eta_{w}+b_{w}\\ \tilde a=R^T(a-g)+\eta_{a}+b_{a} w~=w+ηw+bwa~=RT(a−g)+ηa+ba
根据牛顿运动定律，我们根据简单的运动学关系来推算当前的位姿，接下来我们详细推导一下这个运动学关系。
涉及到IMU的SLAM算法，一般会把系统状态建模为：RWB,pw,vwR_{WB},p_w,v_wRWB,pw,vw，分别对应IMU本体坐标系到世界坐标系的姿态、IMU在世界坐标系下的位置和速度。关于位置和速度的运动学关系，比较容易得到：
p˙w=vw,v˙w=aw\dot{p}_w=v_w,\ \dot{v}_w=a_w p˙w=vw, v˙w=aw
但是关于旋转矩阵的求导怎么表示呢？我刚开始看IMU预积分的时候，觉得角速度都知道了，求姿态那不是很简单吗，先根据角速度积分成欧拉角，再将欧拉角转换成旋转矩阵，然后再不断地乘啊乘，不就得到了最后的旋转矩阵吗？好像还是这么回事。事实上第一个做IMU预积分的还真是这样做的。但是这样做的话，会有一些问题，比如说欧拉角出现死锁啊之类的。而且看VINS的时候发现好像有些细节不太一样。那接下来我们就好好说说这个姿态的运动学。

如上图所示，假设现在有两个坐标系，一个是用实线表示的本体坐标系BBB，另一个是用虚线表示的世界坐标系WWW，两个坐标系之间只存在旋转不存在位移，且本体坐标系到世界坐标系的旋转矩阵为RWBR_{WB}RWB。在本体坐标系中存在一个点ppp，且点ppp在本体坐标系中的位置pBp_BpB是不变的，则点p在世界坐标系下的坐标就是：
pW=RWBpBp_{W}=R_{WB}p_{B} pW=RWBpB
现在假设本体坐标系一直在按照某种方式旋转，也即到世界坐标系下变换矩阵RRR一直在变R(t)R(t)R(t)，则可以得到：
pW=R(t)pBp_{W}=R(t)p_{B} pW=R(t)pB
现在如果我们想求点ppp在世界坐标系下的速度，则可以利用牛顿运动定律得到如下关系：
p˙W=ddtpW=dR(t)dtpB=R˙pB\dot{p}_W=\frac{d}{dt}p_W=\frac{dR(t)}{dt}p_B=\dot{R}p_B p˙W=dtdpW=dtdR(t)pB=R˙pB
终于把这个旋转矩阵的导数引入进来了，那么这个旋转矩阵的导数该怎么求呢？我们可以从上式几何意义的角度来考虑，上式计算的其实是点ppp在世界坐标系下的线速度对吧？我们之前高中以及大学学的理论力学中好像学过这么一个公式：
v⃗=w⃗×r⃗\vec{v}=\vec{w}\times\vec{r} v=w×r
也就是说，线速度等于角速度向量与半径向量的叉乘，根据这个想法我们可以重新计算一下点ppp在世界坐标系下的速度。首先，我们根据IMU测量出来的角速度w⃗\vec{w}w以及点ppp在本体坐标系下的位置pB⃗\vec{p_B}pB，可以求出来点p在本体坐标系下的速度向量v⃗\vec{v}v，也即：
vpB=w⃗×pB⃗v_p^B=\vec{w}\times\vec{p_B} vpB=w×pB
为了求该点在世界坐标系下的速度，我们只需要将速度vpBv_p^BvpB投影到世界坐标系下，即：
vpW=RvPB=Rw⃗×pB⃗=R˙pBv_p^W=Rv_P^B=R\vec{w}\times\vec{p_B}=\dot{R}p_B vpW=RvPB=Rw×pB=R˙pB
这样就得到了旋转矩阵导数的表达式：
R˙=Rw⃗∧\dot{R}=R\vec{w}\wedge R˙=Rw∧
Good Job!！

接下来，就要真正的进入到IMU积分阶段了，首先我们利用刚刚推导的公式重新写一下IMU的运动学关系。
R˙=Rwb⃗∧,p˙w=vw,v˙w=aw\dot{R}=R\vec{w_b}\wedge,\ \dot{p}_w=v_w,\ \dot{v}_w=a_w R˙=Rwb∧, p˙w=vw, v˙w=aw
容易发现，咦好像就是以前大学学过的微分方程啊，那来求一下吧，盘它。首先是旋转矩阵，
R˙=dRdt=Rw⃗b∧\dot{R}=\frac{dR}{dt}=R\vec{w}_b\wedge R˙=dtdR=Rwb∧
学过微分方程的对这种公式应该很熟悉，不就是把dtdtdt挪到等式的右边，然后把RRR挪到等式的左边，即：
1RdR=wb⃗∧dt\frac{1}{R}dR=\vec{w_b}\wedge dt R1dR=wb∧dt
等式两边同时对积分：
∫tt+Δt1R(t)dR(t)=∫tt+Δtwb⃗∧dt\int_t^{t+\Delta t}\frac{1}{R(t)}dR(t)=\int_{t}^{t+\Delta t}\vec{w_b}\wedge dt ∫tt+ΔtR(t)1dR(t)=∫tt+Δtwb∧dt
哪个函数的导数是1x\frac{1}{x}x1来着，应该是ln⁡x\ln xlnx吧，因此可以进一步化简得到：
ln⁡R(t)∣tt+Δt=∫tt+Δtwb⃗∧dt\ln R(t)|_{t}^{t+\Delta t}=\int_{t}^{t+\Delta t}\vec{w_b}\wedge dt lnR(t)∣tt+Δt=∫tt+Δtwb∧dt
即：
ln⁡R(t+Δt)=ln⁡R(t)+∫tt+Δtwb⃗∧dt=ln{R(t)Exp(∫tt+Δtwb⃗∧dt)}\ln R(t+\Delta t)=\ln R(t)+\int_{t}^{t+\Delta t}\vec{w_b}\wedge dt=ln\{R(t)Exp(\int_{t}^{t+\Delta t}\vec{w_b}\wedge dt)\} lnR(t+Δt)=lnR(t)+∫tt+Δtwb∧dt=ln{R(t)Exp(∫tt+Δtwb∧dt)}
也就能得到：
R(t+Δt)=R(t)Exp(∫tt+Δtwb⃗∧dt)R(t+\Delta t)=R(t)Exp(\int_{t}^{t+\Delta t}\vec{w_b}\wedge dt) R(t+Δt)=R(t)Exp(∫tt+Δtwb∧dt)
咦居然有一个指数函数，还记得上一篇博客中我们说的切空间so(3)so ( 3)so(3)到SO(3)SO ( 3)SO(3)群的指数映射吗？忘记了的，可以再去回顾回顾哦。总之，这个公式的意思呢就是，首先要根据利用角速度(陀螺仪)积分得到旋转角，然后利用叉乘运算符得到该旋转角对应的切空间内的反对称阵，最后利用指数映射得到对应的SO(3)SO ( 3 )SO(3)群旋转矩阵，然后再去乘前一时刻的旋转矩阵不就是当前时刻的旋转矩阵吗？现在有没有觉得切空间、SO(3)SO( 3 )SO(3)群、指数映射这些听起来玄乎乎的东西也没那么难理解了。Beautiful！！

有了旋转矩阵的积分推导，剩余的关于位置和速度的推导就简单很多了。
速度：
dvwdt=advw=adt∫tt+Δtdvw=∫tt+Δtadtv∣tt+Δt=∫tt+Δtadtv(t+Δt)=v(t)+∫tt+Δtadt\begin{matrix} \frac{dv_w}{dt}=a\\ dv_w=adt\\ \int_{t}^{t+\Delta t}dv_w=\int_{t}^{t+\Delta t}adt\\ v|_{t}^{t+\Delta t}=\int_{t}^{t+\Delta t}adt\\ v(t+\Delta t)=v(t)+\int_{t}^{t+\Delta t}adt \end{matrix} dtdvw=advw=adt∫tt+Δtdvw=∫tt+Δtadtv∣tt+Δt=∫tt+Δtadtv(t+Δt)=v(t)+∫tt+Δtadt
位置：
dpdt=vdp=vdt∫tt+Δtdp=∫tt+Δtv(τ)dτp(t+Δt)=p(t)+∫tt+Δt(v(t)+∫tτa(T)dT)dτp(t+Δt)=p(t)+v(t)Δt+∫tt+Δt∫tτa(T)dT)dτ\frac{dp}{dt}=v\\ dp=vdt\\ \int_{t}^{t+\Delta t}dp=\int_{t}^{t+\Delta t}v(\tau)d\tau\\ p(t+\Delta t)=p(t)+\int_{t}^{t+\Delta t}(v(t)+\int_{t}^{\tau}a(T)dT)d\tau\\ p(t+\Delta t)=p(t)+v(t)\Delta t+\int_{t}^{t+\Delta t}\int_{t}^{\tau}a(T)dT)d\tau dtdp=vdp=vdt∫tt+Δtdp=∫tt+Δtv(τ)dτp(t+Δt)=p(t)+∫tt+Δt(v(t)+∫tτa(T)dT)dτp(t+Δt)=p(t)+v(t)Δt+∫tt+Δt∫tτa(T)dT)dτ

3.3.2 IMU preintegration

把上面三个东西写到一起就是：
R(t+Δt)=R(t)Exp(∫tt+Δtwb⃗∧dt)v(t+Δt)=v(t)+∫tt+Δtadtp(t+Δt)=p(t)+v(t)Δt+∫tt+Δt∫tτa(T)dT)dτ\begin{matrix} R(t+\Delta t)=R(t)Exp(\int_{t}^{t+\Delta t}\vec{w_b}\wedge dt)\\ v(t+\Delta t)=v(t)+\int_{t}^{t+\Delta t}adt\\ p(t+\Delta t)=p(t)+v(t)\Delta t+\int_{t}^{t+\Delta t}\int_{t}^{\tau}a(T)dT)d\tau \end{matrix} R(t+Δt)=R(t)Exp(∫tt+Δtwb∧dt)v(t+Δt)=v(t)+∫tt+Δtadtp(t+Δt)=p(t)+v(t)Δt+∫tt+Δt∫tτa(T)dT)dτ
但是还有一个问题啊，上式都是定义在连续时间内的。而我们的传感器采样的数据都是离散的，因此呢需要把这东西离散掉，可是这个角速度和加速度都是时变的，还不知道是怎么个变法，这怎么玩？这个时候啊就需要来假设了，一般我们假设加速度和角速度在一小段时间内是不会发生变换的，则上式就可以进一步简化为：
R(t+Δt)=R(t)Exp(wΔt∧)v(t+Δt)=v(t)+aΔtp(t+Δt)=p(t)+v(t)Δt+12a(t)Δt2R(t+\Delta t)=R(t)Exp(w\Delta t\wedge)\\ v(t+\Delta t)=v(t)+a\Delta t\\ p(t+\Delta t)=p(t)+v(t)\Delta t+\frac{1}{2}a(t)\Delta t^2 R(t+Δt)=R(t)Exp(wΔt∧)v(t+Δt)=v(t)+aΔtp(t+Δt)=p(t)+v(t)Δt+21a(t)Δt2
接下来将IMU的测量模型带入上式可得到：
R(t+Δt)=R(t)Exp((w~−ηw−bw)Δt∧)v(t+Δt)=v(t)+(R(a~−ηa−ba)+g)Δt=v(t)+gΔt+R(t)(a~−ηa−ba)Δtp(t+Δt)=p(t)+v(t)Δt+12gΔt2+12R(t)(a~−ηa−ba)Δt2R(t+\Delta t)=R(t)Exp((\tilde w-\eta_w-b_w)\Delta t\wedge)\\ v(t+\Delta t)=v(t)+(R(\tilde a-\eta_a-b_a)+g)\Delta t=v(t)+g\Delta t+R(t)(\tilde a-\eta_a-b_a)\Delta t\\ p(t+\Delta t)=p(t)+v(t)\Delta t+\frac{1}{2}g\Delta t^2+\frac{1}{2}R(t)(\tilde a-\eta_a-b_a)\Delta t^2 R(t+Δt)=R(t)Exp((w~−ηw−bw)Δt∧)v(t+Δt)=v(t)+(R(a~−ηa−ba)+g)Δt=v(t)+gΔt+R(t)(a~−ηa−ba)Δtp(t+Δt)=p(t)+v(t)Δt+21gΔt2+21R(t)(a~−ηa−ba)Δt2
现在给定两个关键帧iii和jjj，则可以根据上式，利用IMU在这段时间内的测量量，计算两帧的相对运动，即：
Rj=Ri∏k=ij−1Exp((wk~−ηwk−bwk)Δt∧)vj=vi+gΔtij+∑k=ij−1(Rk(ak~−ηak−bak)Δt)pj=pi+∑k=ij−1(vkΔt+12gΔt2+12Rk(a~k−ηak−bak)Δt2)R_j=R_i\prod_{k=i}^{j-1}Exp((\tilde{w_k}-\eta_{w}^k-b_w^k)\Delta t\wedge)\\ v_j=v_i+g\Delta t_{ij}+\sum_{k=i}^{j-1}(R_k(\tilde {a_k}-\eta_a^k-b_a^k)\Delta t)\\ p_j=p_i+\sum_{k=i}^{j-1}(v_k\Delta t+\frac{1}{2}g\Delta t^2+\frac{1}{2}R_k(\tilde a_k-\eta_a^k-b_a^k)\Delta t^2) Rj=Rik=i∏j−1Exp((wk~−ηwk−bwk)Δt∧)vj=vi+gΔtij+k=i∑j−1(Rk(ak~−ηak−bak)Δt)pj=pi+k=i∑j−1(vkΔt+21gΔt2+21Rk(a~k−ηak−bak)Δt2)
那么现在我们可以利用这个公式推算两个关键帧之间的相对运动了，但是一般在图slam中优化过程中，需要不断的优化图中每一个节点的状态(位置，姿态等)。这一优化不要紧，利用IMU预测运动时，jjj时刻的状态是依赖于iii时刻的状态的，也就是说只要有其中一个节点的状态发生了变化，后面节点的状态都需要重新积分计算，这计算量谁顶得住啊，而且重复计算一个东西真的cpu也感觉很无聊的啊！
聪敏的预积分作者开始想啊，如果能从这纷繁复杂的公式中抽出一部分不变量，然后先把这部分不变量计算出来存储下来，不就可以避免重复计算了吗？不得不佩服这个作者，实在是太机智了。那么怎么才能抽取出来不变量呢？我们首先来看第一个旋转矩阵的公式，这个看起来好像比较简单，等式右边的连乘项好像和iii时刻系统的状态(位置、姿态、速度)无关啊，于是等式两边同乘RiR_iRi的逆矩阵，就可以得到：
RiTRj=∏k=ij−1Exp((wk~−ηwk−bwk)Δt∧)=ΔRijR_i^TR_j=\prod_{k=i}^{j-1}Exp((\tilde{w_k}-\eta_{w}^k-b_w^k)\Delta t\wedge)=\Delta R_{ij} RiTRj=k=i∏j−1Exp((wk~−ηwk−bwk)Δt∧)=ΔRij
现在来看第二个，第二个看起来就不那么友善了，等式右边不仅存在viv_ivi还存在旋转矩阵RkR_kRk，但是等式右边的连和里面，除了这个旋转矩阵，好像还都与系统的状态无关，因此我们先把viv_ivi和重力加速度这两项挪到等式的左边，就可以得到:
vj−vi−gΔtij=∑k=ij−1(Rk(ak~−ηak−bak)Δt)v_j-v_i-g\Delta t_{ij}=\sum_{k=i}^{j-1}(R_k(\tilde {a_k}-\eta_a^k-b_a^k)\Delta t) vj−vi−gΔtij=k=i∑j−1(Rk(ak~−ηak−bak)Δt)
那么现在怎么把这个旋转矩阵RkR_kRk挪掉呢？还记得之前的ΔRij\Delta R_{ij}ΔRij吗，那这里把RkR_kRk换成ΔRik\Delta R_{ik}ΔRik不就行了吗？于是等式两边同时乘以RiR_iRi的逆矩阵，就得到：
RiT(vj−vi−gΔtij)=∑k=ij−1(ΔRik(ak~−ηak−bak)Δt)=ΔvijR_i^T(v_j-v_i-g\Delta t_{ij})=\sum_{k=i}^{j-1}(\Delta R_{ik}(\tilde {a_k}-\eta_a^k-b_a^k)\Delta t)=\Delta v_{ij} RiT(vj−vi−gΔtij)=k=i∑j−1(ΔRik(ak~−ηak−bak)Δt)=Δvij
现在最后一个好像更复杂，我们一点一点来看。熟悉了之前的套路，这肯定得把等式右边的pip_ipi和12gΔt2\frac{1}{2}g\Delta t^221gΔt2挪到等式左边，也即：
pj−pi−12∑k=ij−1gΔt2=∑k=ij−1(vkΔt+12Rk(a~k−ηak−bak)Δt2)p_j-p_i-\frac{1}{2}\sum_{k=i}^{j-1}g\Delta t^2=\sum_{k=i}^{j-1}(v_k\Delta t+\frac{1}{2}R_k(\tilde a_k-\eta_a^k-b_a^k)\Delta t^2) pj−pi−21k=i∑j−1gΔt2=k=i∑j−1(vkΔt+21Rk(a~k−ηak−bak)Δt2)
这个RkR_kRk比较好处理，这个vkv_kvk怎么办嘞？也是类似的啊，之前不是求了个什么Δvij\Delta v_{ij}Δvij吗，现在可以利用起来啊。
(vk−vi−gΔtik)=RiΔvikvk=vi+gΔtik+RiΔvik(v_k-v_i-g\Delta t_{ik})=R_i\Delta v_{ik}\\ v_k=v_i+g\Delta t_{ik}+R_i\Delta v_{ik} (vk−vi−gΔtik)=RiΔvikvk=vi+gΔtik+RiΔvik
再带进去看一下：
pj−pi−12∑k=ij−1gΔt2=∑k=ij−1((vi+gΔtik+RiΔvik)Δt+12Rk(a~k−ηak−bak)Δt2)p_j-p_i-\frac{1}{2}\sum_{k=i}^{j-1}g\Delta t^2=\sum_{k=i}^{j-1}((v_i+g\Delta t_{ik}+R_i\Delta v_{ik})\Delta t+\frac{1}{2}R_k(\tilde a_k-\eta_a^k-b_a^k)\Delta t^2) pj−pi−21k=i∑j−1gΔt2=k=i∑j−1((vi+gΔtik+RiΔvik)Δt+21Rk(a~k−ηak−bak)Δt2)
再化简一下：
pj−pi−12∑k=ij−1gΔt2−viΔtij−∑k=ij−1gΔtΔtik=∑k=ij−1(RiΔvikΔt+12Rk(a~k−ηak−bak)Δt2)p_j-p_i-\frac{1}{2}\sum_{k=i}^{j-1}g\Delta t^2-v_i\Delta t_{ij}-\sum_{k=i}^{j-1}g\Delta t\Delta t_{ik}=\sum_{k=i}^{j-1}(R_i\Delta v_{ik}\Delta t+\frac{1}{2}R_k(\tilde a_k-\eta_a^k-b_a^k)\Delta t^2) pj−pi−21k=i∑j−1gΔt2−viΔtij−k=i∑j−1gΔtΔtik=k=i∑j−1(RiΔvikΔt+21Rk(a~k−ηak−bak)Δt2)
等式两边再乘以RiR_iRi的逆矩阵，就可以得到：
RiT(pj−pi−12∑k=ij−1gΔt2−viΔtij−∑k=ij−1gΔtΔtik)=∑k=ij−1(ΔvikΔt+12ΔRik(a~k−ηak−bak)Δt2)=ΔpijRiT(pj−pi−12gΔtij2−viΔtij)=∑k=ij−1(ΔvikΔt+12ΔRik(a~k−ηak−bak)Δt2)=ΔpijR_i^T(p_j-p_i-\frac{1}{2}\sum_{k=i}^{j-1}g\Delta t^2-v_i\Delta t_{ij}-\sum_{k=i}^{j-1}g\Delta t\Delta t_{ik})=\sum_{k=i}^{j-1}(\Delta v_{ik}\Delta t+\frac{1}{2}\Delta R_{ik}(\tilde a_k-\eta_a^k-b_a^k)\Delta t^2)=\Delta p_{ij}\\ R_i^T(p_j-p_i-\frac{1}{2}g\Delta t_{ij}^2-v_i\Delta t_{ij})=\sum_{k=i}^{j-1}(\Delta v_{ik}\Delta t+\frac{1}{2}\Delta R_{ik}(\tilde a_k-\eta_a^k-b_a^k)\Delta t^2)=\Delta p_{ij} RiT(pj−pi−21k=i∑j−1gΔt2−viΔtij−k=i∑j−1gΔtΔtik)=k=i∑j−1(ΔvikΔt+21ΔRik(a~k−ηak−bak)Δt2)=ΔpijRiT(pj−pi−21gΔtij2−viΔtij)=k=i∑j−1(ΔvikΔt+21ΔRik(a~k−ηak−bak)Δt2)=Δpij
Oh my god!!先喘口气，因为到这还每结束。
篇幅过长，下篇博客见。

Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（四）相关推荐

Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（一）
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(一) 1. LiDAR inertial odometry and mapping简介 ...
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（二）
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(二) 3. Joint optimization 3.1 Marginalization ...
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（三）
Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析(三) 3. Joint optimization 3.2 IMU preintegrat ...
激光SLAM系统Fast LOAM (Lidar Odometry And Mapping)源码解析
github地址:Fast LOAM (Lidar Odometry And Mapping) Fast LOAM提供了mapping和localization的两个节点,目前只使用其定位部分,以ve ...
loraserver 源码解析 (四) lora-gateway-bridge
lora-gateway-bridge 负责接收 gateway 通过 udp 发送的 packet-forwarder 数据然后通过 MQTT broker 将报文转发给 LoRa Server ...
（LI论文）LIO-Mapping：Tightly Coupled 3D Lidar Inertial Odometry and Mapping
摘要自我运动估计是大多数移动机器人应用的基本要求.通过传感器融合,我们可以弥补独立传感器的不足,并提供更可靠的估计.本文介绍了一种紧耦合的雷达-惯性测量单元融合方法.通过联合最小化激光雷达和惯性测量 ...
Tomcat源码解析四：Tomcat关闭过程
我们在Tomcat启动过程(Tomcat源代码阅读系列之三)一文中已经知道Tomcat启动以后,会启动6条线程,他们分别如下: [java] view plaincopy "ajp-bio- ...
arcengine遍历属性表_Redis源码解析四--跳跃表
Redis 跳跃表(skiplist) 1. 跳跃表(skiplist)介绍定义:跳跃表是一个有序链表,其中每个节点包含不定数量的链接,节点中的第i个链接构成的单向链表跳过含有少于i个链接的节点. ...
kafka-go源码解析四（Writer）
概要 kafka-go区分同步写与异步写.同步写能严格确保写入的顺序,因为在写成功之前它会block住应用程序,同时返回错误信息.有三种控制写入完成的时机,1是消息发送完成即返回,2是leader收到 ...

Tightly Coupled LiDAR Inertial Odometry and Mapping源码解析（四）