LOAM中的坐标变化公式推导

符号约定

• 点云中的点$p_i^c$：获取一激光点时，该点在获取时刻的局部坐标系中的坐标，$i$代表点的序号，$c$代表坐标系。0.1s内获取到的点 { p 0 c 0 , p 1 c 1 , ⋯   , p n c n } \{ p_0^{c_0}, p_1^{c_1}, \cdots ,p_n^{c_n}\} {p0c0​​,p1c1​​,⋯,pncn​​}，组成了一帧点云。

• 位姿用$T_a^b$表示，含义为$p^b=T_a^b{p}^a$，具体地，

$$\begin{gathered} T_a^b=\left[\begin{array}{cc}{C}_a^b& r_a^b\\ 0& 1\end{array}\right] \\ {T_a^b}^{-1}=\left[\begin{array}{cc}{C_a^b}^{-1}& -{C_a^b}^{-1}{r}_a^b\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{C}_b^a& -C_b^a{r}_a^b\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{C}_b^a& r_b^a\\ 0& 1\end{array}\right]=T_b^a\text{\hspace{1em}(1)} \end{gathered}$$

$r_a^b$表示a系原点在b系中的坐标，$r_b^a$表示b系原点在a系中的坐标。

• 关于坐标系符号，有以下约定，

$w$：世界坐标系；

$c$：获取激光点时的激光坐标系；

$s$：每帧点云的起始时刻坐标系；

$e$：每帧点云的终止时刻坐标系；

需要注意的是，loam中假设了imu和激光雷达的外参为$\mathbf{I}$，也就是说，imu局部坐标系与激光局部坐标系是重合的，可以不区分imu系和激光系。

---

对于一帧点云，我们可以用$T_c^s$将点$p_i^c$转换到该帧点云的$s$系，具体地，
$$p_i^s=T_c^s{p}_i^c={T_s^w}^{-1}{T}_c^w{p}_i^c$$
其中，
$$T_s^w=\left[\begin{array}{cc}{C}_s^w& r_s^w\\ 0& 1\end{array}\right],{T_s^w}^{-1}=\left[\begin{array}{cc}{C}_w^s& -C_w^s{r}_s^w\\ 0& 1\end{array}\right],T_c^w=\left[\begin{array}{cc}{C}_c^w& r_c^w\\ 0& 1\end{array}\right]$$
有，
$${T_s^w}^{-1}{T}_c^w=\left[\begin{array}{cc}{C}_w^s{C}_c^w& C_w^s(r_c^w-r_s^w)\\ 0& 1\end{array}\right]$$
imu从$s$系到$c$系的平移运动，可以分成两部分，即匀速部分和非匀速部分，设匀速部分的位移为$r_{unif|c}^w$，非匀速部分的位移为$r_{shift|c}^w$，有$r_c^w-r_s^w=r_{unif|c}^w+r_{shift|c}^w$，带入上式，有，
$${T_s^w}^{-1}{T}_c^w=\left[\begin{array}{cc}{C}_w^s{C}_c^w& C_w^s{r}_{unif|c}^w+C_w^s{r}_{shift|c}^w\\ 0& 1\end{array}\right]$$
如果我们可以得到上方公式中各个变量的值，那么就可以直接将点云中的畸变点转移到同一坐标系$s$系中。但是实际中，我们不能直接得到$r_{unif|c}^w$的值，我们将$r_{unif|c}^w$剔除，得到以下公式，
$$T_c^s={T_s^w}^{-1}{T}_c^w=\left[\begin{array}{cc}{C}_w^s{C}_c^w& C_w^s{r}_{shift|c}^w\\ 0& 1\end{array}\right]\text{\hspace{1em}(2)}$$
我们可以用$T_c^s$将点云转换到$s$系，但此时的点云仍然是有畸变的，该畸变只由匀速运动造成，注意，可以认为这里没有旋转的畸变，只有平移的畸变。我们用$T_c^s{p}_i^c$得到只有匀速运动畸变的$s$系点云，这一步发生在代码的TransformToStartIMU函数中。

