LEGO-LOAM（LOAM）部分公式推导---未完待续

最新推荐文章于 2024-06-26 17:48:27 发布

白色小靴

最新推荐文章于 2024-06-26 17:48:27 发布

阅读量1.5k

点赞数 9

分类专栏： slam学习文章标签： LEGO-LOAM 原理推导

本文链接：https://blog.csdn.net/weixin_44684139/article/details/118581383

版权

slam学习专栏收录该内容

63 篇文章 82 订阅

订阅专栏

本文详细推导了Lego-LOAM中的帧间匹配雅可比矩阵，涉及平面运动中关键位姿参数的求导，并解析了高频率里程计位姿累积函数的计算。重点介绍了如何通过链式法则计算残差对位姿量的影响，以及欧拉角到旋转矩阵的转换过程。

摘要由CSDN通过智能技术生成

一、featureAssociation相关推导

1）帧间匹配雅可比矩阵推导

首先明确LEGO-LOAM中，运动坐标系（符合右手系）的设置为：
在这里插入图片描述
因此对于平面运动来说，影响最大的三个分量为 $t_x,t_z,r_y$ ，因此仅考虑这三个分量对雅可比矩阵，也就是对非线性优化问题的贡献。首先让我们考虑整个非线性优化过程，首先将本帧点 $p$ 转换至上一帧坐标系中，得到 $p^{'}$ ，即

$p^{'} = T p$

通过计算 $p^{'}$ 点和其近邻线（以corner为例，存在于上一帧点云系中）的点线间距离，得到残差 $f$ ，即

$f = d i s t (p^{'}, l i n e) = d i s t (T p, l i n e)$

于是雅可比矩阵可以写为

$=\frac{{\partial f}}{{\partial T}} = \frac{{\partial (dist(Tp,line))}}{{\partial T}}$

由于我们只关心三个分量，且每一帧包含多个点线配对，可以计算许多个残差，则雅可比矩阵写开以后是这个类型: $J =$
在这里插入图片描述 $f$ 对这三个位姿量的求导，使用链式法则来计算，首先将残差对使用这个位姿量转换过去的点 $p^{'} = (x, y, z)$ 进行求导（将本帧点转换至上一帧坐标系中的点），然后再使用被转换过去的点 $p^{'} = (x, y, z)$ 对位姿量求导。也就是：

$\frac{{\partial {f_k}}}{{\partial {t_x}}} = \frac{{\partial {f_k}}}{{\partial x}}\frac{{\partial x}}{{\partial {t_x}}} + \frac{{\partial {f_k}}}{{\partial y}}\frac{{\partial y}}{{\partial {t_x}}} + \frac{{\partial {f_k}}}{{\partial z}}\frac{{\partial z}}{{\partial {t_x}}}$

每一项链式法则的求导分为两部分，在程序中分别在findCorrespondingCornerFeatures函数和calculateTransformationCorner函数中完成。之后我们再细说，在这里先推导 $\frac{{\partial {f_k}}}{{\partial x}}$ 和 $\frac{{\partial x}}{{\partial {t_x}}}$ 的具体形式，首先推导前半部分 $\frac{{\partial {f_k}}}{{\partial x}}$ ，根据公式（这里的公式复制自知乎某回答），点线距离为：
在这里插入图片描述
即

其中，

令分母为：

接下来求 $\frac{{\partial {f_k}}}{{\partial x}}$ ，也就是这里的 $\frac{{\partial {d_\varepsilon }}}{{\partial x}}$ 。 $x$ 即为公式里面的 $x_0$ 。

$\frac{{\partial {d_\varepsilon }}}{{\partial {x_0}}}{\rm{ = }}\frac{{\frac{1}{2}(\sqrt {m_{11}^2 + m_{22}^2 + m_{33}^2} )(2{m_{11}}\frac{{\partial {m_{11}}}}{{\partial {x_0}}} + 2{m_{22}}\frac{{\partial {m_{22}}}}{{\partial {x_0}}})}}{{{l_{12}}}}{\rm{ = }}\frac{{{m_{11}}\frac{{\partial {m_{11}}}}{{\partial {x_0}}} + {m_{22}}\frac{{\partial {m_{22}}}}{{\partial {x_0}}}}}{{{l_{12}}\sqrt {m_{11}^2 + m_{22}^2 + m_{33}^2} }}$

对另两个位姿分量的就省略了，是同理的，这一块对应源码中的：

                tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];
                tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];

                float x0 = pointSel.x;
                float y0 = pointSel.y;
                float z0 = pointSel.z;
                float x1 = tripod1.x;
                float y1 = tripod1.y;
                float z1 = tripod1.z;
                float x2 = tripod2.x;
                float y2 = tripod2.y;
                float z2 = tripod2.z;

                float m11 = ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1));
                float m22 = ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1));
                float m33 = ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1));

                float a012 = sqrt(m11 * m11  + m22 * m22 + m33 * m33);

                float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

                float la =  ((y1 - y2)*m11 + (z1 - z2)*m22) / a012 / l12;

                float lb = -((x1 - x2)*m11 - (z1 - z2)*m33) / a012 / l12;

                float lc = -((x1 - x2)*m22 + (y1 - y2)*m33) / a012 / l12;

                float ld2 = a012 / l12;

