lio-sam框架：点云匹配之手写高斯牛顿优化求状态量更新

• 前言
• 代码分

前言

LIO-SAM的全称是：Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

从全称上可以看出，该算法是一个紧耦合的雷达惯导里程计（Tightly-coupled Lidar Inertial Odometry），借助的手段就是利用GT-SAM库中的方法。

LIO-SAM 提出了一个利用GT-SAM的紧耦合激光雷达惯导里程计的框架。
实现了高精度、实时的移动机器人的轨迹估计和建图。

其中建图优化节点整体如下图

在这篇博客中 https://www.guyuehome.com/39723 分析了 点云匹配前戏之初值计算及局部地图构建

在这篇博客中 https://www.guyuehome.com/39767 分析了 点云配准之角点面点的残差及梯度构建

经过之前两片博客，

• 激光雷达当前帧有了初值的估计，那么则可以利用初值投到地图坐标系下
• 构建了局部地图
• 构建了角点和面点的残差及梯度方向

下面可以通过高斯牛顿下降算法来求解状态更新量了

本篇主要分析：如何通过高斯牛顿下降算法来求解状态更新量

代码分

在上一篇分析了 角点和面点的残差及梯度方向，就是

cornerOptimization();surfOptimization();

这两个函数

下面是通过这个函数将角点和面点的残差及梯度统一到一起

combineOptimizationCoeffs();

来看具体内容：

