LeGO-LOAM 激光里程计计算roll,pitch,tz增量详解
LeGO-LOAM 激光里程计计算\(t_z, \theta_{x},\theta_{y}\)
我们用点\(\boldsymbol{p_i'}\)、\(\boldsymbol{p_i}\)分别代表原始点云、变换后（车子运动引起）点云中的点，目标是求得位姿变换\(\boldsymbol{\xi}=\theta_x,\theta_y,\theta_z,t_x,t_y,t_z\)使得
\[\mathbf{p} _i=\mathbf{R}(\boldsymbol{\xi}) \mathbf{p}_i' + \mathbf{t}(\boldsymbol{\xi}) \tag{1}
\]
LeGO-LOAM 创新之处在于将这6个变量的优化分成了两组\(t_z, \theta_{x}, \theta_{y}\)和\(t_x, t_y,\theta_{z}\)分别优化，既减少了计算量也避免了它们之间的相互干扰。
其中对于\(t_z, \theta_{x}, \theta_{y}\)采用的是点到平面的残差进行优化，这是本文讨论的内容。至于为什么用地面来优化求解这3个变量，可以想象一下，在地面上移动的时候，\(t_x, t_y, \theta_{z}\)都不会让车离开地面也就不会引入残差。
原始代码见原始点到平面残差优化代码，下面我逐次介绍这段代码在最小化什么、数学模型、A/B/X 的含义、以及退化处理。
一、最小化目标函数
对每一个平面特征点\(\mathbf{p} _i\)还原它为\(\mathbf{p} _i'\)，它应该尽可能在平面上：
\[r_i(\boldsymbol{\xi}) = \mathbf{n}_i^\top \mathbf{p}_i' + d_i \tag{2}
\]
把公式(1)代入公式(2)得到
\[r_i(\boldsymbol{\xi}) = \mathbf{n}_i^\top
\mathbf{R}^\top (\boldsymbol{\xi})(\mathbf{p}_i - \mathbf{t}(\boldsymbol{\xi})) + d_i \tag{3}
\]
其中：
-
\(\mathbf{p}_i\)：当前帧中的点（
pointOri） -
\(\mathbf{n}_i = (n_x, n_y, n_z)\)：\(\mathbf{p}_i'\)所在平面的法向量（
coeff.x/y/z） -
\(d_i\)：平面偏移量（
coeff.intensity） -
\(\boldsymbol{\xi}\)：待优化的位姿增量
这里确定平面的方法是：对于当前平面点\(\boldsymbol{p_i}\)，直接拿它的坐标在上一帧点云找到最近点\(\boldsymbol{p_j'}\)，然后找到\(\boldsymbol{p_j'}\)所在的雷达线束上的一个最近点\(\boldsymbol{p_l'}\)，最后找到相邻线束上的一个最近点\(\boldsymbol{p_m'}\)，根据3点\(\boldsymbol{p_j'},\boldsymbol{p_l'},\boldsymbol{p_m'}\)就能确定一个平面。
目标是最小化残差平方和：
\[\min_{\boldsymbol{\xi}}
\sum_i \left( r_i(\boldsymbol{\xi}) \right)^2\]
二、高斯牛顿法计算\(\boldsymbol{\xi}\)
根据高斯-牛顿的方法，我们对残差进行 一阶泰勒展开（Gauss–Newton）：
\[r_i(\boldsymbol{\xi} + \Delta \boldsymbol{\xi})
\approx
r_i(\boldsymbol{\xi})
+
\mathbf{J}_i \Delta \boldsymbol{\xi}\]
每次更新的最小化步\(\Delta \boldsymbol{\xi}\)来自最小化：
\[\min_{\Delta \boldsymbol{\xi}}
\sum_i \left(
\mathbf{J}_i \Delta \boldsymbol{\xi} + r_i
\right)^2\]
写成矩阵形式：
\[\min_{\Delta \boldsymbol{\xi}} \|\mathbf{J}\Delta \boldsymbol{\xi} + \boldsymbol{r}\|^2
\]
对上式求导，取梯度为0，有如下线性方程组：
\[\mathbf{J}^\top\mathbf{J}\Delta \boldsymbol{\xi}=-\mathbf{J}^\top\boldsymbol{r}
\]
对应代码里的求解部分：
matA.at<float>(i, 0) = arx;
matA.at<float>(i, 1) = arz;
matA.at<float>(i, 2) = aty;
matB.at<float>(i, 0) = -0.05 * d2;
}
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
代码里的A就是残差对于各变量求导的雅可比矩阵\(\boldsymbol{J}\)，d2是残差，作者乘以-0.05，一是直接利用0.05作为步长，二是用这里的负号代替\(-\mathbf{J}^\top\boldsymbol{r}\)这里的负号，下面就不用写-matAtB。最后cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR)这一步就是求解上述的线性方程组。
三、matA 里那一堆 a1…c9 是什么？
它们是点到平面残差对 \(t_z, \theta_{x}, \theta_{y}\)的偏导数，即：
\[\mathbf{J}_i =
\begin{bmatrix}
\frac{\partial r_i}{\partial \theta_x} &
\frac{\partial r_i}{\partial \theta_y} &
\frac{\partial r_i}{\partial t_z}
\end{bmatrix}
\]
要注意LeGO-LOAM里的点云坐标系和ROS坐标系x轴朝前，y轴朝左，z轴朝上不同，它是z轴朝前，x轴朝左，y轴朝上，
LOAM 采取的旋转矩阵定义是 \(R=R_zR_xR_y\)，其实也就是ROS坐标系下外旋XYZ方式（也不常见，一般其实是ZYX）
\[\frac{\partial r_i}{\partial \theta_x}=
\mathbf n_i^T
\frac{\partial \mathbf R^T}{\partial \theta_x}
(\mathbf p_i-\mathbf t)
\]
由 TransformToStart() 可知，当前点变换回起始坐标系时使用的是：
\[\mathbf R^T=R_y(-\theta_y)R_x(-\theta_x)R_z(-\theta_z)
\]
记：
\[\begin{aligned}
&s_{rx}=\sin\theta_x,\quad c_{rx}=\cos\theta_x,\\
&s_{ry}=\sin\theta_y,\quad c_{ry}=\cos\theta_y,\\
&s_{rz}=\sin\theta_z,\quad c_{rz}=\cos\theta_z.
\end{aligned}
\]
将 \(\mathbf R^T\) 展开并对 \(\theta_x\) 求导：
\[\frac{\partial \mathbf R^T}{\partial \theta_x}=
\begin{bmatrix}
-c_{rx}s_{ry}s_{rz} & c_{rx}c_{rz}s_{ry} & s_{rx}s_{ry}\\
s_{rx}s_{rz} & -s_{rx}c_{rz} & c_{rx}\\
c_{rx}c_{ry}s_{rz} & -c_{rx}c_{ry}c_{rz} & -c_{ry}s_{rx}
\end{bmatrix}
\]
令：
\[\mathbf p_i-\mathbf t=
\begin{bmatrix}
p_x-t_x\\p_y-t_y\\p_z-t_z
\end{bmatrix},
\qquad
\mathbf n_i=
\begin{bmatrix}n_x\\n_y\\n_z\end{bmatrix},
\]
于是：
\[\begin{aligned}
\frac{\partial r_i}{\partial \theta_x}={}&
\left[-c_{rx}s_{ry}s_{rz}(p_x-t_x)
+c_{rx}c_{rz}s_{ry}(p_y-t_y)
+s_{rx}s_{ry}(p_z-t_z)\right]n_x\\
&+\left[s_{rx}s_{rz}(p_x-t_x)
-s_{rx}c_{rz}(p_y-t_y)
+c_{rx}(p_z-t_z)\right]n_y\\
&+\left[c_{rx}c_{ry}s_{rz}(p_x-t_x)
-c_{rx}c_{ry}c_{rz}(p_y-t_y)
-c_{ry}s_{rx}(p_z-t_z)\right]n_z.
\end{aligned}
\]
将同类项展开并按照 C++ 中的写法定义：
\[\begin{aligned}
&a_1=c_{rx}s_{ry}s_{rz},\quad
a_2=c_{rx}c_{rz}s_{ry},\quad
a_3=s_{rx}s_{ry},\\
&a_4=t_xa_1-t_ya_2-t_za_3,\\
&a_5=s_{rx}s_{rz},\quad
a_6=c_{rz}s_{rx},\\
&a_7=t_ya_6-t_zc_{rx}-t_xa_5,\\
&a_8=c_{rx}c_{ry}s_{rz},\quad
a_9=c_{rx}c_{ry}c_{rz},\quad
a_{10}=c_{ry}s_{rx},\\
&a_{11}=t_za_{10}+t_ya_9-t_xa_8.
\end{aligned}
\]
就得到：
\[\begin{aligned}
\frac{\partial r_i}{\partial \theta_x}={}&
(-a_1p_x+a_2p_y+a_3p_z+a_4)n_x\\
&+(a_5p_x-a_6p_y+c_{rx}p_z+a_7)n_y\\
&+(a_8p_x-a_9p_y-a_{10}p_z+a_{11})n_z.
\end{aligned}
\]
代入平面法向量分量后，C++ 中的 arx 写成：
float arx = (-a1 * pointOri.x + a2 * pointOri.y + a3 * pointOri.z + a4) * coeff.x
+ (a5 * pointOri.x - a6 * pointOri.y + crx * pointOri.z + a7) * coeff.y
+ (a8 * pointOri.x - a9 * pointOri.y - a10 * pointOri.z + a11) * coeff.z;
同理，对 \(\theta_z\) 求导：
\[\frac{\partial r_i}{\partial \theta_z}=
\mathbf n_i^T
\frac{\partial \mathbf R^T}{\partial \theta_z}
(\mathbf p_i-\mathbf t).
\]
将结果整理为：
\[\begin{aligned}
\frac{\partial r_i}{\partial \theta_z}={}&
(c_1p_x+c_2p_y+c_3)n_x\\
&+(c_4p_x-c_5p_y+c_6)n_y\\
&+(c_7p_x+c_8p_y+c_9)n_z,
\end{aligned}
\]
其中：
\[\begin{aligned}
&b_1=-c_{rz}s_{ry}-c_{ry}s_{rx}s_{rz},\\
&b_2=c_{ry}c_{rz}s_{rx}-s_{ry}s_{rz},\\
&b_5=c_{ry}c_{rz}-s_{rx}s_{ry}s_{rz},\\
&b_6=c_{ry}s_{rz}+c_{rz}s_{rx}s_{ry},\\
&c_1=-b_6,\quad c_2=b_5,\quad c_3=t_xb_6-t_yb_5,\\
&c_4=-c_{rx}c_{rz},\quad c_5=c_{rx}s_{rz},\\
&c_6=t_yc_5-t_xc_4,\\
&c_7=b_2,\quad c_8=-b_1,\quad c_9=-t_xb_2+t_yb_1.
\end{aligned}
\]
这正是代码中的 arz。
最后，对平移量 \(t_y\) 求导：
\[\frac{\partial r_i}{\partial t_y}
=-\mathbf n_i^T\mathbf R^T
\begin{bmatrix}0\\1\\0\end{bmatrix}
=-b_6n_x+c_4n_y+b_2n_z,
\]
对应代码：
float aty = -b6 * coeff.x + c4 * coeff.y + b2 * coeff.z;
所以 matA 中的三列就是 arx、arz 和 aty，分别由点到平面残差对对应变量求偏导得到。
这里需要注意，代码中的 coeff.x/y/z 并不是严格意义上的平面单位法向量分量。设平面归一化后的法向量为：
\[\mathbf n_i=\begin{bmatrix}p_a\\p_b\\p_c\end{bmatrix},
\]
代码实际保存的是：
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
因此：
\[\begin{bmatrix}
\texttt{coeff.x}\\
\texttt{coeff.y}\\
\texttt{coeff.z}
\end{bmatrix}
=s\mathbf n_i.
\]
所以 arx、arz 和 aty 是乘过 \(s\) 的加权雅可比，而不是未加权的纯偏导：
\[\texttt{arx}=s\frac{\partial r_i}{\partial\theta_x},\qquad
\texttt{arz}=s\frac{\partial r_i}{\partial\theta_z},\qquad
\texttt{aty}=s\frac{\partial r_i}{\partial t_y}.
\]
这种处理属于加权最小二乘（Weighted Least Squares）。由于 \(s\) 会根据当前残差在每次迭代中重新计算，因此也可以看作迭代重加权最小二乘（Iteratively Reweighted Least Squares，IRLS），属于鲁棒 M 估计的一种实现，用于减小离群点对优化结果的影响。
四、退化检测
cv::eigen(matAtA, matE, matV);
对 \(\mathbf{A}^\top \mathbf{A}\) 做特征分解：
-
特征值 ≈ 该方向的信息量
-
小特征值 ⇒ 该自由度不可观（退化）
if (matE.at<float>(0, i) < eignThre[i]) {
matV2.at<float>(i, j) = 0;
isDegenerate = true;
}
意思是：
如果某个方向的信息量不足，就禁止沿该方向更新
最终构造一个投影矩阵：
\[\mathbf{P} = \mathbf{V}^{-1} \mathbf{V}_{\text{filtered}}
\]
并做：
matX = matP * matX;
这是 LOAM 的“软约束冻结退化自由度”技巧。
附录
原始点到平面残差优化代码
bool FeatureAssociation::calculateTransformationSurf(int iterCount)
{
int pointSelNum = laserCloudOri->points.size();
cv::Mat matA(pointSelNum, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matAt(3, pointSelNum, CV_32F, cv::Scalar::all(0));
cv::Mat matAtA(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matB(pointSelNum, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matAtB(3, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matX(3, 1, CV_32F, cv::Scalar::all(0));
float srx = sin(transformCur[0]);
float crx = cos(transformCur[0]);
float sry = sin(transformCur[1]);
float cry = cos(transformCur[1]);
float srz = sin(transformCur[2]);
float crz = cos(transformCur[2]);
float tx = transformCur[3];
float ty = transformCur[4];
float tz = transformCur[5];
float a1 = crx * sry * srz;
float a2 = crx * crz * sry;
float a3 = srx * sry;
float a4 = tx * a1 - ty * a2 - tz * a3;
float a5 = srx * srz;
float a6 = crz * srx;
float a7 = ty * a6 - tz * crx - tx * a5;
float a8 = crx * cry * srz;
float a9 = crx * cry * crz;
float a10 = cry * srx;
float a11 = tz * a10 + ty * a9 - tx * a8;
float b1 = -crz * sry - cry * srx * srz;
float b2 = cry * crz * srx - sry * srz;
float b5 = cry * crz - srx * sry * srz;
float b6 = cry * srz + crz * srx * sry;
float c1 = -b6;
float c2 = b5;
float c3 = tx * b6 - ty * b5;
float c4 = -crx * crz;
float c5 = crx * srz;
float c6 = ty * c5 + tx * -c4;
float c7 = b2;
float c8 = -b1;
float c9 = tx * -b2 - ty * -b1;
for (int i = 0; i < pointSelNum; i++) {
pointOri = laserCloudOri->points[i];
coeff = coeffSel->points[i];
float arx = (-a1 * pointOri.x + a2 * pointOri.y + a3 * pointOri.z + a4) * coeff.x
+ (a5 * pointOri.x - a6 * pointOri.y + crx * pointOri.z + a7) * coeff.y
+ (a8 * pointOri.x - a9 * pointOri.y - a10 * pointOri.z + a11) * coeff.z;
float arz = (c1 * pointOri.x + c2 * pointOri.y + c3) * coeff.x
+ (c4 * pointOri.x - c5 * pointOri.y + c6) * coeff.y + (c7 * pointOri.x + c8 * pointOri.y + c9) * coeff.z;
float aty = -b6 * coeff.x + c4 * coeff.y + b2 * coeff.z;
float d2 = coeff.intensity;
matA.at<float>(i, 0) = arx;
matA.at<float>(i, 1) = arz;
matA.at<float>(i, 2) = aty;
matB.at<float>(i, 0) = -0.05 * d2;
}
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
if (iterCount == 0) {
cv::Mat matE(1, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV2(3, 3, CV_32F, cv::Scalar::all(0));
cv::eigen(matAtA, matE, matV);
matV.copyTo(matV2);
isDegenerate = false;
isInSmallArea = false;
// float eignThre[3] = {10, 10, 8.0};
// float eignThre[3] = {15, 15, 15};
float eignThre[3] = { 19, 15, 15 };
int count = 0;
for (int i = 2; i >= 0; i--) {
if (matE.at<float>(0, i) < eignThre[i]) {
// printf("lo-surf i: %d, eign: %f < %f\n", i, matE.at<float>(0, i), eignThre[i]);
for (int j = 0; j < 3; j++) {
matV2.at<float>(i, j) = 0;
}
isDegenerate = true;
count++;
} else {
break;
}
}
matP = matV.inv() * matV2;
if (count
>= 3) { // 如果进入条件3次及以上,则认为退化比较严重，是在小空间范围内，如果在狭小空间内开门的情况下，似乎并没有多大的变化
isInSmallArea = true; // 设置为 true
printf("lo-surf eign: %f, %f, %f\n", matE.at<float>(0, 2), matE.at<float>(0, 1), matE.at<float>(0, 0));
} else {
// printf("lo-surf eign: %f, %f, %f\n", matE.at<float>(0, 2), matE.at<float>(0, 1), matE.at<float>(0, 0));
}
}
if (isDegenerate) {
cv::Mat matX2(3, 1, CV_32F, cv::Scalar::all(0));
matX.copyTo(matX2);
matX = matP * matX2;
}
// else
// printf("ooooooook\n");
// std::cout << "matX: " << matX.at<float>(0, 0) << ", " << matX.at<float>(1, 0) << ", " << matX.at<float>(2, 0) <<
// std::endl;
transformCur[0] += matX.at<float>(0, 0);
transformCur[2] += matX.at<float>(1, 0);
transformCur[4] += matX.at<float>(2, 0); // z
// std::cout << "transformCur024: [" << transformCur[0] << ", " << transformCur[2] << ", " << transformCur[4] << "]"
// << std::endl;
for (int i = 0; i < 6; i++) {
if (isnan(transformCur[i])) transformCur[i] = 0;
}
float deltaR = sqrt(pow(rad2deg(matX.at<float>(0, 0)), 2) + pow(rad2deg(matX.at<float>(1, 0)), 2));
float deltaT = sqrt(pow(matX.at<float>(2, 0) * 100, 2));
if (deltaR < 0.1 && deltaT < 0.1) {
return false;
}
return true;
}
SimPy推导脚本: \(r_i(\boldsymbol{\xi})\)对\(t_z, r_x, r_y\)求导
待更新
浙公网安备 33010602011771号