接下来求链式法则的后面那部分，也就是 $\frac{{\partial x}}{{\partial {t_x}}}$ 。 $x$ 指代的是经过变换矩阵变换，将本帧点转换至上一帧坐标系中的点的坐标。因此，首先需要明确旋转变换关系。LEGO-LOAM(LOAM)中的帧间坐标变换关系如下：
在这里插入图片描述

其中， $p^{'} = (x, y, z)$ 表示被转换到上一帧坐标系中的点， $p=(x_c,y_c,z_c)$ 表示本帧这个点。 $R$ 和 $t_x,t_y,t_z$ 分别代表旋转和平移变换关系，角度为本帧坐标系转了多少欧拉角能转到上一帧坐标系，象征着所有欧拉角加符号带入公式才能获得本帧坐标系点转换到上一帧坐标系中。至于LOAM为什么这样设定不清楚（有点拧巴）。

在这里插入图片描述

要求 $\frac{{\partial x}}{{\partial {t_x}}}$ ，必须要明确R是什么形式。根据wiki上给出的公式（上图右边红色框），欧拉角（-rx,-ry,-rz）转换为旋转矩阵的公式为：

这里之所以加负号，与欧拉角设定相关？

因此易求 $\frac{{\partial x}}{{\partial {t_x}}}$ ，仅看第一行第一列乘出来的东西就行，即：
在这里插入图片描述
化简可以得到

2）高频率里程计位姿累积函数：integrateTransformation

    void integrateTransformation(){
        float rx, ry, rz, tx, ty, tz;
        AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], 
                           -transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);

        float x1 = cos(rz) * (transformCur[3] - imuShiftFromStartX) 
                 - sin(rz) * (transformCur[4] - imuShiftFromStartY);
        float y1 = sin(rz) * (transformCur[3] - imuShiftFromStartX) 
                 + cos(rz) * (transformCur[4] - imuShiftFromStartY);
        float z1 = transformCur[5] - 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);

        PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, 
                          imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);

        transformSum[0] = rx;
        transformSum[1] = ry;
        transformSum[2] = rz;
        transformSum[3] = tx;
        transformSum[4] = ty;
        transformSum[5] = tz;
    }

（1）AccumulateRotation函数：

输入：

上一帧粗配准的激光里程计的累积位姿（全局），仅取旋转部分欧拉角：transformSum[0], transformSum[1], transformSum[2]
上一帧和本帧间的位姿变化量（增量），仅取旋转部分欧拉角： -transformCur[0], -transformCur[1], -transformCur[2]。由于transformCur处于本帧坐标系，取负号表示从上一帧变换到本帧的欧拉角变化量。
本帧粗配准的激光里程计的累积位姿，仅取旋转部分欧拉角，由以上两组量求得：rx, ry, rz

在这里插入图片描述

首先从位姿变换基础公式入手，根据wiki上给出的公式（上图画黄色框处），这里欧拉角（x,y,z）转换为旋转矩阵的公式为：
在这里插入图片描述
设上一帧全局位姿的旋转部分为 $R_1=(x_1, y_1, z_1)$ ，本帧全局位姿的旋转部分为 $R_2=(x_2, y_2, z_2)$ ，上一帧到本帧的旋转变化量 $\delta R=(\delta x, \delta y, \delta z)$ ，于是根据姿态变换公式：

$R_2=R_1·\delta R$

根据上面欧拉角转旋转矩阵的通式，与待求欧拉角相关的 $R_2$ 为：
在这里插入图片描述
观察到，第二行第三列直接与 $x_2$ 相关，因此考虑先求 $x_2$ ，即先求 $R_2^{(2,3)}$ ，也就是 ( $R_1$ 和 $\delta R$ 也按照上面通式转为旋转矩阵) $R_1$ 的第二行乘以 $\delta R$ 的第三列，即：

展开就能用来求 $sinx_2$ ，于是就有了AccumulateRotation函数内部的：

float srx = cos(lx)*cos(cx)*sin(ly)*sin(cz) - cos(cx)*cos(cz)*sin(lx) - cos(lx)*cos(ly)*sin(cx);
ox = -asin(srx);

接下来求 $y_2$ ，可以看到：

$arctan(R_2^{(1,3)} / R_2^{(2,3)})=y_2$ ，同理，按照上述几行几列的思路写出公式，可得函数内部的：

 float srycrx = 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(lx)*cos(ly)*cos(cx)*sin(cy);
 float crycrx = cos(lx)*cos(ly)*cos(cx)*cos(cy) - 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));
 oy = atan2(srycrx / cos(ox), crycrx / cos(ox));

接下来求 $z_2$ ，可以看到：
$arctan(R_2^{(2,1)} / R_2^{(2,2)})=z_2$ ，同理，按照上述几行几列的思路写出公式，可得函数内部的：

float srzcrx = 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(lx)*cos(cx)*cos(cz)*sin(lz);
float crzcrx = cos(lx)*cos(lz)*cos(cx)*cos(cz) - 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));
oz = atan2(srzcrx / cos(ox), crzcrx / cos(ox));

接下来观察旋转部分，这里我们忽略IMU的使用，即imuShiftFromStartZ = imuShiftFromStartY = 0 （LEGO-LOAM的IMU听说也很鸡肋）。就有如下代码：

        float x1 = cos(rz) * (transformCur[3]) 
                 - sin(rz) * (transformCur[4]);
        float y1 = sin(rz) * (transformCur[3]) 
                 + cos(rz) * (transformCur[4]);
        float z1 = transformCur[5];

        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);