对于匀速运动的畸变，我们可以设他的位姿矩阵为$T_s^{e^{\prime }}$，具体如下，
$$\begin{gathered} T_s^{e^{\prime }}=\left[\begin{array}{cc}{C}_s^{e^{\prime }}& r_s^{e^{\prime }}\\ 0& 1\end{array}\right] \\ {C}_s^{e^{\prime }}=C_{roll}{C}_{pitch}{C}_{yaw}=C_z{C}_x{C}_y \end{gathered}$$
其中$C_s^{e^{\prime }}$为$e^{\prime }$系按$roll$（$z$轴）->$pitch$（$x$轴）->$yaw$（$y$轴）得到与$s$系“平行”的坐标系，$r_s^{e^{\prime }}$为$s$系原点在$e^{\prime }$系中的坐标。其中$e^{\prime }$代表从$s$开始，匀速运动的终点，前面提到的$roll,pitch,yaw$以及$r_s^{e^{\prime }}$就是odom中需要求解的位姿。根据$e^{\prime }$的意义，我们有，
$$T_{e^{\prime }}^w=\left[\begin{array}{cc}{C}_s^w& r_s^w+r_{unif|e}^w\\ 0& 1\end{array}\right]$$
旋转矩阵使用$C_s^w$是因为经过$T_c^s{p}_i^c$的变换，只有平移上的位姿变换，没有旋转。

将点云转换到$\mathbf{e}$系

利用$T_c^s{p}_i^c$，可以将原始激光点云，转换到$s$系，且只有匀速运动畸变，设转换后的点为$p_i^{c^{\prime }}$，这也是后面提取特征点以及odom、mapping使用的局部点云。

对于点$p_i^{c^{\prime }}$，我们用线性插值的方式将其转换到$s$系中，如下，
$$p_i^s={(\alpha T_s^{e^{\prime }})}^{-1}{p}_i^{c^{\prime }}=\left[\begin{array}{cc}{(\alpha C_s^{e^{\prime }})}^{-1}& -{(\alpha C_s^{e^{\prime }})}^{-1}{r}_s^{e^{\prime }}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{p}_i^{c^{\prime }}\\ 1\end{array}\right]\text{\hspace{1em}(3)}$$
其中$\alpha$是线性插值因子，上方公式中的转换发生在TransformToStart中。

对于去掉运动畸变的$s$系中的点$p_i^s$，转换到$e$系的公式如下，
$$\begin{gathered} p_i^e=T_{e^{\prime }}^e{T}_s^{e^{\prime }}{p}_i^s \\ {T}_{e^{\prime }}^e={T_e^w}^{-1}{T}_{e^{\prime }}^w=\left[\begin{array}{cc}{C_e^w}^{-1}& -{C_e^w}^{-1}{r}_e^w\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{C}_s^w& r_s^w+r_{unif|e}^w\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{C_e^w}^{-1}{C}_s^w& {C_e^w}^{-1}(r_s^w+r_{unif|e}^w-r_e^w)\\ 0& 1\end{array}\right] \end{gathered}$$

因为$r_e^w-r_s^w=r_{unif|e}^w+r_{shift|e}^w$，且$r_{shift|e}^w=C_s^w{r}_{shift|e}^s$，因此$T_{e^{\prime }}^e$如下，
$$T_{e^{\prime }}^e=\left[\begin{array}{cc}{C_e^w}^{-1}{C}_s^w& -{C_e^w}^{-1}{C}_s^w{r}_{shift|e}^s\\ 0& 1\end{array}\right]$$
综上，将点$p_i^{c^{\prime }}$转到$e$系的各个位姿矩阵都已经清楚了，直接带入即可。过程为：
$$p_i^{c^{\prime }}\to p_i^s\to p_i^{e^{\prime }}\to p_i^e$$
这一过程在TransformToEnd函数中。

