LEGO LOAM 学习理解总结

参考资料

  • LOAM等内容的博客 需要好好研读
  • LeGO-LOAM 源码阅读笔记(imageProjecion.cpp)
  • LeGO-LOAM 源码阅读笔记(featureAssociation.cpp)
  • LeGO-LOAM 源码阅读笔记(mapOptmization.cpp)

代码理解(ongoing)

  1. image Projection 的原因之一是, 必须找到所有激光点对应的位置。需要将所有激光点投射到16个channel中去,并且每个channel中的激光点总数应该保持一致。激光雷达得到的数据看成一个16x1800的点云阵列

PCL_ADD_POINT4D

  145   union EIGEN_ALIGN16 { \
  146     float data[4]; \
  147     struct { \
  148       float x; \
  149       float y; \
  150       float z; \
  151     }; \
  152   }; \
  144  #define PCL_ADD_POINT4D \
  145   union EIGEN_ALIGN16 { \
  146     float data[4]; \
  147     struct { \
  148       float x; \
  149       float y; \
  150       float z; \
  151     }; \
  152   }; \
  194 #define PCL_ADD_INTENSITY \
  195     struct \
  196     { \
  197       float intensity; \
  198     }; \
  

pcl::PointCloud::Ptr reset

pcl::PointCloud<PointType>::Ptr fullCloud;
laserCloudIn.reset(new pcl::PointCloud<PointType>());

ImageProjection cloudHandler

    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){

        // 1. Convert ros message to pcl point cloud
        copyPointCloud(laserCloudMsg);
        // 2. Start and end angle of a scan
        //    根据节点第一个点和最后一个殿的 x,y,z 推测雷达角度差异
        findStartEndAngle();
        // 3. Range image projection
        //     计算竖直角度,并投射到16个channel中
        projectPointCloud();
        // 4. Mark ground points
        //    由上下两线之间点的XYZ位置得到两线之间的俯仰角,如果俯仰角在10度以内,则判定(i,j)为地面点,
        //    
        groundRemoval();
        // 5. Point cloud segmentation
        //    将不同类型的点云放到不同的点云块中去,例如outlierCloud,segmentedCloudPure等
        cloudSegmentation();
        // 6. Publish all clouds
        publishCloud();
        // 7. Reset parameters for next iteration
        resetParameters();
    }

