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 分析了 点云匹配前戏之初值计算及局部地图构建
经过上篇博客,
有了上面两步,就可以构建角点和面点的:
其中残差就是点到直线/面 的距离,方向就是距离缩小的方向。
本篇主要分析:如何构建角点面点的残差及梯度
点云配准的代码在 mapOptmization.cpp 中
在点云的回调函数中,调用了 点云配准 的函数 : scan2MapOptimization();
函数功能:得到一个旋转和平移,使得当前帧最好的贴合到局部地图中去
void scan2MapOptimization()
{
if (cloudKeyPoses3D->points.empty())
return;
如果没有关键帧,那也没办法做当前帧到局部地图的匹配,则直接return
if (laserCloudCornerLastDSNum > edgeFeatureMinValidNum && laserCloudSurfLastDSNum > surfFeatureMinValidNum)
{
... (配准过程)
}
判断当前帧的角点数和面点数是否足够
默认 角点要求10 面点要求100
else {
ROS_WARN("Not enough features! Only %d edge and %d planar features available.", laserCloudCornerLastDSNum, laserCloudSurfLastDSNum);
}
角点或者面点的数量不够,则终端提示信息
下面来看配准过程
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMapDS);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMapDS);
分别把角点和面点 局部地图构建 kdtree
for (int iterCount = 0; iterCount < 30; iterCount++)
{
迭代求解,迭代30次
里面是手写的优化器
lio-sam是继承了 loam和lego-loam中的 高斯牛顿的 优化方法
cornerOptimization();
角点优化
来看里面具体内容
void cornerOptimization()
{
updatePointAssociateToMap();
将当前帧的先验位姿(初值估计那来的) 将欧拉角转成eigen的形式
for (int i = 0; i < laserCloudCornerLastDSNum; i++)
{
遍历当前帧的角点
pointOri = laserCloudCornerLastDS->points[i];
取出该点
pointAssociateToMap(&pointOri, &pointSel);
将该点从当前帧通过初始的位姿变换到地图坐标系下去
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
在角点地图里面寻找距离当前点比较近的5个点
if (pointSearchSqDis[4] < 1.0) {
计算找到的点中距离当前点最远的点,如果距离太大那说明这个约束不可信,就跳过
for (int j = 0; j < 5; j++) {
cx += laserCloudCornerFromMapDS->points[pointSearchInd[j]].x;
cy += laserCloudCornerFromMapDS->points[pointSearchInd[j]].y;
cz += laserCloudCornerFromMapDS->points[pointSearchInd[j]].z;
}
cx /= 5; cy /= 5; cz /= 5;
计算找到的5个点的均值
float a11 = 0, a12 = 0, a13 = 0, a22 = 0, a23 = 0, a33 = 0;
for (int j = 0; j < 5; j++) {
float ax = laserCloudCornerFromMapDS->points[pointSearchInd[j]].x - cx;
float ay = laserCloudCornerFromMapDS->points[pointSearchInd[j]].y - cy;
float az = laserCloudCornerFromMapDS->points[pointSearchInd[j]].z - cz;
a11 += ax * ax; a12 += ax * ay; a13 += ax * az;
a22 += ay * ay; a23 += ay * az;
a33 += az * az;
}
a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5;
matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13;
matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23;
matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33;
计算协方差矩阵
cv::eigen(matA1, matD1, matV1);
特征值分解 为 证明这5个点是一条直线
if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
这是线特征 , 要求 最大 特征值 大于3倍的次大特征值
判断不是线特征,则直接不处理这个特征点了
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
本次处理的特征点
float x1 = cx + 0.1 * matV1.at<float>(0, 0);
float y1 = cy + 0.1 * matV1.at<float>(0, 1);
float z1 = cz + 0.1 * matV1.at<float>(0, 2);
float x2 = cx - 0.1 * matV1.at<float>(0, 0);
float y2 = cy - 0.1 * matV1.at<float>(0, 1);
float z2 = cz - 0.1 * matV1.at<float>(0, 2);
进行了一个直线的构建
通过点的均值往两边拓展
最大特征值对应的特征向量对应的就是直线的方向向量。所以用的matV1的第0行
现在有了 一个点,和构建的两个点,下面要求整个点到构建的两个点形成直线的距离和方向,即残差与残差方向
float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
这部分求得就是
叉乘的结果就是OC,垂直所在平面向上
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
这个就是AB的模长
float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
- (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
这部分求得是
也就是垂线的方向
float ld2 = a012 / l12;
float s = 1 - 0.9 * fabs(ld2);
求距离 也就是残差,根据三角形面积
一个简单的核函数 , 残差越大 权重 越 低
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.intensity = s * ld2;
残差 x y z 代表方向 intensity 为大小
if (s > 0.1) {
laserCloudOriCornerVec[i] = pointOri;
coeffSelCornerVec[i] = coeff;
laserCloudOriCornerFlag[i] = true;
}
如果残差小于10cm,就认为是一个有效的约束
下面来看面点的优化部分
void surfOptimization()
{
updatePointAssociateToMap();
将当前帧的先验位姿(初值估计那来的) 将欧拉角转成eigen的形式
for (int i = 0; i < laserCloudSurfLastDSNum; i++)
{
遍历当前帧每个面点
pointOri = laserCloudSurfLastDS->points[i];
取出 该点
pointAssociateToMap(&pointOri, &pointSel);
将该点从当前帧通过初始的位姿变换到地图坐标系下去
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
在面点地图里面寻找距离当前点比较近的5个点
Eigen::Matrix<float, 5, 3> matA0;// 五个点 3个未知量 所以是 5*3
Eigen::Matrix<float, 5, 1> matB0;
Eigen::Vector3f matX0;
下面不是通过特征值分解 来求特征向量的 通过超定方程来求解
matA0.setZero();
matB0.fill(-1);//填充为-1
matX0.setZero();
平方方程 Ax + By + Cz + 1 = 0
if (pointSearchSqDis[4] < 1.0) {
最大距离不能超过1m
for (int j = 0; j < 5; j++) {
matA0(j, 0) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].x;
matA0(j, 1) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].y;
matA0(j, 2) = laserCloudSurfFromMapDS->points[pointSearchInd[j]].z;
}
填充 matA0 矩阵
matX0 = matA0.colPivHouseholderQr().solve(matB0);
求解 Ax = b 这个超定方程
float pa = matX0(0, 0);
float pb = matX0(1, 0);
float pc = matX0(2, 0);
float pd = 1;
求出来x的就是这个平面的法向量
for (int j = 0; j < 5; j++) {
if (fabs(pa * laserCloudSurfFromMapDS->points[pointSearchInd[j]].x +
pb * laserCloudSurfFromMapDS->points[pointSearchInd[j]].y +
pc * laserCloudSurfFromMapDS->points[pointSearchInd[j]].z + pd) > 0.2) {
planeValid = false;
break;
}
}
将每个点代入平面方程,计算点到平面的距离,如果距离大于0.2m,认为这个平面曲率偏大,就是无效的平面
if (planeValid) {
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
计算残差,点到平面的距离
float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));
残差到权重的换算 分母部分 点离激光越远 则 分母越大,那么权重就越大,
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
残差 x y z 代表梯度方向 intensity 为大小
if (s > 0.1) {
laserCloudOriSurfVec[i] = pointOri;
coeffSelSurfVec[i] = coeff;
laserCloudOriSurfFlag[i] = true;
}
如果权重大于阈值,就认为是一个有效的约束