对于odom中全局位姿（transformSum）的计算

设前一帧的全局位姿为$T_{sum|i}^w$，由于最后我们将无畸变的点云转换到了$e$系，且前一帧的$e$系就是当前帧的$s$系，所以当前帧的全局位姿为，
$$\begin{array}{rl}{T}_{sum|i+1}^w& =T_{sum|i}^w{T}_e^s=T_{sum|i}^w{T_s^{e^{\prime }}}^{-1}{T_{e^{\prime }}^e}^{-1}\\ & =\left[\begin{array}{cc}{C}_{sum|i}^w& r_{sum|i}^w\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{C_s^{e^{\prime }}}^{-1}& -{C_s^{e^{\prime }}}^{-1}{r}_s^{e^{\prime }}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{C_s^w}^{-1}{C}_e^w& r_{shift|e}^s\\ 0& 1\end{array}\right]\\ & =\left[\begin{array}{cc}{C}_{sum|i}^w{C_s^{e^{\prime }}}^{-1}& -C_{sum|i}^w{C_s^{e^{\prime }}}^{-1}{r}_s^{e^{\prime }}+r_{sum|i}^w\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{C_s^w}^{-1}{C}_e^w& r_{shift|e}^s\\ 0& 1\end{array}\right]\\ & =\left[\begin{array}{cc}{C}_{sum|i}^w{C_s^{e^{\prime }}}^{-1}{C_s^w}^{-1}{C}_e^w& -C_{sum|i}^w{C_s^{e^{\prime }}}^{-1}(r_s^{e^{\prime }}-r_{shift|e}^s)+r_{sum|i}^w\\ 0& 1\end{array}\right]\end{array}\text{\hspace{1em}(4)}$$
上方公式中的$C_{sum|i}^w{C_s^{e^{\prime }}}^{-1}$，在AccumulateRotation函数中计算，$C_{sum|i}^w{C_s^{e^{\prime }}}^{-1}{C_s^w}^{-1}{C}_e^w$在PluginIMURotation函数中计算。$-C_{sum|i}^w{C_s^{e^{\prime }}}^{-1}(r_s^{e^{\prime }}-r_{shift|e}^s)+r_{sum|i}^w$在代码中的计算如下，

float x1 = cos(rz) * (transform[3] - imuShiftFromStartX) 
    - sin(rz) * (transform[4] - imuShiftFromStartY);
float y1 = sin(rz) * (transform[3] - imuShiftFromStartX) 
    + cos(rz) * (transform[4] - imuShiftFromStartY);
float z1 = transform[5] * 1.05 - imuShiftFromStartZ;

float x2 = x1;
float y2 = cos(rx) * y1 - sin(rx) * z1;
float z2 = sin(rx) * y1 + cos(rx) * z1;
//全局位姿的平移量
tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
ty = transformSum[4] - y2;
tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);

---

AccumulateRotation的公式推导

由于从imu得到的欧拉角$yaw,pitch,roll$的约定为，世界系$w$为$x$轴向前，$y$轴向左，$z$轴向上的坐标系，沿$z$轴按右手系转$yaw$角，按旋转后的坐标系沿$y$轴按右手系转$pitch$角，接着按旋转后的坐标系沿$x$轴按右手系转$roll$角，得到imu局部坐标系$b$。

