lego_loam 代码阅读与总结

LeGO-LOAM

Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

这篇论文中主要针对小型嵌入式系统（轮式小车）来优化 LOAM 算法，从而得到高质量的六自由度的位姿。

SLAM 优化 https://www.zhihu.com/question/40829091

Introduction

• LOAM简介：

  • 通过判断点周围的平滑性来提取出角点特征（点与点之间曲率变化很大）和面特征（周围曲率很小），并且其将估计问题分解成两个独立的子问题，分别利用高频率相对低精度和低频率高精度两个线程独立工作来达到整体实时性的效果。

• LeGO-LOAM 主要就是为了解决具有轻量级以及对地面情况的嵌入式轮式小车：

  • 本文针对嵌入式系统优化Loam
  • LeGO-LOAM 主要就是为了解决这些问题，具有轻量级（Light-weighted）以及对地面情况进行优化（Ground-Optimized），相对于原始的 LOAM 改变主要有：
    • 对激光点进行分类，从而排除掉可能来自不可靠特征的点
    • 对地面进行优化，分两步来对位姿估计进行优化：
      • 首先，通过平面特征匹配来的得到：$[t_z,{\theta }_{roll},{\theta }_{pitch}]$，
      • 再通过角点特征匹配估计：$[t_x,t_z,{\theta }_{yaw}]$
    • 加入回环检测来修正偏移
  • 这篇论文中主要针对小型嵌入式系统（轮式小车）来优化 LOAM 算法，从而得到高质量的六自由度的位姿。对轮式小车而言，位姿估计的难点主要在于：
    • 小型嵌入式系统的算力通常不足，很难达到实时性计算的要求
    • 小车在不同平面运动时位姿的变化曲线不会很平滑，因此获取的点云数据会存在一定畸变
    • 小车快速运动时，两帧点云之间的覆盖率可能不高，给特征匹配造成一定困难
  • 除此之外，在小车使用 LOAM 算法还会有以下问题：
    • 小车的算力不足，在使用 LOAM 时，特征匹配很难达到实时性
    • 由于小车（以及配备的雷达）比较靠近地面，因此地面上的一些测量误差会经常出现，例如草地中的草附近由于环境比较复杂会被识别成角点特征，或者叶子被识别成平面特征等等。这些不可靠的特征会很大程度上影响匹配的精度

System

• 系统从激光雷达以 10 Hz 的频率得到激光点云，再以同样频率输出六自由度位姿。系统整体分为 5 个模块：

  • Segmentation：接收点云并将其投影至一个距离图像中进行识别

  • Feature Extraction：对识别好的点云进行角点和平面点的提取

  • Lidar Odometry：利用提取出的特征进行前后两帧点云位姿之间的变化

  • Lidar Mapping：同样接受提取出的特征并将其插入到全局点云地图中

  • Transform Integration：将里程计和建图模块输出的位姿进行融合来生成最终位姿

• 核心点：点云分割

  • 设$P_t=(p_1,p_2,\cdots ,p_n)$ 是 t 时刻获取的点云，点云首先被投影到距离图像上，图像分辨率为 $1800\times 16$（VLP-16 具有水平和垂直角分辨率分别为 0.2°和2°），$P_t$中的每个有效点在图像中占据一个像素，每个点的像素值为该点到传感器的欧氏距离。
  • 然后对图像中每一列距离评估来识别出属于地面的点（具体的做法是在每一列中选取一系列相邻点，计算其连线的俯仰角，如果小于一定阈值则考虑是地面），这一步识别出来的地面点被被单独归类不会进行后续的识别。
  • 对图像中剩余的部分利用图像识别算法分割成不同组（cluster），每个组被分配一个单独的标签，（地面也是一个标签）。
  • 将图像中小于 30 个像素点的点云组忽略，从而过滤掉一些不可靠的特征，例如树叶和草等等。经过过滤之后，剩余的点表示了一些较大的特征，例如树干、墙面等等。对于剩余的这些点，系统会利用他们的三个参数：标签（地面点或者已分类点）、在距离图像中的行和列以及它的距离。