void combineOptimizationCoeffs(){ for (int i = 0; i < laserCloudCornerLastDSNum; ++i){// if (laserCloudOriCornerFlag[i] == true){laserCloudOri->push_back(laserCloudOriCornerVec[i]);//点coeffSel->push_back(coeffSelCornerVec[i]);//残差及梯度}}

只有标记为true的时候才是有效约束
点和残差及梯度加入到 laserCloudOri 和coeffSel 中

for (int i = 0; i < laserCloudSurfLastDSNum; ++i){if (laserCloudOriSurfFlag[i] == true){laserCloudOri->push_back(laserCloudOriSurfVec[i]);coeffSel->push_back(coeffSelSurfVec[i]);}}

同理处理面点

std::fill(laserCloudOriCornerFlag.begin(), laserCloudOriCornerFlag.end(), false);std::fill(laserCloudOriSurfFlag.begin(), laserCloudOriSurfFlag.end(), false);

标志位清零

当数据准备好之后，即可开始下面的优化

if (LMOptimization(iterCount) == true)break;

执行优化求解
虽然函数名写的是LM优化算法，但是函数内部是高斯牛顿下降优化算法

来看具体内容：

bool LMOptimization(int iterCount){ float srx = sin(transformTobeMapped[1]);float crx = cos(transformTobeMapped[1]);float sry = sin(transformTobeMapped[2]);float cry = cos(transformTobeMapped[2]);float srz = sin(transformTobeMapped[0]);float crz = cos(transformTobeMapped[0]);

首先求一些 x y z 的三角函数
注意这里有个坐标变换

camera <- lidar 就是雷达到相机坐标系的变换是这样的：

x = y
y = z
z = x
roll = pitch
pitch = yaw
yaw = roll

lidar <- camera 相机到雷达坐标系的变换是这样的：

x = z
y = x
z = y
roll = yaw
pitch = roll
yaw = pitch

transformTobeMapped 是一个6维的数组 分别存放：roll 、pitch、yaw、x、y、z
代码在这里已经做了 雷达到相机的坐标转化

int laserCloudSelNum = laserCloudOri->size();if (laserCloudSelNum < 50) {return false;}

检测有多少个约束，如果约束小于50，则直接return false

cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));

用cv Mat做了些变量定义

for (int i = 0; i < laserCloudSelNum; i++) {

迭代每一个约束点进行优化求解

pointOri.x = laserCloudOri->points[i].y;pointOri.y = laserCloudOri->points[i].z;pointOri.z = laserCloudOri->points[i].x;

首先将当前点转到相机系 lidar -> camera

coeff.x = coeffSel->points[i].y;coeff.y = coeffSel->points[i].z;coeff.z = coeffSel->points[i].x;coeff.intensity = coeffSel->points[i].intensity;

将当前点到线（面）的单位向量转到相机系

float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x+ (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y+ (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;float ary = ((cry*srx*srz - crz*sry)*pointOri.x + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x+ ((-cry*crz - srx*sry*srz)*pointOri.x + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x+ (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y+ ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;

求 雅克比矩阵 旋转部分
相机系下的旋转顺序是 Y - X - Z 对应 lidar 系下的 Z-Y-X

matA.at<float>(i, 0) = arz;matA.at<float>(i, 1) = arx;matA.at<float>(i, 2) = ary;matA.at<float>(i, 3) = coeff.z;matA.at<float>(i, 4) = coeff.x;matA.at<float>(i, 5) = coeff.y;matB.at<float>(i, 0) = -coeff.intensity;

这里就是把camera转到lidar了
赋值 雅克比矩阵

下面 构造 JTJ 以及 -JTe 矩阵

cv::transpose(matA, matAt);

matA 是 雅克比矩阵 J
matAt 是 JT

matAtA = matAt * matA;

JTJ

matAtB = matAt * matB;

-JTe

cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);

利用CV的方法求解增量
JTJX=-JTe

if (iterCount == 0) {cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));cv::eigen(matAtA, matE, matV);matV.copyTo(matV2);

检测一下 是否有退化得情况
对JTJ 进行特征值分解

for (int i = 5; i >= 0; i--) {if (matE.at<float>(0, i) < eignThre[i]) {for (int j = 0; j < 6; j++) {matV2.at<float>(i, j) = 0;}isDegenerate = true;} else {break;}}

特征值从小到大遍历，如果小于阈值就认为退化
对应的特征向量全部置0
退化标志位 为 true

if (isDegenerate){cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX = matP * matX2;}

如果发生退化，就对增量进行修改，退化方向不更新

transformTobeMapped[0] += matX.at<float>(0, 0);transformTobeMapped[1] += matX.at<float>(1, 0);transformTobeMapped[2] += matX.at<float>(2, 0);transformTobeMapped[3] += matX.at<float>(3, 0);transformTobeMapped[4] += matX.at<float>(4, 0);transformTobeMapped[5] += matX.at<float>(5, 0);

增量更新

float deltaR = sqrt(pow(pcl::rad2deg(matX.at<float>(0, 0)), 2) +pow(pcl::rad2deg(matX.at<float>(1, 0)), 2) +pow(pcl::rad2deg(matX.at<float>(2, 0)), 2));float deltaT = sqrt(pow(matX.at<float>(3, 0) * 100, 2) +pow(matX.at<float>(4, 0) * 100, 2) +pow(matX.at<float>(5, 0) * 100, 2));

计算更新的旋转和平移大小

if (deltaR < 0.05 && deltaT < 0.05) {return true; }

如果更新的旋转和平移过小，怎么认为达到最优解

return false; }

否则继续优化

求解出来优化结果后 把结果和imu进行一次加权融合

transformUpdate(); if (cloudInfo.imuAvailable == true){

判断 可以获取九轴imu的世界坐标系下的姿态

if (std::abs(cloudInfo.imuPitchInit) < 1.4){

角度没有很大

transformQuaternion.setRPY(transformTobeMapped[0], 0, 0);imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);transformTobeMapped[0] = rollMid;

lidar 匹配获得的roll角转成四元数
imu 获得的roll角转成四元数
使用四元的球面插值
插值的结果作为roll的最终结果

transformQuaternion.setRPY(0, transformTobeMapped[1], 0);imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);transformTobeMapped[1] = pitchMid;

同理pitch 角度

transformTobeMapped[0] = constraintTransformation(transformTobeMapped[0], rotation_tollerance);transformTobeMapped[1] = constraintTransformation(transformTobeMapped[1], rotation_tollerance);transformTobeMapped[5] = constraintTransformation(transformTobeMapped[5], z_tollerance);

对roll pitch 和z进行一些约束，主要针对室内2d场景下，已知2d先验可以加上这些约束

incrementalOdometryAffineBack = trans2Affine3f(transformTobeMapped);

最终的结果也可以转成eigen的结构

总结

以上是生活随笔为你收集整理的lio-sam框架：点云匹配之手写高斯牛顿下降优化求状态量更新的全部内容，希望文章能够帮你解决所遇到的问题。

如果觉得生活随笔网站内容还不错，欢迎将生活随笔推荐给好友。