从零入门激光SLAM（十三）——LeGo-LOAM源码超详细解析3

Lego_Loam包括了Image projection、Feature association、MapOptmization、Transform Fusion四个部分，下面博主将按照算法的逻辑顺序对代码中的重要函数进行讲解。本节是解析Feature association文件中的特征匹配部分。 该专栏的其他章节链接如下 从零入门激光SLAM系列——总目录与更新情况_kiss icp-CSDN博客

一、Feature Extraction

经过上一节的点云特征提取，我们已经获得了稳健的角点和面点如下图所示，下面将介绍如何配准得到位姿Feature Association部分的源码解析 

 1. Feature Extraction特征提取
// 1.1 点云运动补偿
        adjustDistortion();
// 1.2 计算平滑度
        calculateSmoothness();
// 1.3 标记遮挡点
        markOccludedPoints();
// 1.4 提取特征
        extractFeatures();
// 1.5 发布点云
        publishCloud();
2. Feature Association特征匹配
	if (!systemInitedLM) {
			// 2.1 检查系统初始化，点云投影到结束点
				checkSystemInitialization();
				return;}
// 2.2 更新初始猜测位置
        updateInitialGuess();
// 2.3 特征匹配，输出Transformation
        updateTransformation();
// 2.4 融合IMU坐标Transformation
        integrateTransformation();

二、函数解析

2.1 publishCloudsLast

• 作用：将当前帧点云投影到结束点并发布，作为下一帧匹配的对象，（匹配之前需要有一个匹配对象点云）
• 输入： cornerPointsSharp角特征
  cornerPointsLessSharp缓角特征
  surfPointsFlat面特征
  surfPointsLessFlat缓面特征 adjustCloud修正点云
• 输出：
  /laser_cloud_corner_last /laser_cloud_surf_last /outlier_cloud_last /laser_odom_to_init
• 代码：

 void publishCloudsLast(){
//把特征点云投影到每帧的结尾时刻
updateImuRollPitchYawStartSinCos();
int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();
for (int i = 0; i < cornerPointsLessSharpNum; i++) {
    TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]); }
    int surfPointsLessFlatNum = surfPointsLessFlat->points.size();
for (int i = 0; i < surfPointsLessFlatNum; i++) {
    TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);}
//用KD树存储当前帧点云，为后续匹配做准备
pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
cornerPointsLessSharp = laserCloudCornerLast;
laserCloudCornerLast = laserCloudTemp;
laserCloudTemp = surfPointsLessFlat;
surfPointsLessFlat = laserCloudSurfLast;
laserCloudSurfLast = laserCloudTemp;
laserCloudCornerLastNum = laserCloudCornerLast->points.size();
laserCloudSurfLastNum = laserCloudSurfLast->points.size();
if (frameCount >= skipFrameNum + 1) {
            frameCount = 0;
//发布上一帧outlier
pubOutlierCloudLast.publish(outlierCloudLast2);
//发布上一帧线特征
pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
//发布上一帧面特征
pubLaserCloudSurfLast.publish(laserCloudSurfLast2);}

下图中红色为前一帧，绿色为当前帧 

2.2 updateInitialGuess

• 作用：把当前IMU数值作为位姿先验
• 输入：IMU数值
• 输出：transformCur位姿矩阵

// 如果IMU有旋转，则更新变换矩阵以反映新的旋
void updateInitialGuess(){
if (imuAngularFromStartX != 0 || imuAngularFromStartY != 0 || imuAngularFromStartZ != 0){transformCur[0] = - imuAngularFromStartY;  // 根据Y轴旋转更新变换矩阵的第一个元素
transformCur[1] = - imuAngularFromStartZ;  // 根据Z轴旋转更新变换矩阵的第二个元素
transformCur[2] = - imuAngularFromStartX;  // 根据X轴旋转更新变换矩阵的第三个元素}
// 如果IMU有移动，则更新变换矩阵以反映新的平移
if (imuVeloFromStartX != 0 || imuVeloFromStartY != 0 || imuVeloFromStartZ != 0)
{transformCur[3] -= imuVeloFromStartX * scanPeriod;  // 根据X轴移动更新变换矩阵的第四个元素
transformCur[4] -= imuVeloFromStartY * scanPeriod;  // 根据Y轴移动更新变换矩阵的第五个元素
transformCur[5] -= imuVeloFromStartZ * scanPeriod;  // 根据Z轴移动更新变换矩阵的第六个元素 }   }

2.3 updateTransformation

• 作用：匹配角和面特征点，计算位姿
• 输入： laserCloudSurfLast surfPointsFlat laserCloudCornerLast cornerPointsSharp
• 输出：transformCur位姿矩阵
• 代码

 void updateTransformation(){
        //检查特征点
        if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)
            return;
//  面特征匹配
        for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {
            laserCloudOri->clear(); 
            coeffSel->clear(); 
            findCorrespondingSurfFeatures(iterCount1); 
         if (laserCloudOri->points.size() < 10)
                continue;
            // 通过面特征的匹配，计算变换矩阵
            if (calculateTransformationSurf(iterCount1) == false)
                break;
        }
// 线特征匹配
        for (int iterCount2 = 0; iterCount2 < 25; iterCount2++) {

            laserCloudOri->clear();
            coeffSel->clear();

            findCorrespondingCornerFeatures(iterCount2); 
            if (laserCloudOri->points.size() < 10)
                continue;
            if (calculateTransformationCorner(iterCount2) == false) 
                break;
        }
    }

2.4 findCorrespondingFeatures

计算残差之前，需要先找到对应点，对于角点找点到最近的直线，对于面点找点到最近的平面，，如下图所示 

2.4.1 findCorrespondingCornerFeatures

• 作用：找到当前角点的对应点
• 输入：laserCloudCornerLast 、cornerPointsSharp
• 输出：coeffSel所有角点的特征匹配集
• 代码 

  

void findCorrespondingCornerFeatures(int iterCount){
 for (int i = 0; i < cornerPointsSharpNum; i++) {
     TransformToStart(&cornerPointsSharp->points[i], &pointSel); //转换到起始点
     tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];
     tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];
                float x0 = pointSel.x;
                float y0 = pointSel.y;
                float z0 = pointSel.z;
                float x1 = tripod1.x;
                float y1 = tripod1.y;
                float z1 = tripod1.z;
                float x2 = tripod2.x;
                float y2 = tripod2.y;
                float z2 = tripod2.z;
         if (s > 0.1 && ld2 != 0) {
                    coeff.x = s * la; 
                    coeff.y = s * lb;
                    coeff.z = s * lc;
                    coeff.intensity = s * ld2; //线外一点到直线的距离
                    laserCloudOri->push_back(cornerPointsSharp->points[i]);
                    coeffSel->push_back(coeff);}

2.4.2 findCorrespondingSurfFeatures

• 作用：找到当前面点的对应面
• 输入：laserCloudCornerLast、surfPointsFlat
• 输出：coeffSel所有面点的特征匹配集
• 代码： 

  

void findCorrespondingSurfFeatures(int iterCount){
TransformToStart(&surfPointsFlat->points[i], &pointSel);
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0) {
// 分别取这3个点(在前一帧的平面点取)
	tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]];  //a
	tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]];  //b
	tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]];  //c