• 平坦角度计算：

Node and topics

整体节点示意图：

ImageProjection

该篇最大特点是 点云的处理，因此为核心

• 激光回调函数为：

  // ros 点云转换为 pcl 格式
  copyPointCloud(laserCloudMsg);
  // 计算激光的起始角度(在图像上，从左到右变顺时针了)
  findStartEndAngle();
  // 确定每个点 行列值
  projectPointCloud();
  // 地面移除
  groundRemoval();
  // 点云分割
  cloudSegmentation();
  // 发布点云
  publishCloud();
  resetParameters();
  

projectPointCloud

• 遍历所有激光点，对每个点做如下操作：
  • 计算当前点的垂直角度，$\tan 2(z,\sqrt{x^2+y^2})$
  • 计算当前点是水平方向上第几线的： rowIdn = (verticalAngle + ang_bottom) / ang_res_y
  • 计算当前点的水平角度，$\tan 2(x,y)$
  • 计算当前点是竖直方向上第几线的：columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2
  • 记录行列 ： (float)rowIdn + (float)columnIdn / 10000.0;
  • 记录index：index = columnIdn + rowIdn * Horizon_SCAN;

groundRemoval

• 遍历 水平方向 点，[0,1800]
  • 在遍历 垂直方向，[0,7] ，(水平安装时再往上也不可能地面了)
    • 若上下两层 任意一个无点时，标记非地面点 return
    • 由上下两线确定是否为地面
      • 上下两点的 垂直距离z，与水平距离 xy欧式，的夹角
      • 如果俯仰角在10度以内，则判定(i,j)为地面点
      • 否则，非地面，跳过进行后续
• 找到所有点中的地面点，并将他们标记为-1，labelMat(i,j)=-1
  • 地面点 或 rangeMat==FLT_MAT都标记
  • FLT_MAT 代表该点未被赋值
• 如果有节点订阅groundCloud，那么就需要把地面点发布出来

cloudSegmentation

/**
 * @brief 分割点云
 */
void cloudSegmentation(){
    
    // 按照列行遍历 每个点云
    for (size_t i = 0; i < N_SCAN; ++i)
        for (size_t j = 0; j < Horizon_SCAN; ++j)
            // 如果labelMat[i][j]=0,表示没有对该点进行过分类，需要对该点进行聚类
            if (labelMat.at<int>(i,j) == 0)
                labelComponents(i, j);

    int sizeOfSegCloud = 0;
    for (size_t i = 0; i < N_SCAN; ++i) {

        // segMsg.startRingIndex[i]，segMsg.endRingIndex[i]
        // 表示第i线的点云起始序列和终止序列，开始4点和末尾6点舍去不要
        segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;

        for (size_t j = 0; j < Horizon_SCAN; ++j) {
            // 找到可用的 特征点 或者 地面点
            if (labelMat.at<int>(i,j) > 0 || groundMat.at<int8_t>(i,j) == 1){
                
                // labelMat数值为999999表示这个点是因为聚类数量不够30而被舍弃的点
                // 需要舍弃的点直接continue跳过本次循环，
                // 当列数为5的倍数，并且行数较大，可以认为非地面点的，将它保存进异常点云(界外点云)中
                // 然后再跳过本次循环
                if (labelMat.at<int>(i,j) == 999999){
                    if (i > groundScanInd && j % 5 == 0){
                        outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                        continue;
                    }else{
                        continue;
                    }
                }
                // 如果是地面点,对于列数不为5的倍数的，直接跳过不处理
                if (groundMat.at<int8_t>(i,j) == 1){
                    if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
                        continue;
                }
                // 上面多个if语句已经去掉了不符合条件的点，这部分直接进行信息的拷贝和保存操作
                // 保存完毕后sizeOfSegCloud递增
                segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
                segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
                segMsg.segmentedCloudRange[sizeOfSegCloud]  = rangeMat.at<float>(i,j);
                segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                ++sizeOfSegCloud;
            }
        }

        segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
    }

    // 如果有节点订阅SegmentedCloudPure,
    // 那么把点云数据保存到segmentedCloudPure中去
    if (pubSegmentedCloudPure.getNumSubscribers() != 0){
        for (size_t i = 0; i < N_SCAN; ++i){
            for (size_t j = 0; j < Horizon_SCAN; ++j){
                // 需要选择不是地面点(labelMat[i][j]!=-1)和没被舍弃的点
                if (labelMat.at<int>(i,j) > 0 && labelMat.at<int>(i,j) != 999999){
                    segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                    segmentedCloudPure->points.back().intensity = labelMat.at<int>(i,j);
                }
            }
        }
    }
}