由于代码中采用的是$x$轴向左，$y$轴向上，$z$轴向前的坐标系，我们仍按之前的$yaw$，$pitch$，$roll$的旋转，也就是，世界系$w$先沿$y$轴按右手系转$yaw$（或记为$ry$）角，按旋转后的坐标系沿$x$轴按右手系转$pitch$（或记为$rx$）角，接着按旋转后的坐标系沿$z$轴按右手系转$roll$（或记为$rz$）角，旋转矩阵$C_I^w$如下，
$$\begin{gathered} C_I^w=R_{ry}{R}_{rx}{R}_{rz} \\ {R}_{ry}=\left[\begin{array}{ccc}\cos ry& 0& \sin ry\\ 0& 1& 0\\ -\sin ry& 0& \cos ry\end{array}\right] \\ {R}_{rx}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos rx& -\sin rx\\ 0& \sin rx& \cos rx\end{array}\right] \\ {R}_{rz}=\left[\begin{array}{ccc}\cos rz& -\sin rz& 0\\ \sin rz& \cos rz& 0\\ 0& 0& 1\end{array}\right] \end{gathered}$$
需要注意，使用上方公式时，需要先将获取的原始点转换到$x$轴向左，$y$轴向上，$z$轴向前的局部坐标系。

由于$C_{sum|i}^w{C_s^{e^{\prime }}}^{-1}=C_{sum|i}^w{C}_{e^{\prime }}^s$，AccumulateRotation中计算的是$C_{sum|i}^w{C}_{e^{\prime }}^s$，设$C_{sum|i}^w$为$w$系按$cy$，$cx$，$cz$转到$sum|i$系，$C_{e^{\prime }}^s$为$s$系按$ly$，$lx$，$lz$（这里的量是$C_s^{e^{\prime }}$中按三轴旋转量的负值，因为从$e^{\prime }$系到$s$系为按$roll$（$z$轴）->$pitch$（$x$轴）->$yaw$（$y$轴）的旋转，所以从$s$系到$e^{\prime }$系就是按$-yaw$（$y$轴）->$-pitch$（$x$轴）->$-roll$（$z$轴））转到$e^{\prime }$系。我们现在想得到$C_{e^{\prime }}^w=C_{sum|i}^w{C}_{e^{\prime }}^s$的$yaw$，$pitch$，$roll$（$oy$，$ox$，$oz$），即
$$R_{oy}{R}_{ox}{R}_{oz}=R_{cy}{R}_{cx}{R}_{cz}{R}_{ly}{R}_{lx}{R}_{lz}$$
AccumulateRotation函数的输入为transformSum的$yaw,pitch,roll$，以及$C_{e^{\prime }}^s$的$yaw,pitch,roll$（也就是$C_s^{e^{\prime }}$的$yaw,pitch,roll$的负值），所以函数的关于transfrom的输入是带负号的。$R_{oy}{R}_{ox}{R}_{oz}$， $R_{cy}{R}_{cx}{R}_{cz}{R}_{ly}{R}_{lx}{R}_{lz}$的计算使用matlab的符号表达式计算，m代码如下，

syms cx cy cz lx ly lz ox oy oz fout fin1 fin2 fin;
%C_{sum|i}^{w}
Rcx = [1,0,0;
       0,cos(cx),-sin(cx);
       0,sin(cx),cos(cx)];
Rcy = [cos(cy),0,sin(cy);
       0,1,0;
       -sin(cy),0,cos(cy)];
Rcz = [cos(cz),-sin(cz),0;
       sin(cz),cos(cz),0;
       0,0,1];
