lego-loam代码分析(3)-激光里程计
上节分析了其点云特征提取,目的是用于帧间匹配获取激光里程计。而像常见的点云匹配如NDT、ICP等匹配算法,均是对整个点云进行匹配。而loam为提高效率和实时性,则采用特征点进行匹配。
匹配初始化
由于激光里程计相临两帧点云进行匹配获取,即当前帧与上一帧进行匹配,故开启slam收到的第一帧点云数据则需要对cloud_last
进行赋值,即对上一帧进行记录和存储,不做匹配运算。
pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp; // 第一次last 和 curr 应一样, 即赋初始状态
cornerPointsLessSharp = laserCloudCornerLast;
laserCloudCornerLast = laserCloudTemp;
laserCloudTemp = surfPointsLessFlat;
surfPointsLessFlat = laserCloudSurfLast;
laserCloudSurfLast = laserCloudTemp;
kdtreeCornerLast.setInputCloud(laserCloudCornerLast); // 初始化KD TREE
kdtreeSurfLast.setInputCloud(laserCloudSurfLast);
laserCloudCornerLastNum = laserCloudCornerLast->points.size(); // 获取两种特征点云个数
laserCloudSurfLastNum = laserCloudSurfLast->points.size();
TransformToStart
激光点云进行运动补偿很容易理解,由于雷达扫描一圈是需要时间的,如果激光雷达静止不动,则一圈所有激光光束测量的距离为基于同一个原点测量所得;而若雷达在运动,显然每一次扫描中的光束则基于不同原点进行测量,换算成笛卡尔坐标系时,显然会发现产生了畸变。因此需要进行运动补偿,即根据每个点的时刻计算其对应的原点,然后重新计算在笛卡尔坐标系下的坐标。常用的方法即采用imu等方法估计雷达运动速度,根据每条光速的时刻进行积分,可获取每条光束对应的原点坐标,进行修正。
但是TransformToStart
根据他人的解释为运动补偿,即将每帧中点云均转换为以第一个扫描点为同一坐标系,即为运动补偿。但是分析其代码一直不太理解其具体意义(望理解的人分享一下)。
有人如此分析,
LeGo-LOAM源码阅读笔记(三)—featureAssociation.cpp,
// s代表的其实是一个比例,s的计算方法应该如下:
// s=(pi->intensity - int(pi->intensity))/SCAN_PERIOD
表示无法理解,因为(pi->intensity - int(pi->intensity))
为强度信息中的小数部分,小数部分应该为_scan_period * relTime
,可看下面代码,这是预处理坐标系更改void adjustDistortion()
中赋值。
point.intensity =
int(segmentedCloud->points[i].intensity) + _scan_period * relTime;
查找多方面资料,都没有找到详细解释,望有高手分享
void FeatureAssociation::TransformToStart(PointType const *const pi,
PointType *const po) {
// 插值系数,为相对与第一个点的时间差
// 其中10为系数,可调整,感觉像假设10是个速度?????,但是速度如何来的呢
float s = 10 * (pi->intensity - int(pi->intensity)); // 由于强度为thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0; // 强度根据不同层则不一样,可用于显示区分
// 而在特征提取进行了修改 intensity = int(segmentedCloud->points[i].intensity) + _scan_period * relTime;
// 如此可看出, s应该为每扫描行中点云与第一个点云的时间差即10*_scan_period * relTime
// 线性插值, 如果静止,显然无需插值,则每个点相对激光原点(或者相对于上刻位置),全部一致
// 由于匀速运动,因此认为点云中的每个点相对于上刻的位置有一定的偏移,其偏移量为s
// 如果s不是速度,像是个比例系数,即每个原点都在如此的比例系数下进行平移和旋转?????
// 不太理解,s针对一圈中固定索引的点云基本相当于一个常数
// 获取了每个点的原点坐标,然后校准新的笛卡尔坐标
float ry = s * transformCur[1];
float rx = s * transformCur[0];
float rz = s * transformCur[2];
float tx = s * transformCur[3];
float ty = s * transformCur[4];
float tz = s * transformCur[5];
// 将该点根据自己的畸变进行旋转和平移
float x1 = cos(rz) * (pi->x - tx) + sin(rz) * (pi->y - ty);
float y1 = -sin(rz) * (pi->x - tx) + cos(rz) * (pi->y - ty);
float z1 = (pi->z - tz);
float x2 = x1;
float y2 = cos(rx) * y1 + sin(rx) * z1;
float z2 = -sin(rx) * y1 + cos(rx) * z1;
po->x = cos(ry) * x2 - sin(ry) * z2;
po->y = y2;
po->z = sin(ry) * x2 + cos(ry) * z2;
po->intensity = pi->intensity;
}
TransformToEnd
与TransformToStart
方法一致,应该是将所有点统一到扫描的最后一点时刻的坐标系,其方法是先采用TransformToStart
方法转换到第一个点,然后再转换到最后一点。
两次LM匹配
由于已经获取前后两帧的角点和平面点特征点云,因此进行帧间点云匹配,则可获取相邻两帧平移旋转变换,由初始位姿进行累计,从而获得激光里程计信息。
lego-loam采用将两次即两种特征点分别匹配,平面匹配和2维位姿匹配。
其中LM优化的流程为构建约束方程 -> 约束方程求偏导构建Jaccobian矩阵 -> L-M求解。
平面匹配
匹配点查找
采用KDtree在上一帧特征点云中搜索对应当前点云一个点最近点,然后在找到前后两个扫描线中最近的两个点。如下图所示。其中i为当前时刻中一个点云,而m,j,l为上一帧中对应的三个点,此三个点可构成一个平面。
// 代码略,较为简单,容易理解
目标点到匹配平面的距离
匹配采用的方法为点到平面匹配,即需求点到平面的距离,具体公式截图论文。
公式不容易理解,简单解释就是求出三个点构成的平面的方向量并求出平面方程。
具体可参考点、平面和法线关系一文中有具体推导。其中法线计算为平面上两个不平行的向量叉乘。
代码中A、B、C三个点为对应的平面上的点;
- 求出两个向量AB和AC
- 计算AB和AC的叉乘,即获取方向量
- 带回平面方程,计算出平面方程常量D,获取平面方程;
- 求出方向量的模,获取单位方向量;
- 计算目标点到达法向量的投影,即为目标点到达平面的距离;
// 获取三个点,构成一个平面的三角形
PointType tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]]; // A点
PointType tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]]; // B点
PointType tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]]; // C点
// 平面向量
// AB (tripod2.x - tripod1.x, tripod2.y - tripod1.y, tripod2.z - tripod1.z)
// AC (tripod3.x - tripod1.x, tripod3.y - tripod1.y, tripod3.z - tripod1.z)
// 向量叉乘获得平面法向量AB × AC
// 法向量三个分量(可套公式获得)
float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) -
(tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);
float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) -
(tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);
float pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) -
(tripod3.x - tripod1.x) * (tripod2.y - tripod1.y);
// 平面方程 Ax+By+Cz+D = 0
// 获取平面方程的常数D
float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);
// 平面法向量的模
float ps = sqrt(pa * pa + pb * pb + pc * pc);
// 换算成单位向量
pa /= ps;
pb /= ps;
pc /= ps;
pd /= ps;
// 计算目标点到达平面的距离, 即目标点在方向量上的投影
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
// 考虑权重,距离越小,权重越大,应该是经验值??????
float s = 1;
if (iterCount >= 5) {
s = 1 -
1.8 * fabs(pd2) /
sqrt(sqrt(pointSel.x * pointSel.x + pointSel.y * pointSel.y +
pointSel.z * pointSel.z));
}
// 记录平面参数和点到平面的距离,权重过小则忽略
if (s > 0.1 && pd2 != 0) {
PointType coeff;
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;
laserCloudOri->push_back(surfPointsFlat->points[i]); // 存放原始点云
coeffSel->push_back(coeff); // 存放平面信息和点到平面距离
}
LM求解
获取矩阵方程A*X=B,即求出矩阵A和B。其中A为一阶偏导数矩阵,而B即为对应点到面的距离矩阵。考虑到矩阵A不一定满秩,无法直接QR分解求解,因此矩阵方程左右两侧均左乘以A的转置。即 A T ∗ A ∗ x = A T ∗ B A^T * A * x = A^T *B AT∗A∗x=AT∗B。然后再进行QR分解求值。
for (int i = 0; i < pointSelNum; i++) {
PointType pointOri = laserCloudOri->points[i];
PointType 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;
// A 矩阵
matA(i, 0) = arx;
matA(i, 1) = arz;
matA(i, 2) = aty;
// B 矩阵
matB(i, 0) = -0.05 * d2;
}
// A的转置
matAt = matA.transpose();
// A和B 同左乘A的转置, 目的是希望左侧矩阵满秩
matAtA = matAt * matA;
matAtB = matAt * matB;
// matAtA * X = matAt * matB
// QR分解求解
matX = matAtA.colPivHouseholderQr().solve(matAtB);
// ????不是太明白,矩阵退化判断
if (iterCount == 0) {
Eigen::Matrix<float,1,3> matE;
Eigen::Matrix<float,3,3> matV;
Eigen::Matrix<float,3,3> matV2;
Eigen::SelfAdjointEigenSolver< Eigen::Matrix<float,3,3> > esolver(matAtA);
matE = esolver.eigenvalues().real();
matV = esolver.eigenvectors().real();
matV2 = matV;
isDegenerate = false;
float eignThre[3] = {10, 10, 10};
for (int i = 2; i >= 0; i--) {
if (matE(0, i) < eignThre[i]) {
for (int j = 0; j < 3; j++) {
matV2(i, j) = 0;
}
isDegenerate = true;
} else {
break;
}
}
matP = matV.inverse() * matV2;
}
//
if (isDegenerate) {
Eigen::Matrix<float,3,1> matX2;
matX2 = matX;
matX = matP * matX2;
}
// 叠加每次迭代的旋转和平移量
transformCur[0] += matX(0, 0);
transformCur[2] += matX(1, 0);
// y 轴平移 (原点云的z的平移)
transformCur[4] += matX(2, 0);
// 无效值就恢复0
for (int i = 0; i < 6; i++) {
if (std::isnan(transformCur[i])) transformCur[i] = 0;
}
// 计算旋转平移误差矩阵
float deltaR = sqrt(pow(RAD2DEG * (matX(0, 0)), 2) +
pow(RAD2DEG * (matX(1, 0)), 2));
float deltaT = sqrt(pow(matX(2, 0) * 100, 2));
// 旋转平移误差小于一定阈值,则停止迭代
if (deltaR < 0.1 && deltaT < 0.1) {
return false;
}
注:1.没有认真进行偏导数的推导;2.矩阵退化没有完全理解;3.作者完全手写推导出来的,效果估计会现成的优化库快
角点匹配
角点的整个匹配与平面匹配方法完全一致,其具体实现和代码注释不再详述。
匹配点查找
采用KDtree在上一帧特征点云中搜索对应当前点云一个点最近点,然后在找到前后两个扫描线中最近的一个点。如下图所示。 其中i为当前时刻中一个点云,而j,l为上一帧中对应的两个点,此两点可构成一条直线。
目标点到直线的距离
匹配采用的是目标点到达参考直线的距离,方法如下,已知A和O两个点,求出Q到达OA直线的距离。
具体计算可参考点到直线的距离一文中有具体公式推导。
但是从loam代码中却看出计算公式如下,即求出i到直线j,l的最短距离,公式却是
d
=
∣
i
j
⃗
×
i
l
∣
⃗
∣
j
l
⃗
∣
d = \frac{|\vec{ij} × \vec{il|}}{|\vec{jl}|}
d=∣jl∣∣ij×il∣
按照平面法向量的思想可知,其结果一致。
odometry 发布
经过两次LM匹配,即可认为是经过帧间匹配可获取相临帧的位姿转移矩阵,通过里程计思想进行累加,则可获取当前帧的全局位姿,即获取激光里程计。
// 矩阵转换,根据变换的矩阵, 更新全局位姿
// 先旋转, 在平移
// 已知初始全局位置和相临两帧的位姿转移矩阵,
// 累加获取当前帧全局位置
void FeatureAssociation::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] ) -
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);
// 获取当前帧全局位置,即构建里程计
transformSum[0] = rx;
transformSum[1] = ry; //
transformSum[2] = rz; // 绕z轴方向的yaw 航向角
transformSum[3] = tx; // x 坐标
transformSum[4] = ty; // y 坐标
transformSum[5] = tz; // z 坐标
}
发布建图数据
整个特征点提取其目的包括两个:用于获取高频率的激光里程计和用于低频率的建图匹配。而获取的激光里程计频率较高,可为后期建图匹配提供预测值。可以说前3节所有处理均可认为是slam中传感器的预处理,将预处理后的数据输入slam主流程。
其预处理后的数据包括:
- 平面点云;
- 角点点云;
- 非特征点云;
- 激光里程计;
其中1,2,3三种点云的集合则为原始点云。而激光里程计也可以有其他里程计进行替换,如编码器,imu等。
其整个特征提取以及里程计处理主流程代码如下:
void FeatureAssociation::runFeatureAssociation() {
while (ros::ok()) {
ProjectionOut projection;
_input_channel.receive(projection); // 接收三类数据
if( !ros::ok() ) break;
//--------------
outlierCloud = projection.outlier_cloud; //分别为未被分类的孤立点云簇
segmentedCloud = projection.segmented_cloud; //被分类的包含地面的点云
segInfo = std::move(projection.seg_msg); //分类的相关信息
_scan_msg = std::move(projection.scan_msg);
cloudHeader = segInfo.header;
timeScanCur = cloudHeader.stamp.toSec();
/** 1. Feature Extraction */
adjustDistortion(); // 坐标系转换
calculateSmoothness(); // 计算平滑性
markOccludedPoints(); // 标记在水平扫描方向上,屏蔽不可靠点
extractFeatures(); // 特征提取包括,角点和平坦点
publishCloud(); // cloud for visualization
// Feature Association
if (!systemInitedLM) { // 仅执行一次,用于初始化
checkSystemInitialization();
continue;
}
updateTransformation(); // 计算两帧激光数据之间转换矩阵
integrateTransformation(); // 迭代更新,将每两帧之间变换,进行累计变换,构建里程计
publishOdometry();
publishCloudsLast(); // cloud to mapOptimization
// 以上为高频率的激光里程计获取
//--------------
_cycle_count++;
// 建图_mapping_frequency_div倍降频更新
if (_cycle_count == _mapping_frequency_div) {
_cycle_count = 0;
AssociationOut out;
out.cloud_corner_last.reset(new pcl::PointCloud<PointType>());
out.cloud_surf_last.reset(new pcl::PointCloud<PointType>());
out.cloud_outlier_last.reset(new pcl::PointCloud<PointType>());
*out.cloud_corner_last = *laserCloudCornerLast; // 将平面信息 和角点发出给建图算法
*out.cloud_surf_last = *laserCloudSurfLast;
*out.cloud_outlier_last = *outlierCloud; // 将非特征点发给建图算法
out.laser_odometry = laserOdometry; // 激光里程计
}
}