labelComponents

• 对该点云基于平坦度聚类，若聚类后点个数大于30 或竖直方向点个数大于5，则聚类成功，否则将该组聚类点标记为 无效点。
• 平面点，外点，地面 3类

// 图像中的 每一行 每一列  的 index
void labelComponents(int row, int col){
    float d1, d2, alpha, angle;
    int fromIndX, fromIndY, thisIndX, thisIndY; 
    
    // 行标记
    bool lineCountFlag[N_SCAN] = {false};

    // queueIndX、allPushedIndX 有一个就够了啊
    // 队列下标： 行，列 
    queueIndX[0] = row;
    queueIndY[0] = col;
    
    // 队列的个数
    int queueSize = 1;
    
    int queueStartInd = 0;
    int queueEndInd = 1;
	
    // 放入队列的下表
    allPushedIndX[0] = row;
    allPushedIndY[0] = col;
    int allPushedIndSize = 1;

   
    while(queueSize > 0){  // 队列个数不为空时执行
        
        // 取第 0 个队列的下标
        fromIndX = queueIndX[queueStartInd];
        fromIndY = queueIndY[queueStartInd];
        
        --queueSize;   // 查询队列 -1
        ++queueStartInd;  // 队列的开始下表 加1
        
        // labelCount的初始值为1，后面会递增
        labelMat.at<int>(fromIndX, fromIndY) = labelCount;

        // neighbor=[[-1,0];[0,1];[0,-1];[1,0]]
        // 遍历点[fromIndX,fromIndY]边上的四个邻点
        for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
			// 周围某点的 坐标
            thisIndX = fromIndX + (*iter).first;
            thisIndY = fromIndY + (*iter).second;
			
            // 竖直不在该图像上 跳过
            if (thisIndX < 0 || thisIndX >= N_SCAN)
                continue;
			
            // 水平超出阈值时  首尾相连
            if (thisIndY < 0)
                thisIndY = Horizon_SCAN - 1;
            if (thisIndY >= Horizon_SCAN)
                thisIndY = 0;

            // 如果点[thisIndX,thisIndY]已经标记过
            // labelMat中，-1代表地面点，0代表未进行标记过，其余为其他的标记
            // 如果当前的邻点已经标记过，则跳过该点。不是地面点为什么跳过???
            // 如果labelMat已经标记为正整数，则已经聚类完成，不需要再次对该点聚类
            if (labelMat.at<int>(thisIndX, thisIndY) != 0)
                continue;

            // 取当前点和周围点的 最大最小距离
            d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), 
                          rangeMat.at<float>(thisIndX, thisIndY));
            d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), 
                          rangeMat.at<float>(thisIndX, thisIndY));

            // alpha代表角度分辨率，
            // X方向上角度分辨率是segmentAlphaX(rad)
            // Y方向上角度分辨率是segmentAlphaY(rad)
            if ((*iter).first == 0)
                alpha = segmentAlphaX;
            else
                alpha = segmentAlphaY;

            // 通过下面的公式计算这两点之间是否有平面特征
            // atan2(y,x)的值越大，d1，d2之间的差距越小,越平坦
            angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));

            // segmentTheta=1.0472<==>60度
            // 如果算出角度大于60度，则假设这是个平面
            if (angle > segmentTheta){
				
                // 当前点放入队列，队列个数加1，队列结束加1
                queueIndX[queueEndInd] = thisIndX; 
                queueIndY[queueEndInd] = thisIndY;
                ++queueSize;
                ++queueEndInd;

                // 当前点标记设为 labelCount
                labelMat.at<int>(thisIndX, thisIndY) = labelCount;
                lineCountFlag[thisIndX] = true;

                // allput更新
                allPushedIndX[allPushedIndSize] = thisIndX;
                allPushedIndY[allPushedIndSize] = thisIndY;
                ++allPushedIndSize;
            }
        }
    }


    bool feasibleSegment = false;

    // 如果聚类超过30个点，直接标记为一个可用聚类，labelCount需要递增
    if (allPushedIndSize >= 30)
        feasibleSegment = true;
    else if (allPushedIndSize >= segmentValidPointNum){
        // 如果聚类点数小于30大于等于5，统计竖直方向上的聚类点数
        int lineCount = 0;
        for (size_t i = 0; i < N_SCAN; ++i)
            if (lineCountFlag[i] == true)
                ++lineCount;

        // 竖直方向上超过3个也将它标记为有效聚类
        if (lineCount >= segmentValidLineNum)
            feasibleSegment = true;            
    }

    if (feasibleSegment == true){
        ++labelCount;
    }else{
        for (size_t i = 0; i < allPushedIndSize; ++i){
            // 标记为999999的是需要舍弃的聚类的点，因为他们的数量小于30个
            labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
        }
    }
}