可以借鉴的 Publish结构

    void publishCloud(){
        // 1. Publish Seg Cloud Info
        segMsg.header = cloudHeader;
        pubSegmentedCloudInfo.publish(segMsg);
        // 2. Publish clouds
        sensor_msgs::PointCloud2 laserCloudTemp;

        pcl::toROSMsg(*outlierCloud, laserCloudTemp);
        laserCloudTemp.header.stamp = cloudHeader.stamp;
        laserCloudTemp.header.frame_id = "base_link";
        pubOutlierCloud.publish(laserCloudTemp);
        // segmented cloud with ground
        pcl::toROSMsg(*segmentedCloud, laserCloudTemp);
        laserCloudTemp.header.stamp = cloudHeader.stamp;
        laserCloudTemp.header.frame_id = "base_link";
        pubSegmentedCloud.publish(laserCloudTemp);
        // projected full cloud
        if (pubFullCloud.getNumSubscribers() != 0){
            pcl::toROSMsg(*fullCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubFullCloud.publish(laserCloudTemp);
        }
        // original dense ground cloud
        if (pubGroundCloud.getNumSubscribers() != 0){
            pcl::toROSMsg(*groundCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubGroundCloud.publish(laserCloudTemp);
        }
        // segmented cloud without ground
        if (pubSegmentedCloudPure.getNumSubscribers() != 0){
            pcl::toROSMsg(*segmentedCloudPure, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubSegmentedCloudPure.publish(laserCloudTemp);
        }
        // projected full cloud info
        if (pubFullInfoCloud.getNumSubscribers() != 0){
            pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "base_link";
            pubFullInfoCloud.publish(laserCloudTemp);
        }
    }

featureAssociation.cpp

  • 它分为特征点的提取与匹配两部分
    void runFeatureAssociation()
    {
        //判断分割点云和异常值的实时性
        if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
            std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
            std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05)
	{
            newSegmentedCloud = false;
            newSegmentedCloudInfo = false;
            newOutlierCloud = false;
        }
	else
	{
            return;
        }
        //进行形状特征的调整
        adjustDistortion();
        calculateSmoothness();
        markOccludedPoints();
        //提取特征
        extractFeatures();
        publishCloud();
        //在配准之前,检验LM法是否初始化,接下来就是里程计部分
        if (!systemInitedLM)
	{
            checkSystemInitialization();
            return;
        }
        //提供粗配准的先验以供优化
        updateInitialGuess();
        //优化并发送里程计信息
        updateTransformation();
        integrateTransformation();
        publishOdometry();
        //发送点云以供建图使用
        publishCloudsLast();
    }

void adjustDistortion()

将点云数据进行坐标变换,进行插补等工作

  • 莫名其妙的坐标轴变换
point.x = segmentedCloud->points[i].y;
point.y = segmentedCloud->points[i].z;
point.z = segmentedCloud->points[i].x;

void calculateSmoothness()

用于计算光滑性

void calculateSmoothness()
{
    int cloudSize = segmentedCloud->points.size();
    for (int i = 5; i < cloudSize - 5; i++) {

        float diffRange = segInfo.segmentedCloudRange[i-5] + segInfo.segmentedCloudRange[i-4]
                        + segInfo.segmentedCloudRange[i-3] + segInfo.segmentedCloudRange[i-2]
                        + segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i] * 10
                        + segInfo.segmentedCloudRange[i+1] + segInfo.segmentedCloudRange[i+2]
                        + segInfo.segmentedCloudRange[i+3] + segInfo.segmentedCloudRange[i+4]
                        + segInfo.segmentedCloudRange[i+5];            

        cloudCurvature[i] = diffRange*diffRange;

        // 在markOccludedPoints()函数中对该参数进行重新修改
        cloudNeighborPicked[i] = 0;
		// 在extractFeatures()函数中会对标签进行修改,
		// 初始化为0,surfPointsFlat标记为-1,surfPointsLessFlatScan为不大于0的标签
		// cornerPointsSharp标记为2,cornerPointsLessSharp标记为1
        cloudLabel[i] = 0;

        cloudSmoothness[i].value = cloudCurvature[i];
        cloudSmoothness[i].ind = i;
    }
}

void extractFeatures()

进行特征抽取,然后分别保存到cornerPointsSharp等等队列中去。 保存到不同的队列是不同类型的点云,进行了标记的工作,这一步中减少了点云数量,使计算量减少。 函数首先清空了cornerPointsSharp,cornerPointsLessSharp,surfPointsFlat,surfPointsLessFlat 然后对cloudSmoothness队列中sp到ep之间的点的平滑数据进行从小到大的排列。 然后按照不同的要求,将点的索引放到不同的队列中去。 另外还对点进行了标记。 最后,因为点云太多时,计算量过大,因此需要对点云进行下采样,减少计算量。 代码如下:

void extractFeatures()
{
    cornerPointsSharp->clear();
    cornerPointsLessSharp->clear();
    surfPointsFlat->clear();
    surfPointsLessFlat->clear();

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

        surfPointsLessFlatScan->clear();

        for (int j = 0; j < 6; j++) {

            // sp和ep的含义是什么???startPointer,endPointer?
            int sp = (segInfo.startRingIndex[i] * (6 - j)    + segInfo.endRingIndex[i] * j) / 6;
            int ep = (segInfo.startRingIndex[i] * (5 - j)    + segInfo.endRingIndex[i] * (j + 1)) / 6 - 1;

            if (sp >= ep)
                continue;

            // 按照cloudSmoothness.value从小到大排序
            std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());

            int largestPickedNum = 0;
            for (int k = ep; k >= sp; k--) {
                // 每次ind的值就是等于k??? 有什么意义?
                // 因为上面对cloudSmoothness进行了一次从小到大排序,所以ind不一定等于k了
                int ind = cloudSmoothness[k].ind;
                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] > edgeThreshold &&
                    segInfo.segmentedCloudGroundFlag[ind] == false) {
                
                    largestPickedNum++;
                    if (largestPickedNum <= 2) {
                        // 论文中nFe=2,cloudSmoothness已经按照从小到大的顺序排列,
                        // 所以这边只要选择最后两个放进队列即可
                        // cornerPointsSharp标记为2
                        cloudLabel[ind] = 2;
                        cornerPointsSharp->push_back(segmentedCloud->points[ind]);
                        cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
                    } else if (largestPickedNum <= 20) {
						// 塞20个点到cornerPointsLessSharp中去
						// cornerPointsLessSharp标记为1
                        cloudLabel[ind] = 1;
                        cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
                    } else {
                        break;
                    }

                    cloudNeighborPicked[ind] = 1;
                    for (int l = 1; l <= 5; l++) {
                        // 从ind+l开始后面5个点,每个点index之间的差值,
                        // 确保columnDiff<=10,然后标记为我们需要的点
                        int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));
                        if (columnDiff > 10)
                            break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l >= -5; l--) {
						// 从ind+l开始前面五个点,计算差值然后标记
                        int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
                        if (columnDiff > 10)
                            break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }

            int smallestPickedNum = 0;
            for (int k = sp; k <= ep; k++) {
                int ind = cloudSmoothness[k].ind;
                // 平面点只从地面点中进行选择???为什么要这样做???
                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] < surfThreshold &&
                    segInfo.segmentedCloudGroundFlag[ind] == true) {

                    cloudLabel[ind] = -1;
                    surfPointsFlat->push_back(segmentedCloud->points[ind]);

                    // 论文中nFp=4,将4个最平的平面点放入队列中
                    smallestPickedNum++;
                    if (smallestPickedNum >= 4) {
                        break;
                    }

                    cloudNeighborPicked[ind] = 1;
                    for (int l = 1; l <= 5; l++) {
                        // 从前面往后判断是否是需要的邻接点,是的话就进行标记
                        int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));
                        if (columnDiff > 10)
                            break;

                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l >= -5; l--) {
                        // 从后往前开始标记
                        int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
                        if (columnDiff > 10)
                            break;

                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }

            for (int k = sp; k <= ep; k++) {
                if (cloudLabel[k] <= 0) {
                    surfPointsLessFlatScan->push_back(segmentedCloud->points[k]);
                }
            }
        }

        // surfPointsLessFlatScan中有过多的点云,如果点云太多,计算量太大
        // 进行下采样,可以大大减少计算量
        surfPointsLessFlatScanDS->clear();
        downSizeFilter.setInputCloud(surfPointsLessFlatScan);
        downSizeFilter.filter(*surfPointsLessFlatScanDS);

        *surfPointsLessFlat += *surfPointsLessFlatScanDS;
    }
}

