LaserMapping源码分析¶
先来梳理一下：点云数据进来后，经过前两个节点的处理可以完成一个完整但粗糙的视觉里程计，可以概略地估计出Lidar的相对运动。如果不受任何测量噪声的影响，这个运动估计的结果足够精确，没有任何漂移，那我们可以直接利用估计的Lidar位姿和对应时刻的量测值完成建图。但这就如同现实中不存在一个不受外力就能匀速直线运动的小球一样，量测噪声是不可避免的，因此Lidar位姿估计偏差一定存在。（有兴趣的读者可以试一试，直接拿这两个节点的结果在rviz里显示一下，能不能分辨的出这一块仅仅依靠位姿估计拼接起来的点云是否正确）回过头来说说这个节点的代码。 概述 为了搞懂这部分内容，首先有两个问题需要明确： 1.为什么要有这个建图节点？ 2.建图节点又到底起了什么作用？ 第一个问题：为什么要有建图节点。正如我们刚才所说，Lidar里程计的结果不准确，拼起来的点也完全不成样子，且它会不断发散，因此误差也会越来越大。试想一下我们对特征的提取仅仅只是关注了他们的曲率，这种方式怎么可能完美的得到两帧点云准确的配准点。且点云中的点是离散的，我们也无法保证上一帧的点在下一帧中仍会被扫到。总之，无论我们多努力想让Lidar里程计的估计结果变得精准，残酷且冰冷的显示都会把我们的幻想击碎。因此，我们需要依靠别的方式去优化Lidar里程计的位姿估计精度。在SLAM领域，一般会采用与地图匹配的方式来优化这一结果。其实道理也很简单，我们始终认为后一时刻的观测较前一时刻带有更多的误差，换而言之，我们更加信任前一时刻结果。这就是回归到贝叶斯估计那一套东西了。因此我们对已经构建地图的信任程度远高于临帧点云配准后的Lidar运动估计。所以我们可以利用已构建地图对位姿估计结果进行修正。 第二个问题：建图节点起到了什么作用？在回答上一个问题时也已经提到了，它的作用就是优化Lidar里程计的位姿估计结果。怎么做呢？没错，就是利用地图。试想一下，你在得到第一帧点云时你的lidar就扫到了数万个点，此时Lidar的位置我们把它作为（0,0,0），在不考虑测量噪声的情况下这数万个点都是相对精确的，我们把这数万个点所构成的环境作为此时的地图。而后Lidar运动了一小段，我们通过Lidar里程计的方法估算了它的相对运动，于是就可以将此时的Lidar位姿及此时的点按照我们估计的相对运动情况，转换到上一时刻（建立地图时的坐标系）的坐标系下。只不过由于里程计估计误差，地图可能拼歪了。既然这样，如果把之前的地图也进行匹配，是不是就可以优化此时Lidar的位姿了呢？这就是建图节点起到的关键作用。只不过拿当前扫描的点云和地图中所有点云去配准，这个计算消耗太大，因此为了保证实时性，作者在这里采用了一种低频处理方法，即调用建图节点的频率仅为调用里程计节点频率的十分之一。 有了这个整体的思路，我们就可以来看看具体是怎么做的,
接着看看输入输出：
lasermapping节点main函数的套路和之前类似：
/** Main node entry point. */
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserMapping");
ros::NodeHandle node;
ros::NodeHandle privateNode("~");
loam::LaserMapping laserMapping(0.1);
if (laserMapping.setup(node, privateNode)) {
// initialization successful
laserMapping.spin();
}
return 0;
}
setup()完成参数初始化，topic订阅和发布。spin()函数启动回调，同时调用process()进行处理。 重点看process()函数，大致步骤分为2步，1.坐标转换
void LaserMapping::process()
{
if (!hasNewData()) {
// waiting for new data to arrive...
return;
}
// reset flags, etc.
reset();
// skip some frames?!?
_frameCount++;
if (_frameCount < _stackFrameNum) {
return;
}
_frameCount = 0;
pcl::PointXYZI pointSel;
// relate incoming data to map将相关坐标转移到世界坐标系下->得到可用于建图的Lidar坐标
transformAssociateToMap();
// 将上一时刻所有边特征转到世界坐标系下
size_t laserCloudCornerLastNum = _laserCloudCornerLast->points.size();
for (int i = 0; i < laserCloudCornerLastNum; i++) {
pointAssociateToMap(_laserCloudCornerLast->points[i], pointSel);
_laserCloudCornerStack->push_back(pointSel);
}
// 将上一时刻所有面特征转到世界坐标系下
size_t laserCloudSurfLastNum = _laserCloudSurfLast->points.size();
for (int i = 0; i < laserCloudSurfLastNum; i++) {
pointAssociateToMap(_laserCloudSurfLast->points[i], pointSel);
_laserCloudSurfStack->push_back(pointSel);
}
2.优化处理,先看看论文介绍：
先把之前的点云保存在10m*10m*10m的立方体中，若cube中的点与当前帧中的点云有重叠部分就把他们提取出来保存在KD树中。我们找地图中的点时，要在特征点附近宽为10cm的立方体邻域内搜索（实际代码中是10cm×10cm×5cm）。代码如下：
_laserCloudCenWidth(10),//搜索邻域宽度, cm为单位,
_laserCloudCenHeight(5),// 搜索邻域高度
_laserCloudCenDepth(10),// 搜索邻域深度
_laserCloudWidth(21),//子cube沿宽方向的分割个数，每个子cube 50mm =1m/20
_laserCloudHeight(11), // 高方向个数 50mm
_laserCloudDepth(21),// 深度方向个数 50mm
_laserCloudNum(_laserCloudWidth * _laserCloudHeight * _laserCloudDepth),// 子cube总数
而后我们就要找当前估计的Lidar位姿属于哪个子cube。I、J、K对应了cube的索引。可以看出，当坐标属于[-25,25]时，cube对应与(10,5,10)即正中心的那个cube。
pcl::PointXYZI pointOnYAxis;//当前Lidar坐标系{L}y轴上的一点(0,10,0)
pointOnYAxis.x = 0.0;
pointOnYAxis.y = 10.0;
pointOnYAxis.z = 0.0;
pointAssociateToMap(pointOnYAxis, pointOnYAxis);//转到世界坐标系{W}下
// cube中心位置索引
int centerCubeI = int((_transformTobeMapped.pos.x() + 25.0) / 50.0) + _laserCloudCenWidth;
int centerCubeJ = int((_transformTobeMapped.pos.y() + 25.0) / 50.0) + _laserCloudCenHeight;
int centerCubeK = int((_transformTobeMapped.pos.z() + 25.0) / 50.0) + _laserCloudCenDepth;
if (_transformTobeMapped.pos.x() + 25.0 < 0) centerCubeI--;
if (_transformTobeMapped.pos.y() + 25.0 < 0) centerCubeJ--;
if (_transformTobeMapped.pos.z() + 25.0 < 0) centerCubeK--;
//如果取到的子cube在整个大cube的边缘则将点对应的cube的索引向中心方向挪动一个单位，这样做主要是截取边沿cube。
while (centerCubeI < 3) {// 将点的指针向中心方向平移
for (int j = 0; j < _laserCloudHeight; j++) {
for (int k = 0; k < _laserCloudDepth; k++) {
for (int i = _laserCloudWidth - 1; i >= 1; i--) {
const size_t indexA = toIndex(i, j, k);
const size_t indexB = toIndex(i-1, j, k);
std::swap( _laserCloudCornerArray[indexA], _laserCloudCornerArray[indexB] );
std::swap( _laserCloudSurfArray[indexA], _laserCloudSurfArray[indexB]);
}
}
}
centerCubeI++;
_laserCloudCenWidth++;
}
后面类似，处理完毕边沿点，接下来就是在取到的子cube的5*5*5的邻域内找对应的配准点了。
_laserCloudValidInd.clear();
_laserCloudSurroundInd.clear();
for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++) {
for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++) {
for (int k = centerCubeK - 2; k <= centerCubeK + 2; k++) {
if (i >= 0 && i < _laserCloudWidth &&
j >= 0 && j < _laserCloudHeight &&
k >= 0 && k < _laserCloudDepth) {
// 计算子cube对应的点坐标，由于ijk均为整数，坐标取值为中心点坐标
float centerX = 50.0f * (i - _laserCloudCenWidth);
float centerY = 50.0f * (j - _laserCloudCenHeight);
float centerZ = 50.0f * (k - _laserCloudCenDepth);
pcl::PointXYZI transform_pos = (pcl::PointXYZI) _transformTobeMapped.pos;
// 取邻近的8个点坐标
bool isInLaserFOV = false;
for (int ii = -1; ii <= 1; ii += 2) {
for (int jj = -1; jj <= 1; jj += 2) {
for (int kk = -1; kk <= 1; kk += 2) {
pcl::PointXYZI corner;
corner.x = centerX + 25.0f * ii;
corner.y = centerY + 25.0f * jj;
corner.z = centerZ + 25.0f * kk;
float squaredSide1 = calcSquaredDiff(transform_pos, corner);//计算位姿到cube中心的距离
float squaredSide2 = calcSquaredDiff(pointOnYAxis, corner);
float check1 = 100.0f + squaredSide1 - squaredSide2
- 10.0f * sqrt(3.0f) * sqrt(squaredSide1);//??
float check2 = 100.0f + squaredSide1 - squaredSide2
+ 10.0f * sqrt(3.0f) * sqrt(squaredSide1);//??
if (check1 < 0 && check2 > 0) {//取FOV中合适的点
isInLaserFOV = true;
}
}
}
}
size_t cubeIdx = i + _laserCloudWidth*j + _laserCloudWidth * _laserCloudHeight * k;
if (isInLaserFOV) {
_laserCloudValidInd.push_back(cubeIdx);
}
_laserCloudSurroundInd.push_back(cubeIdx);
}
}
}
}
这里还需要判断一下该点是否属于当前Lidar的可视范围内，可以根据余弦公式对距离范围进行推导。根据代码中的式子，只要点在x轴±60°的范围内都认为是FOV中的点(作者这么做是因为Lidar里程计的估计结果太不准确了，只能概略的取一个较大的范围)。于是我们就得到了在当前Lidar位置的邻域内有效的地图特征点。我们就不需要对庞大的所有地图点云进行处理了，只需要处理这些邻域cube内的地图特征点即可，可以节省大量的运算资源。为了保证当前帧的点云足够平滑，还对点云进行了滤波处理。
// prepare valid map corner and surface cloud for pose optimization
_laserCloudCornerFromMap->clear();
_laserCloudSurfFromMap->clear();
size_t laserCloudValidNum = _laserCloudValidInd.size();
for (int i = 0; i < laserCloudValidNum; i++) {
*_laserCloudCornerFromMap += *_laserCloudCornerArray[_laserCloudValidInd[i]];
*_laserCloudSurfFromMap += *_laserCloudSurfArray[_laserCloudValidInd[i]];
}
// prepare feature stack clouds for pose optimization将世界坐标系下的当前帧特征点转到当前Lidar坐标系下
size_t laserCloudCornerStackNum2 = _laserCloudCornerStack->points.size();
for (int i = 0; i < laserCloudCornerStackNum2; i++) {
pointAssociateTobeMapped(_laserCloudCornerStack->points[i], _laserCloudCornerStack->points[i]);
}
size_t laserCloudSurfStackNum2 = _laserCloudSurfStack->points.size();
for (int i = 0; i < laserCloudSurfStackNum2; i++) {
pointAssociateTobeMapped(_laserCloudSurfStack->points[i], _laserCloudSurfStack->points[i]);
}
// down sample feature stack clouds对所有当前帧特征点进行滤波处理
_laserCloudCornerStackDS->clear();
_downSizeFilterCorner.setInputCloud(_laserCloudCornerStack);
_downSizeFilterCorner.filter(*_laserCloudCornerStackDS);
size_t laserCloudCornerStackNum = _laserCloudCornerStackDS->points.size();
_laserCloudSurfStackDS->clear();
_downSizeFilterSurf.setInputCloud(_laserCloudSurfStack);
_downSizeFilterSurf.filter(*_laserCloudSurfStackDS);
size_t laserCloudSurfStackNum = _laserCloudSurfStackDS->points.size();
_laserCloudCornerStack->clear();
_laserCloudSurfStack->clear();
做完这些工作以后，我们就有了在当前Lidar所在位置附近的所有地图特征点以及当前帧的点云特征点，后面的工作就是怎么把这两块点匹配在一起！于是再次拿出KD树，来寻找最邻近的5个点。对点云协方差矩阵进行主成分分析：若这五个点分布在直线上，协方差矩阵的特征值包含一个元素显著大于其余两个，与该特征值相关的特征向量表示所处直线的方向；若这五个点分布在平面上，协方差矩阵的特征值存在一个显著小的元素，与该特征值相关的特征向量表示所处平面的法线方向。因此我们可以很轻易的根据特征向量找到直线上两点从而利用论文中点到直线的距离公式构建优化问题。平面特征也是相同的思路。完成了优化问题的构建之后就可以对它进行求解了，求解方法还是L-M迭代。这部分代码与laserOdometry部分的几乎一致。
void LaserMapping::optimizeTransformTobeMapped()
{
if (_laserCloudCornerFromMap->points.size() <= 10 || _laserCloudSurfFromMap->points.size() <= 100) {
return;
}
pcl::PointXYZI pointSel, pointOri, pointProj, coeff;
std::vector<int> pointSearchInd(5, 0);//搜索最近邻的5个点
std::vector<float> pointSearchSqDis(5, 0);
nanoflann::KdTreeFLANN<pcl::PointXYZI> kdtreeCornerFromMap;
nanoflann::KdTreeFLANN<pcl::PointXYZI> kdtreeSurfFromMap;
kdtreeCornerFromMap.setInputCloud(_laserCloudCornerFromMap);
kdtreeSurfFromMap.setInputCloud(_laserCloudSurfFromMap);
Eigen::Matrix<float, 5, 3> matA0;
Eigen::Matrix<float, 5, 1> matB0;
Eigen::Vector3f matX0;
Eigen::Matrix3f matA1;
Eigen::Matrix<float, 1, 3> matD1;
Eigen::Matrix3f matV1;
matA0.setZero();
matB0.setConstant(-1);
matX0.setZero();
matA1.setZero();
matD1.setZero();
matV1.setZero();
bool isDegenerate = false;
Eigen::Matrix<float, 6, 6> matP;
size_t laserCloudCornerStackNum = _laserCloudCornerStackDS->points.size();
size_t laserCloudSurfStackNum = _laserCloudSurfStackDS->points.size();
pcl::PointCloud<pcl::PointXYZI> laserCloudOri;
pcl::PointCloud<pcl::PointXYZI> coeffSel;
for (size_t iterCount = 0; iterCount < _maxIterations; iterCount++) {
laserCloudOri.clear();
coeffSel.clear();
for (int i = 0; i < laserCloudCornerStackNum; i++) {
pointOri = _laserCloudCornerStackDS->points[i];
pointAssociateToMap(pointOri, pointSel);
kdtreeCornerFromMap.nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis );
// 如果5个最近点中最远的距离也小于1m，认为是潜在匹配线段,构建这五个点的(x,y,z)方向的3*3的协方差矩阵，之后根据特征根来判断是否能拟合成直线。判断的方法是最大的特征根大于次大的特征根3倍。如果使用matlab，eig(cov(x,y,z)),其中的x,y,z是点云的各个分量向量。
if (pointSearchSqDis[4] < 1.0) {
//compute mean
Vector3 vc(0,0,0);
for (int j = 0; j < 5; j++) {
vc += Vector3(_laserCloudCornerFromMap->points[pointSearchInd[j]]);
}
vc /= 5.0;
//compute CovarianceMatrix协方差矩阵
Eigen::Matrix3f mat_a;
mat_a.setZero();
for (int j = 0; j < 5; j++) {
Vector3 a = Vector3(_laserCloudCornerFromMap->points[pointSearchInd[j]]) - vc;
// 5个点的协方差矩阵
mat_a(0,0) += a.x() * a.x();
mat_a(0,1) += a.x() * a.y();
mat_a(0,2) += a.x() * a.z();
mat_a(1,1) += a.y() * a.y();
mat_a(1,2) += a.y() * a.z();
mat_a(2,2) += a.z() * a.z();
}
matA1 = mat_a / 5.0;
//compute eigenvalues and eigenvectors计算特征值，特征向量
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> esolver(matA1);
matD1 = esolver.eigenvalues().real();//特征值
matV1 = esolver.eigenvectors().real();//特征向量
//compute corner line and coefficients
if (matD1(0, 0) > 3 * matD1(0, 1)) {//最大特征值是次大特征值的3倍以上
const Eigen::Vector3f &point = pointSel.getVector3fMap();
Eigen::Vector3f largestEigenVect = matV1.row(0);
Eigen::Vector3f centroidMinus = vc.head(3) - largestEigenVect*0.1;
Eigen::Vector3f centroidPlus = vc.head(3) + largestEigenVect*0.1;
pcl::PointXYZI coefficients;
if (getCornerFeatureCoefficients(centroidMinus, centroidPlus, point, coefficients)) {
laserCloudOri.push_back(pointOri);
coeffSel.push_back(coefficients);//计算雅克比矩阵的值,i点所对应的偏导数
}
}
}
}
//面特征采用类似思想，求雅克比矩阵，用LM求tf
for (int i = 0; i < laserCloudSurfStackNum; i++) {
pointOri = _laserCloudSurfStackDS->points[i];
pointAssociateToMap(pointOri, pointSel);
kdtreeSurfFromMap.nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis );
if (pointSearchSqDis[4] < 1.0) {
for (int j = 0; j < 5; j++) {
matA0(j, 0) = _laserCloudSurfFromMap->points[pointSearchInd[j]].x;
matA0(j, 1) = _laserCloudSurfFromMap->points[pointSearchInd[j]].y;
matA0(j, 2) = _laserCloudSurfFromMap->points[pointSearchInd[j]].z;
}
matX0 = matA0.colPivHouseholderQr().solve(matB0);
Eigen::Vector4f planeCoef;
planeCoef(0) = matX0(0, 0);
planeCoef(1) = matX0(1, 0);
planeCoef(2) = matX0(2, 0);
planeCoef(3) = 0;
float norm = planeCoef.norm();
planeCoef(3) = 1;
planeCoef /= norm;
//check if any closet point far away with plane
bool planeValid = true;
for (int j = 0; j < 5; j++) {
float distance = planeCoef.head(3).dot( _laserCloudSurfFromMap->points[pointSearchInd[j]].getVector3fMap()) + planeCoef(3);
if (fabs(distance) > 0.2) {
planeValid = false;
break;
}
}
if (planeValid) {
pcl::PointXYZI coefficients;
if (getSurfaceFeatureCoefficients(planeCoef, pointSel, coefficients)) {
laserCloudOri.push_back(pointOri);
coeffSel.push_back(coefficients);
}
}
}
}
float srx = _transformTobeMapped.rot_x.sin();
float crx = _transformTobeMapped.rot_x.cos();
float sry = _transformTobeMapped.rot_y.sin();
float cry = _transformTobeMapped.rot_y.cos();
float srz = _transformTobeMapped.rot_z.sin();
float crz = _transformTobeMapped.rot_z.cos();
size_t laserCloudSelNum = laserCloudOri.points.size();
if (laserCloudSelNum < 50) {
continue;
}
Eigen::Matrix<float, Eigen::Dynamic, 6> matA(laserCloudSelNum, 6);
Eigen::Matrix<float, 6, Eigen::Dynamic> matAt(6, laserCloudSelNum);
Eigen::Matrix<float, 6, 6> matAtA;
Eigen::VectorXf matB(laserCloudSelNum);
Eigen::VectorXf matAtB;
Eigen::VectorXf matX;
for (int i = 0; i < laserCloudSelNum; i++) {
pointOri = laserCloudOri.points[i];
coeff = coeffSel.points[i];
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;
// matA 是雅克比矩阵，matAt*matA*matX = matAt*matB;
matA(i, 0) = arx;
matA(i, 1) = ary;
matA(i, 2) = arz;
matA(i, 3) = coeff.x;
matA(i, 4) = coeff.y;
matA(i, 5) = coeff.z;
matB(i, 0) = -coeff.intensity;
}
matAt = matA.transpose();
matAtA = matAt * matA;
matAtB = matAt * matB;
matX = matAtA.colPivHouseholderQr().solve(matAtB);// 其中matX是步长，（roll , ptich ,yaw,x,y,z）
if (iterCount == 0) {
Eigen::Matrix<float, 1, 6> matE;
Eigen::Matrix<float, 6, 6> matV;
Eigen::Matrix<float, 6, 6> matV2;
Eigen::SelfAdjointEigenSolver< Eigen::Matrix<float,6, 6> > esolver(matAtA);
matE = esolver.eigenvalues().real();
matV = esolver.eigenvectors().real();
matV2 = matV;
isDegenerate = false;
float eignThre[6] = {100, 100, 100, 100, 100, 100};
for (int i = 5; i >= 0; i--) {
if (matE(0, i) < eignThre[i]) {
for (int j = 0; j < 6; j++) {
matV2(i, j) = 0;
}
isDegenerate = true;
} else {
break;
}
}
matP = matV.inverse() * matV2;
}
if (isDegenerate) {
Eigen::Matrix<float,6, 1> matX2(matX);
matX = matP * matX2;
}
_transformTobeMapped.rot_x += matX(0, 0);
_transformTobeMapped.rot_y += matX(1, 0);
_transformTobeMapped.rot_z += matX(2, 0);
_transformTobeMapped.pos.x() += matX(3, 0);
_transformTobeMapped.pos.y() += matX(4, 0);
_transformTobeMapped.pos.z() += matX(5, 0);
float deltaR = sqrt(pow(rad2deg(matX(0, 0)), 2) +
pow(rad2deg(matX(1, 0)), 2) +
pow(rad2deg(matX(2, 0)), 2));
float deltaT = sqrt(pow(matX(3, 0) * 100, 2) +
pow(matX(4, 0) * 100, 2) +
pow(matX(5, 0) * 100, 2));
if (deltaR < _deltaRAbort && deltaT < _deltaTAbort) {
break;
}
}
transformUpdate();
}
截止到这里，我们就完成了当前帧点云与地图点云的配准，并对Lidar里程计的运动估计结果进行完了优化。更新完成后，我们还需要将当前帧扫描得到的特征点云封装在不同的cube中，并在地图数组中保存。最后就是将各种信息发布出去了。这里需要说明的是，为了保证运行效率环境点云每5帧发布一次。