featureAssociation

主函数：

• 首先构造了 FeatureAssociation对象

• 其次该主函数与其他不同，它使用了 ros::spin0nce()

• 按照200hz的频率 一直循环查询

• 调用了 runFeatureAssociation()

Callback

三种点云都未使用 互斥锁

• 因为回调函数，只有spinOnce执行时调用一次

laserCloudHandler

• topic：segmented_cloud 1
• 描述：主要是被分割点和经过降采样的地面点,其 intensity 记录了该点在深度图上的索引位置
• 数据转换为 pcl后，数据赋值： segmentedCloud

laserCloudInfoHandler

• topic：segmented_cloud_info 1
• 数据赋值：segInfo

outlierCloudHandler

• topic：outlier_cloud 1
• 数据转换为 pcl后，数据赋值：outlierCloud

imuHandler

• topic：imuTopic 50

• imu 数据 去重力，并进行坐标转换

  • 接收 imu 角度四元素得到： roll,pitch,yaw

  • 基于 roll,pitch 将加速度的重力方向去除，并进行坐标转换

    x = y y=z z=x

• 将欧拉角，加速度，速度保存到循环队列中：

  • 时间、角度位置、加速度、速度
  • 循环队列不需要一直申请内存，感觉用deque更好

• 对速度，角速度，加速度进行积分，得到位移，角度和速度 AccumulateIMUShiftAndRotation

runFeatureAssociation

• 数据时间判别：
  • 三种点云数据都到了，且3者时间相差很小时，则符合条件，向下执行
  • 否则直接 return
• adjustDistortion() 去畸变
• calculateSmoothness() 点云光滑度计算，并保存结果
  • 基于左右5个点计算 平滑度，跟loam一样
• markOccludedPoints() 标记阻塞点
  • 跟loam 一样，具体见 loam
• extractFeatures() 特征提取，然后分别保存到其队列中去
  • 跟loam 一样，具体见 loam
• publishCloud() 发布cornerPointsSharp等4种类型的点云数据
• 若系统未初始化 LM
  • checkSystemInitialization()
• updateInitialGuess() 预测位姿
  • 角度：来自imu 角度
  • 位移：imu 速度乘以时间，当前变换中的位移
• updateTransformation() 更新变换
• integrateTransformation() 积分总变化
• 发布里程计和点云
  • publishOdometry()
  • publishCloudsLast()

adjustDistortion()