void updateTransformation()

中主要是两个部分,一个是找特征平面,通过面之间的对应关系计算出变换矩阵。 另一个部分是通过角、边特征的匹配,计算变换矩阵。 该函数主要由其他四个部分组成:findCorrespondingSurfFeatures,calculateTransformationSurf findCorrespondingCornerFeatures, calculateTransformationCorner 这四个函数分别是对应于寻找对应面、通过面对应计算变换矩阵、寻找对应角/边特征、通过角/边特征计算变换矩阵。

void updateTransformation(){

    if (laserCloudCornerLastNum < 10 || laserCloudSurfLastNum < 100)
        return;

    for (int iterCount1 = 0; iterCount1 < 25; iterCount1++) {
        laserCloudOri->clear();
        coeffSel->clear();

        // 找到对应的特征平面
        // 然后计算协方差矩阵,保存在coeffSel队列中
        // laserCloudOri中保存的是对应于coeffSel的未转换到开始时刻的原始点云数据
        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;
    }
}

void integrateTransformation()

计算了旋转角的累积变化量。 这个函数首先通过AccumulateRotation()将局部旋转左边切换至全局旋转坐标。 然后同坐变换转移到世界坐标系下。 再通过PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);插入imu旋转,更新姿态。

mapOptmization.cpp

mapOptmization.cpp进行的内容主要是地图优化,将得到的局部地图信息融合到全局地图中去。

publishGlobalMap()

  • publishGlobalMap()主要进行了3个步骤:
    1. 通过KDTree进行最近邻搜索;
    2. 通过搜索得到的索引放进队列;
    3. 通过两次下采样,减小数据量;