if (s > 0.1 && pd2 != 0) {
	// [x,y,z]是整个平面的单位法量
	// intensity是平面外一点到该平面的距离
	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);

2.5 calculateTransformation

• 作用：优化参数目标
• 输入： coeffSel
• 输出：transformCur位姿参数
• 步骤 损失函数 

  

   待优化变量 

  

   平移旋转矩阵 

  

   通过平移旋转矩阵将点云投影，构建残差方程 

  

   使用列文虎克迭代求解变量 

  

   旋转平移矩阵变化足够小，停止迭代

float deltaR = sqrt(
	pow(rad2deg(matX.at<float>(0, 0)), 2) +
	pow(rad2deg(matX.at<float>(1, 0)), 2) +
	pow(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.1 && deltaT < 0.1) {
	return false;

2.6 integrateTransformation

• 作用：将IMU信息融入到位姿更新当中
• 输入：transformCur transformSum IMU当前数据
• 输出：transformCur transformSum
• 代码

void integrateTransformation(){
	float rx, ry, rz, tx, ty, tz;
	//transformSum 是 IMU累计的变化量 ，0，1，2 分别代表的是 pitch yaw roll 的内容，
	//transformCur 是当前的分量，函数的作用是局部坐标转换成全局坐标
	AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], -transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);
	float x1 = cos(rz) * (transformCur[3] - imuShiftFromStartX) - sin(rz) * (transformCur[4] - imuShiftFromStartY);
	float y1 = sin(rz) * (transformCur[3] - imuShiftFromStartX) + cos(rz) * (transformCur[4] - imuShiftFromStartY);
	float z1 = transformCur[5] - imuShiftFromStartZ;
	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);
	//加入 imu 当前数据 更新位姿
	PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, 
	imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);
	transformSum[0] = rx;
	transformSum[1] = ry;
	transformSum[2] = rz;
	transformSum[3] = tx;
	transformSum[4] = ty;
	transformSum[5] = tz;
}

2.7 publishOdometry

作用：发布位姿信息 输入： transformSum 输出： /laser_odom_to_init 代码

void publishOdometry(){
	//从转角创建四元数
	geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(transformSum[2], -transformSum[0], -transformSum[1]); 
	// rx,ry,rz转化为四元数发布
	laserOdometry.header.stamp = cloudHeader.stamp;
	laserOdometry.pose.pose.orientation.x = -geoQuat.y;
	laserOdometry.pose.pose.orientation.y = -geoQuat.z;
	laserOdometry.pose.pose.orientation.z = geoQuat.x;
	laserOdometry.pose.pose.orientation.w = geoQuat.w;
	laserOdometry.pose.pose.position.x = transformSum[3];
	laserOdometry.pose.pose.position.y = transformSum[4];
	laserOdometry.pose.pose.position.z = transformSum[5];
	pubLaserOdometry.publish(laserOdometry);  //scan与scan的位姿变换
	// laserOdometryTrans 是用于tf广播
	laserOdometryTrans.stamp_ = cloudHeader.stamp;
	laserOdometryTrans.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
	laserOdometryTrans.setOrigin(tf::Vector3(transformSum[3], transformSum[4], transformSum[5]));
	tfBroadcaster.sendTransform(laserOdometryTrans); //发布TF
    }
}

最终发布位姿，格式如下图所示