• 遍历所有激光点，做如下操作：

  • 坐标转换，跟laboshin_loam代码中的一样经过坐标轴变换

    • point.x = y point.y = z point.z =x

    • 计算偏航角 yaw： -atan2(p.x,p.z)==>-atan2(y,x)，

      • ori表示的是偏航角yaw，因为前面有负号，ori=[-M_PI,M_PI)
      • 调整ori大小，满足start
      • 没有转过一半，但是start-ori>M_PI/2
      • 没有转过一半，但是ori-start>3/2*M_PI,说明ori太大，不合理（正常情况在前半圈的话，ori-start范围[0,M_PI]）
      • 转过一半，end-ori>3/2*PI，ori太小
      • 转过一半，ori-end>M_PI/2，太大

• 计算与起始点的时间差 relTime

• 利用imu去畸变，先时间轴对齐，后进行去畸变

  • 数据同步
    • 该条件内imu数据比激光数据早，但是没有更后面的数据
    • 在imu数据充足的情况下可以进行插补
  • 更新 R,P,Y
    • 该条件内imu数据比激光数据早，但是没有更后面的数据时，直接赋值当前数据
    • 否则，进行差值计算
    • 更新数据 updateImuRollPitchYawStartSinCos

• 去完畸变赋值

updateTransformation

• 若 角特征小于10 或 面特征小于 100时直接return
• 循环迭代求解 面特征 对应的方向
  • 找到对应的特征平面 findCorrespondingSurfFeatures
  • 若找到的平面点 个数＜10时，跳过
  • 通过面特征的匹配，计算变换矩阵 calculateTransformationSurf
• 循环迭代求解 角特征 对应的方向
  • 找到对应的特征边/角点 findCorrespondingCornerFeatures
  • 若找到的平面点 个数＜10时，跳过
  • 通过角特征的匹配，计算变换矩阵 calculateTransformationCorner

integrateTransformation

• 将局部旋转坐标转换至全局旋转坐标 AccumulateRotation
• 转移到世界坐标系下
• 插入imu旋转，更新位姿 PluginIMURotation
• 世界坐标系数据赋值

feature_relative

findCorrespondingSurfFeatures

• 遍历平面特征，计算dist，法向量等

  • 坐标变换到开始时刻，TransformToStart

  • 若迭代次数对5取余为0，找该特征最近的3个平面特征

    • 找临近点，作为第一个点
    • 临近点往上，作为第二个点
    • 临近点往前找，作为第三个点

  • 计算法向量和误差距离，影响因子

    • 法向量 上述3个点形成的平面
    • 距离，点到平面的距离
    • 影响因子，s=距离/该点长度 ，长度固定

  • 未经变换的点放入laserCloudOri队列，距离，法向量值放入coeffSel

findCorrespondingCornerFeatures

• 遍历角点特征，计算dist，法向量等
  • 坐标变换到开始时刻，TransformToStart
  • 若迭代次数对5取余为0，找该特征最近的2个角点特征
    • 找临近点，作为第一个点
    • 临近点往上，作为第二个点
    • 临近点往下，作为第三个点
    • 第二第三那个距离特征点近，作为最终的第二个点
  • 计算点到直线的距离，tripod即三角形，根据三角形余弦定理计算距离并求偏导数
    • 可以查看我 loam推导 Loam 回顾
  • 未经变换的点放入laserCloudOri队列，距离，法向量值放入coeffSel

calculateTransformationCorner

• 感觉跟loam 一样，故省略

mapOptimization

