LeGO-LOAM 激光里程计计算roll,pitch,tz增量详解
LeGO-LOAM 激光里程计计算\(t_z, \theta_{roll}/r_x, \theta_{pitch}/r_y\)
我们用点\(\boldsymbol{p_i'}\)、\(\boldsymbol{p_i}\)分别代表原始点云、变换后（车子在动引起）点云中的点，目标是求得位姿变换\(\boldsymbol{\xi}=r_x,r_y,r_z,t_x,t_y,t_z\)使得$\boldsymbol{p_i'} =\mathbf{R}(\boldsymbol{\xi}) \mathbf{p}_i + \mathbf{t}(\boldsymbol{\xi}) $。
LeGO-LOAM 创新之处在于将这6个变量的优化分成了两组\(t_z, \theta_{roll}/r_x, \theta_{pitch}/r_y\)和\(t_x, t_y,\theta_{yaw}/r_z\)分别优化，既减少了计算量也避免了它们之间的相互干扰。
其中对于\(t_z, \theta_{roll}/r_x, \theta_{pitch}/r_y\)采用的是点到平面的残差进行优化，这是本文讨论的内容。至于为什么用地面来优化求解这3个变量，可以想象一下，在地面上移动的时候，\(t_x, t_y, r_z\)都不会让车离开地面也就不会引入残差。
原始代码见原始点到平面残差优化代码，下面我逐次介绍这段代码在最小化什么、数学模型、A/B/X 的含义、以及退化处理。
一、最小化目标函数
对每一个平面特征点，有一个点到平面的约束：
其中：
-
\(\mathbf{p}_i\)：当前帧中的点（
pointOri） -
\(\mathbf{n}_i = (n_x, n_y, n_z)\)：平面法向量（存在
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'}\)就能确定一个平面。
目标是最小化残差平方和：
二、高斯牛顿法计算\(\boldsymbol{\xi}\)
根据高斯-牛顿的方法，我们对残差进行 一阶泰勒展开（Gauss–Newton）：
每次更新的最小化步\(\Delta \boldsymbol{\xi}\)来自最小化：
写成矩阵形式：
对上式求导，取梯度为0，有如下线性方程组：
对应代码里的求解部分：
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_{roll}, \theta_{pitch}\)的偏导数，即：
对应到代码中时，要注意LeGO-LOAM里的点云坐标系和ROS坐标系x轴朝前，y轴朝左，z轴朝上不同，它是z轴朝前，x轴朝左，y轴朝上
matA.at<float>(i, 0) = arx; // ∂r/∂pitch
matA.at<float>(i, 1) = arz; // ∂r/∂roll
matA.at<float>(i, 2) = aty; // ∂r/∂tz
这些复杂的 a1…a11, b1…c9：
-
是 R = Rz·Ry·Rx 的显式展开
-
再对 roll / pitch 求导
-
再与平面法向量 \(\mathbf{n}\) 点乘
也就是说：
至于对于\(t_z\)的求导就相当简单：
但是代码中这一步还引入了旋转矩阵的影响：
float aty = -b6 * coeff.x + c4 * coeff.y + b2 * coeff.z;
我认为是作者这里写错了，作者应该是把位姿变换写成\(\mathbf{R}(\mathbf{p}+\mathbf{t})\)
四、退化检测
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;
}
意思是：
如果某个方向的信息量不足，就禁止沿该方向更新
最终构造一个投影矩阵：
并做：
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;
}
浙公网安备 33010602011771号