%C_{e'}^{s}
Rlx = [1,0,0;
       0,cos(lx),-sin(lx);
       0,sin(lx),cos(lx)];
Rly = [cos(ly),0,sin(ly);
       0,1,0;
       -sin(ly),0,cos(ly)];
Rlz = [cos(lz),-sin(lz),0;
       sin(lz),cos(lz),0;
       0,0,1];
%C_{e'}^{w}
Rox = [1,0,0;
       0,cos(ox),-sin(ox);
       0,sin(ox),cos(ox)];
Roy = [cos(oy),0,sin(oy);
       0,1,0;
       -sin(oy),0,cos(oy)];
Roz = [cos(oz),-sin(oz),0;
       sin(oz),cos(oz),0;
       0,0,1];
%R_out   
fout = Roy*Rox*Roz

fin1 = Rcy*Rcx*Rcz
fin2 = Rly*Rlx*Rlz
%R_in
fin = fin1*fin2

为简化符号，记$R_{oy}{R}_{ox}{R}_{oz}$为$R_{out}$，$R_{cy}{R}_{cx}{R}_{cz}{R}_{ly}{R}_{lx}{R}_{lz}$记为$R_{in}$，矩阵中的行列序号从1开始，$R_{out}(1,1)$为第一行第一列，$R_{out}$的计算结果如下，
$$R_{out}=\left[\begin{array}{ccc}\cos (oy)\cos (oz)+\sin (ox)\sin (oy)\sin (oz)& \cos (oz)\sin (ox)\sin (oy)-\cos (oy)\sin (oz)& \cos (ox)\sin (oy)\\ \cos (ox)\sin (oz)& \cos (ox)\cos (oz)& -\sin (ox)\\ \cos (oy)\sin (ox)\sin (oz)-\cos (oz)\sin (oy)& \sin (oy)\sin (oz)+\cos (oy)\cos (oz)\sin (ox)& \cos (ox)\cos (oy)\end{array}\right]$$

用$R_{in}(2,3)=R_{out}(2,3)$来计算$ox$，用下式计算$oy$，
$$\tan oy=\frac{\frac{R_{in}(1,3)}{\cos ox}}{\frac{R_{in}(3,3)}{\cos ox}}=\frac{\frac{R_{out}(1,3)}{\cos ox}}{\frac{R_{out}(3,3)}{\cos ox}}$$
$R_{out}(1,3)$的matlab计算结果如下，

sin(lx)*(cos(cy)*sin(cz) - cos(cz)*sin(cx)*sin(cy)) + cos(lx)*sin(ly)*(cos(cy)*cos(cz) + sin(cx)*sin(cy)*sin(cz)) + cos(cx)*cos(lx)*cos(ly)*sin(cy)

$R_{out}(3,3)$的matlab计算结果如下，

cos(cx)*cos(cy)*cos(lx)*cos(ly) - cos(lx)*sin(ly)*(cos(cz)*sin(cy) - cos(cy)*sin(cx)*sin(cz)) - sin(lx)*(sin(cy)*sin(cz) + cos(cy)*cos(cz)*sin(cx))

可以看到，该计算结果与代码中是一样的。

$oz$的计算如下，
$$\tan oz=\frac{\frac{R_{in}(2,1)}{\cos ox}}{\frac{R_{in}(2,2)}{\cos ox}}=\frac{\frac{R_{out}(2,1)}{\cos ox}}{\frac{R_{out}(2,2)}{\cos ox}}$$
$R_{out}(2,1)$的matlab计算结果如下，

sin(cx)*(cos(lz)*sin(ly) - cos(ly)*sin(lx)*sin(lz)) + cos(cx)*sin(cz)*(cos(ly)*cos(lz) + sin(lx)*sin(ly)*sin(lz)) + cos(cx)*cos(cz)*cos(lx)*sin(lz)

$R_{out}(2,2)$的matlab计算结果如下，

cos(cx)*cos(cz)*cos(lx)*cos(lz) - cos(cx)*sin(cz)*(cos(ly)*sin(lz) - cos(lz)*sin(lx)*sin(ly)) - sin(cx)*(sin(ly)*sin(lz) + cos(ly)*cos(lz)*sin(lx))

可以看到，该计算结果与代码中是一样的。

PluginIMURotation的公式推导

$C_{sum|i}^w{C_s^{e^{\prime }}}^{-1}{C_s^w}^{-1}{C}_e^w$在PluginIMURotation函数中计算，前面已经计算了$C_{sum|i}^w{C_s^{e^{\prime }}}^{-1}$的欧拉角，为与代码中的符号一致，这里记$C_{sum|i}^w{C_s^{e^{\prime }}}^{-1}$的欧拉角为$bcx,bcy,bcz$，记$C_s^w$的欧拉角为$blx,bly,blz$，记$C_e^w$的欧拉角为$alx,aly,alz$。我们在PluginIMURotation函数中计算得到的$C_{sum|i+1}^w$的欧拉角，记为$acx,acy,acz$，计算方法与AccumulateRotation类似，都是找恒等关系，这里的恒等关系如下，
$$\begin{gathered} C_{sum|i+1}^w=C_{e^{\prime }}^w{C_s^w}^{-1}{C}_e^w \\ {R}_{acy}{R}_{acx}{R}_{acz}=R_{bcy}{R}_{bcx}{R}_{bcz}{R_{blz}}^{-1}{R_{blx}}^{-1}{R_{bly}}^{-1}{R}_{aly}{R}_{alx}{R}_{alz} \end{gathered}$$
这里同样记等号左边为$R_{out}$，等号右边为$R_{in}$，matlab代码如下，

syms bcx bcy bcz blx bly blz alx aly alz acx acy acz fout fin1 fin2 fin3 fin;
%C_{e'}^{w}
Rbcx = [1,0,0;
       0,cos(bcx),-sin(bcx);
       0,sin(bcx),cos(bcx)];
Rbcy = [cos(bcy),0,sin(bcy);
       0,1,0;
       -sin(bcy),0,cos(bcy)];
Rbcz = [cos(bcz),-sin(bcz),0;
       sin(bcz),cos(bcz),0;
       0,0,1];
%C_{s}^{w}
Rblx = [1,0,0;
       0,cos(blx),-sin(blx);
       0,sin(blx),cos(blx)];
Rbly = [cos(bly),0,sin(bly);
       0,1,0;
       -sin(bly),0,cos(bly)];
Rblz = [cos(blz),-sin(blz),0;
       sin(blz),cos(blz),0;
       0,0,1];
%C_{e}^{w}
Ralx = [1,0,0;
       0,cos(alx),-sin(alx);
       0,sin(alx),cos(alx)];
Raly = [cos(aly),0,sin(aly);
       0,1,0;
       -sin(aly),0,cos(aly)];
Ralz = [cos(alz),-sin(alz),0;
       sin(alz),cos(alz),0;
       0,0,1];
%C_{sum|i+1}^{w}
Racx = [1,0,0;
       0,cos(acx),-sin(acx);
       0,sin(acx),cos(acx)];
Racy = [cos(acy),0,sin(acy);
       0,1,0;
       -sin(acy),0,cos(acy)];
Racz = [cos(acz),-sin(acz),0;
       sin(acz),cos(acz),0;
       0,0,1];
%R_out  
fout = Racy*Racx*Racz

fin1 = Rbcy*Rbcx*Rbcz
fin2 = (Rblz.')*(Rblx.')*(Rbly.')
fin3 = Raly*Ralx*Ralz
%R_in
fin = fin1*fin2*fin3
%展开表达式，便于与代码对比
%求x
expand(fin(2,3))
%求y
expand(fin(1,3))
expand(fin(3,3))
%求z
expand(fin(2,1))
expand(fin(2,2))

用$R_{in}(2,3)=R_{out}(2,3)$来计算$acx$，用下式计算$acy$，
$$\tan acy=\frac{\frac{R_{in}(1,3)}{\cos acx}}{\frac{R_{in}(3,3)}{\cos acx}}=\frac{\frac{R_{out}(1,3)}{\cos acx}}{\frac{R_{out}(3,3)}{\cos acx}}$$

$acz$的计算如下，
$$\tan acz=\frac{\frac{R_{in}(2,1)}{\cos acx}}{\frac{R_{in}(2,2)}{\cos acx}}=\frac{\frac{R_{out}(2,1)}{\cos acx}}{\frac{R_{out}(2,2)}{\cos acx}}$$

经过对比，matlab的结果与代码相同。

---

mapping中transformAssociateToMap的推导

对于第$i$帧点云，我们有上一帧优化后的位姿$T_{aft|i-1}^w$，上一帧的odom结果$T_{sum|i-1}^w$，当前帧的odom结果$T_{sum|i}^w$，我们想得到当前帧的优化初值$T_{tobe|i}^w$，有$T_{tobe|i}^w=T_{aft|i-1}^w{T_{sum|i-1}^w}^{-1}{T}_{sum|i}^w$，具体如下，
$$\begin{array}{rl}\left[\begin{array}{cc}{C}_{tobe|i}^w& r_{tobe|i}^w\\ 0& 1\end{array}\right]& =\left[\begin{array}{cc}{C}_{aft|i-1}^w& r_{aft|i-1}^w\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{C_{sum|i-1}^w}^{-1}& -{C_{sum|i-1}^w}^{-1}{r}_{sum|i-1}^w\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{C}_{sum|i}^w& r_{sum|i}^w\\ 0& 1\end{array}\right]\\ & =\left[\begin{array}{cc}{C}_{aft|i-1}^w& r_{aft|i-1}^w\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{C_{sum|i-1}^w}^{-1}{C}_{sum|i}^w& -{C_{sum|i-1}^w}^{-1}(r_{sum|i-1}^w-r_{sum|i}^w)\\ 0& 1\end{array}\right]\\ & =\left[\begin{array}{cc}{C}_{aft|i-1}^w{C_{sum|i-1}^w}^{-1}{C}_{sum|i}^w& -C_{aft|i-1}^w{C_{sum|i-1}^w}^{-1}(r_{sum|i-1}^w-r_{sum|i}^w)+r_{aft|i-1}\\ 0& 1\end{array}\right]\\ & =\left[\begin{array}{cc}{C}_{aft|i-1}^w{C_{sum|i-1}^w}^{-1}{C}_{sum|i}^w& -C_{aft|i-1}^w{C_{sum|i-1}^w}^{-1}{C}_{sum|i}^w{C_{sum|i}^w}^{-1}(r_{sum|i-1}^w-r_{sum|i}^w)+r_{aft|i-1}\\ 0& 1\end{array}\right]\end{array}$$
${C_{sum|i}^w}^{-1}(r_{sum|i-1}^w-r_{sum|i}^w)$就是transformAssociateToMap中的transformIncre，$C_{aft|i-1}^w{C_{sum|i-1}^w}^{-1}{C}_{sum|i}^w$的计算与PluginIMURotation中的计算公式一样。

至此，关于imu的推导结束。

附录

对于$C_X^w$，其中$X$为局部坐标系（如$e,s,sum|i$等），表示从$w$系沿$yaw$->$pitch$->$roll$旋转得到$X$系，也就是$C_X^w=R_{yaw}{R}_{pitch}{R}_{roll}$。对于上面提到的$s$系以及$e^{\prime }$系，我们可以认为是$s$系沿$yaw$->$pitch$->$roll$旋转得到$e^{\prime }$系，但是odom中优化的是$T_s^{e^{\prime }}$，为了使约定保持一致，所以规定从$e^{\prime }$系沿$roll$->$pitch$->$yaw$旋转得到$s$系，这也解释了TransformToEnd中$C_s^{e^{\prime }}{p}^s$公式为什么是$R_{roll/rz}{R}_{pitch/rx}{R}_{yaw/ry}{p}^s$，对应TransformToEnd下面这段代码，

//绕y轴旋转（ry）
float x4 = cos(ry) * x3 + sin(ry) * z3;
float y4 = y3;
float z4 = -sin(ry) * x3 + cos(ry) * z3;

//绕x轴旋转（rx）
float x5 = x4;
float y5 = cos(rx) * y4 - sin(rx) * z4;
float z5 = sin(rx) * y4 + cos(rx) * z4;

//绕z轴旋转（rz），再平移
float x6 = cos(rz) * x5 - sin(rz) * y5 + tx;
float y6 = sin(rz) * x5 + cos(rz) * y5 + ty;
float z6 = z5 + tz;