打印时加 \033[1;32m---->\033[0m 可以显示颜色，跟python一样

主函数：

• 初始化 node节点
• 申明 mapOptimization 对象
• 创建 闭环线程 loopClosureThread
• 创建 显示线程 visualizeGlobalMapThread
• 200hz 的回调与查询
  • rosspinOnce(200)
  • 执行：run()

Construct

• 订阅话题：
  • 话题：laser_cloud_corner_last ，回调： laserCloudCornerLastHandler()
  • 话题：laser_cloud_surf_last ，回调： laserCloudSurfLastHandler()
  • 话题：outlier_cloud_last ，回调： laserCloudOutlierLastHandler()
  • 话题：laser_odom_to_init ，回调： laserOdometryHandler()
  • 话题：imuTopic ，回调： imuHandler()
• 发布话题：
  • 关键帧位姿、激光平面点、里程计、历史关键帧、icp关键帧、最近关键帧
• 定义一些数据
  • 降采样对象
  • 重置内存

Callback

laserCloudOutlierLastHandler

• 更新其时间
• 通过 pcl 数据转换后，更新数据
• 更新 新数据到了标记

laserCloudCornerLastHandler

• 更新其时间
• 通过 pcl 数据转换后，更新数据
• 更新 新数据到了标记

laserCloudSurfLastHandler

• 更新其时间
• 通过 pcl 数据转换后，更新数据
• 更新 新数据到了标记

laserOdometryHandler

• 更新激光里程计 坐标系转换有点意思
  • transformSum rpy xyz
• 更新完成后 更新 新数据到了标记

imuHandler

• 循环队列，只更新 time，roll，pitch

loopClosureThread

闭环线程

• 若不是能闭环线程时，直接 return

  • loopClosureEnableFlag == false

• while 循环，1s周期

  • 执行：performLoopClosure()

performLoopClosure()

• 因子图中位姿为空时，直接return
• 若 无潜在的闭环时，potentialLoopFlag == false
  • 若有闭环检测时，detectLoopClosure()== true
    • 存在潜在闭环检测 potentialLoopFlag = true
  • 若 无潜在的闭环时，直接return
• 构造ICP匹配器
  • pcl::IterativeClosestPoint，最大距离100，最大迭代100,
  • Epsilon,trans=1e-6,rotate=1e-6 ransac 0
  • icp source：最近平面关键帧
  • icp target：最近历史关键帧（降采样之后的）
• 求解，并评判
  • 求解： icp.align(*unused_result);
  • 评判： 未收敛 或 匹配误差大于阈值(0.3) ，则return
• 得到 平移和旋转
  • icp.getFinalTransformation();
  • 转换坐标系 ，因为 点云是在 相机坐标系的，转换为世界坐标系
  • 匹配误差赋值协方差 noiseModel::Diagonal::Variances(Vector6)
• 更新 因子图数据
  • ISAM2 对象 isam->update(gtSAMgraph);
  • isam->update();
• 有闭环为 true

detectLoopClosure

• 找出机器人位姿临近的关键帧

  • 将系统中所有关键帧位姿放入 kdTree中
  • 通过kdTree找出临近的关键帧，半径5m

• 找到最近历史帧ID

  • 临近关键帧基于到位姿最近距离排序
  • 遍历 临近关键帧，找出超30s的第一个帧
    • 若两时间差值＞30s，则break

• 若 无最近历史帧，则return

• 取系统中最近关键帧点云

  • 取系统中最近关键帧，将角点和平面点基于最近关键帧位姿转换到世界坐标系
  • 将世界坐标系的点云 相加放到 latestSurfKeyFrameCloud

• 取临近历史帧周围的帧的特征点组成submap

  • 临近历史帧周围 [-25,25]，若其超出 系统帧个数则return
  • 将每个帧的特征点 按照各自位姿转换到世界坐标系中，并存于nearHistorySurfKeyFrameCloud

• 历史帧submap 进行降采样 [0.4,0.4,0.4]

• 若有订阅，则发布 历史帧submap

run

• 都有新帧 且 各个激光数据与里程计时间间隔 ＜ 5ms 时，才执行，否则return

• 距离上次 因子图优化 <300ms时，直接 return

• 执行：

  // 将坐标转移到世界坐标系下,得到可用于建图的Lidar坐标，即修改了transformTobeMapped的值
  transformAssociateToMap();
  
  // 提取周围关键帧点，也可能是回环的
  extractSurroundingKeyFrames();
  
  // 降采样当前帧
  downsampleCurrentScan();
  
  // 优化求解
  scan2MapOptimization();
  
  // 保存关键帧
  saveKeyFramesAndFactor();
  
  correctPoses();
  
  publishTF();
  
  publishKeyPosesAndFrames();
  
  clearCloud();
  

extractSurroundingKeyFrames

• 若 因子图中 无帧时直接 return
  • cloudKeyPoses3D->points.empty() == true
• 若使能闭环线程时：
  • 若 recentCornerCloudKeyFrames保存的点云数量小于预设值时
    • 清空后重新塞入新的点云直至数量够
    • 角点，平面点，外点 三种特征点
  • 否则，recentCornerCloudKeyFrames数量符合要求
    • 若 latestFrameID 与 因子图中最新帧 Id不一样时：
      • pop队列最前端的一个，然后在push后面一个
      • push的为：因子图中最新ID对应的
  • 将 最近帧 对应的三种特征点都 相加起开
    • 角点，平面点，外点
    • 这三种点都在世界坐标系下
• 若未使能闭环线程时：
  • 找出机器人位姿临近的关键帧
    • 将系统中所有关键帧位姿放入 kdTree中
    • 通过kdTree找出临近的关键帧，半径5m
  • 将临近帧点云 一起叠加到 surroundingKeyPoses，并进行降采样
  • 遍历已存在的关键帧，在遍历周围关键帧，删除无效数据
    • 双重循环，不断对比surroundingExistingKeyPosesID[i]和surroundingKeyPosesDS的点的index
    • 如果能够找到一样的，说明存在相同的关键点(因为surroundingKeyPosesDS从cloudKeyPoses3D中筛选而来)
    • 如果surroundingExistingKeyPosesID[i]对比了一轮的已经存在的关键位姿的索引后（intensity保存的就是size()）
    • 没有找到相同的关键点，那么把这个点从当前队列中删除
    • 否则的话，existingFlag为true，该关键点就将它留在队列中
  • 上一个两重for循环主要用于删除数据，此处的两重for循环用来添加数据
    • 把surroundingExistingKeyPosesID内没有对应的点放进一个队列里
    • 这个队列专门存放周围存在的关键帧，但是和surroundingExistingKeyPosesID的点没有对应的，也就是新的点
  • 将存在对应关系涉及到的 三种特征各自加到一起
• 进行两次下采样，最后的输出结果是laserCloudCornerFromMapDS和laserCloudSurfFromMapDS

scan2MapOptimization

• 要求当前帧特征点数量足够多，且匹配的点数够多，才执行优化
• 将上一步提取的地图放入kdTree中，方便查找最近点，迭代10次优化
  • 当前激光帧角点寻找局部map匹配点 cornerOptimization()
    • 更新当前帧位姿，(与哪个地图匹配需转到哪个地图)
    • 将当前帧角点坐标变换到map系下，在局部map中查找5个最近点，距离小于1m，且5个点构成直线，则认为匹配上了（用距离中心点的协方差矩阵，特征值进行判断）
    • 计算当前帧角点到直线的距离、垂线的单位向量，存储为角点参数
  • 当前激光帧平面点寻找局部map匹配点 surfOptimization()
    • 更新当前帧位姿，将当前帧平面点坐标交换到map系下，在局部map中查找5个最近点，距离小于1米，且5个点构成平面（最小二乘拟合平面），则认为匹配上了
    • 计算当前帧平面点到平面的距离、垂线的单位向量，存储为平面点参数
  • 提取当前帧中与局部map匹配上了的角点、平面点，加入同一集合 combineOptimizationCoeffs()
  • LM迭代优化 LMOptimization 对匹配特征点计算Jacobian矩阵，观测值为特征点到直线、平面的距离，构建高斯牛顿方程，迭代优化当前位姿，存transformTobeMapped
    • 基于特征值大小评判该方向是否可观，进而评判是否优化该方向
• 优化结果更新 transformUpdate ，用imu原始RPY数据与scan-to-map优化后的位姿进行加权融合，更新当前帧位姿的roll pirch 约束z坐标

saveKeyFramesAndFactor

• 当前帧位姿与 因子图最新位姿相差小于 0.3 时，return
• 若因子图中无帧时，添加该帧的角度，位置0
  • 无帧时当前帧即为世界坐标系
• 若有帧时，添加该帧
• 更新求解器
  • isam->update(gtSAMgraph, initialEstimate);
  • isam->update();
• 根据上次更新期间计算的不完整线性增量计算估计值
  • 通过 isam得到 当前估计值
• 将求解器得到的数据添加到 系统关键帧中
  • 位姿
  • 点云

TransformFusion

融合激光里程计 和 地图匹配的结果

主函数：

• 初始化 ros节点
• 申明 TransformFusion 对象

TransformFusion

• 回调
  • 激光里程计数据，话题：laser_odom_to_init， laserOdometryHandler
  • 地图优化后的数据，话题：aft_mapped_to_init， odomAftMappedHandler
• 发布topic的定义
  • /integrated_to_init"(nav_msgs::Odometry)

odomAftMappedHandler

• 通过odomAftMappedHandler函数获取精配准后的位姿作为transformAftMapped，而获取配准后的速度信息作为transformBefMapped准备下一次计算。

void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped)
{
    double roll, pitch, yaw;
    geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation;
    tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);

    //位姿作为计算的基础
    transformAftMapped[0] = -pitch;
    transformAftMapped[1] = -yaw;
    transformAftMapped[2] = roll;

    transformAftMapped[3] = odomAftMapped->pose.pose.position.x;
    transformAftMapped[4] = odomAftMapped->pose.pose.position.y;
    transformAftMapped[5] = odomAftMapped->pose.pose.position.z;

    //速度作为下一次计算的先验
    transformBefMapped[0] = odomAftMapped->twist.twist.angular.x;
    transformBefMapped[1] = odomAftMapped->twist.twist.angular.y;
    transformBefMapped[2] = odomAftMapped->twist.twist.angular.z;

    transformBefMapped[3] = odomAftMapped->twist.twist.linear.x;
    transformBefMapped[4] = odomAftMapped->twist.twist.linear.y;
    transformBefMapped[5] = odomAftMapped->twist.twist.linear.z;
}