通过储存的 cloudKeyPoses3D 关键点的位置, 找到邻近的关键帧,默认显示500m范围内的关键位置。

 void publishGlobalMap(){

    if (pubLaserCloudSurround.getNumSubscribers() == 0)
        return;

    if (cloudKeyPoses3D->points.empty() == true)
        return;

    std::vector<int> pointSearchIndGlobalMap;
    std::vector<float> pointSearchSqDisGlobalMap;

    mtx.lock();
    kdtreeGlobalMap->setInputCloud(cloudKeyPoses3D);
    // 通过KDTree进行最近邻搜索
    kdtreeGlobalMap->radiusSearch(currentRobotPosPoint, globalMapVisualizationSearchRadius, pointSearchIndGlobalMap, pointSearchSqDisGlobalMap, 0);
    mtx.unlock();

    for (int i = 0; i < pointSearchIndGlobalMap.size(); ++i)
      globalMapKeyPoses->points.push_back(cloudKeyPoses3D->points[pointSearchIndGlobalMap[i]]);

    // 对globalMapKeyPoses进行下采样
    downSizeFilterGlobalMapKeyPoses.setInputCloud(globalMapKeyPoses);
    downSizeFilterGlobalMapKeyPoses.filter(*globalMapKeyPosesDS);

    for (int i = 0; i < globalMapKeyPosesDS->points.size(); ++i){
		int thisKeyInd = (int)globalMapKeyPosesDS->points[i].intensity;
		*globalMapKeyFrames += *transformPointCloud(cornerCloudKeyFrames[thisKeyInd],&cloudKeyPoses6D->points[thisKeyInd]);
		*globalMapKeyFrames += *transformPointCloud(surfCloudKeyFrames[thisKeyInd],&cloudKeyPoses6D->points[thisKeyInd]);
		*globalMapKeyFrames += *transformPointCloud(outlierCloudKeyFrames[thisKeyInd],&cloudKeyPoses6D->points[thisKeyInd]);
    }

    // 对globalMapKeyFrames进行下采样
    downSizeFilterGlobalMapKeyFrames.setInputCloud(globalMapKeyFrames);
    downSizeFilterGlobalMapKeyFrames.filter(*globalMapKeyFramesDS);

    sensor_msgs::PointCloud2 cloudMsgTemp;
    pcl::toROSMsg(*globalMapKeyFramesDS, cloudMsgTemp);
    cloudMsgTemp.header.stamp = ros::Time().fromSec(timeLaserOdometry);
    cloudMsgTemp.header.frame_id = "/camera_init";
    pubLaserCloudSurround.publish(cloudMsgTemp);  

    globalMapKeyPoses->clear();
    globalMapKeyPosesDS->clear();
    globalMapKeyFrames->clear();
    globalMapKeyFramesDS->clear();     
}

void run()

void run(){

    if (newLaserCloudCornerLast  && 
	std::abs(timeLaserCloudCornerLast  - timeLaserOdometry) < 0.005 &&
        newLaserCloudSurfLast    && 
		std::abs(timeLaserCloudSurfLast    - timeLaserOdometry) < 0.005 &&
        newLaserCloudOutlierLast && 
		std::abs(timeLaserCloudOutlierLast - timeLaserOdometry) < 0.005 &&
        newLaserOdometry)
    {
        newLaserCloudCornerLast = false; 
		newLaserCloudSurfLast = false; 
		newLaserCloudOutlierLast = false; 
		newLaserOdometry = false;

        std::lock_guard<std::mutex> lock(mtx);

        if (timeLaserOdometry - timeLastProcessing >= mappingProcessInterval) {

            timeLastProcessing = timeLaserOdometry;

            transformAssociateToMap();

            extractSurroundingKeyFrames();

            downsampleCurrentScan();

            scan2MapOptimization();

            saveKeyFramesAndFactor();

            correctPoses();

            publishTF();

            publishKeyPosesAndFrames();

            clearCloud();
        }
    }
}

参考资料

  • LOAM等内容的博客 需要好好研读
  • LeGO-LOAM 源码阅读笔记(imageProjecion.cpp)
  • LeGO-LOAM 源码阅读笔记(featureAssociation.cpp)
  • LeGO-LOAM 源码阅读笔记(mapOptmization.cpp)

你可能感兴趣的:(ROS,系统,SLAM,算法)