laserOdometryHandler

• laserOdometryHandler是将粗配准的里程计信息与精配准的里程计信息融合计算，并在回调函数中便发送了最终外发的里程计话题。在该回调函数中的TF与里程计话题才是最终决定的。

void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{
    currentHeader = laserOdometry->header;

    double roll, pitch, yaw;
    geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
    tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);

    transformSum[0] = -pitch;
    transformSum[1] = -yaw;
    transformSum[2] = roll;

    transformSum[3] = laserOdometry->pose.pose.position.x;
    transformSum[4] = laserOdometry->pose.pose.position.y;
    transformSum[5] = laserOdometry->pose.pose.position.z;

    //点云坐标转化到世界坐标
    //位姿与速度的融合计算
    transformAssociateToMap();

    geoQuat = tf::createQuaternionMsgFromRollPitchYaw
        (transformMapped[2], -transformMapped[0], -transformMapped[1]);

    laserOdometry2.header.stamp = laserOdometry->header.stamp;
    laserOdometry2.pose.pose.orientation.x = -geoQuat.y;
    laserOdometry2.pose.pose.orientation.y = -geoQuat.z;
    laserOdometry2.pose.pose.orientation.z = geoQuat.x;
    laserOdometry2.pose.pose.orientation.w = geoQuat.w;
    laserOdometry2.pose.pose.position.x = transformMapped[3];
    laserOdometry2.pose.pose.position.y = transformMapped[4];
    laserOdometry2.pose.pose.position.z = transformMapped[5];
    pubLaserOdometry2.publish(laserOdometry2);

    laserOdometryTrans2.stamp_ = laserOdometry->header.stamp;
    laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
    laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5]));
    tfBroadcaster2.sendTransform(laserOdometryTrans2);
}