LOAM算法（论文+代码）详解（一）—— 引言+特征提取

最新推荐文章于 2026-03-31 19:23:29 发布

原创最新推荐文章于 2026-03-31 19:23:29 发布 · 7.2k 阅读

49 ·

本内容遵循CC 4.0 BY-SA版权协议

标签

#计算机视觉 #slam #c++

SLAM 专栏收录该内容

1 篇文章

订阅专栏

本文详细介绍了LOAM（Lidar Odometry and Mapping）算法，这是一种实时激光雷达SLAM方法。主要内容包括雷达里程计的特征提取，包括平滑度计算和特征点分类，如平面点和边缘点的选择。通过对点云数据的预处理，减少数据量，提高配准效率。LOAM算法分为点云注册、雷达里程计和雷达建图三个步骤，为构建三维点云地图提供基础。

AI助手已提取文章相关产品：

引言

LOAM: Lidar Odometry and Mapping in Real-time（RSS2014），是常年位于KITTI上前列的激光雷达SLAM算法，作为该领域发表时间较早的论文，启发了后续很多类似的工作。理解LOAM有助于初学者理解激光雷达SLAM的基本原理和整体流程。本系列文章将为各位同学系统梳理LOAM算法论文和代码中涉及的重点和难点。

图1 LOAM在KITTI上的排名

传送门：

论文：www.ri.cmu.edu/pub_files/2014/7/...

原味代码：github.com/laboshinl/loam_velodyne

改进代码：github.com/HKUST-Aerial-Robotics（使用Ceres_Solver库做非线性优化，简化了代码结构）。

论文亮点：

雷达运动畸变的补偿；
特征提取减小了点云配准的数据量；
ICP使用了非线性迭代优化（LM方法），而不是传统的SVD分解。

符号定义

在正式讲解算法之前，首先要搞清楚原文中的两个名词——scan和sweep。由于时间较为久远，原作者使用的激光雷达型号为Hokuyo（北阳） UTM-30LX，是一个2D激光线扫设备，一次只能获取一条的视角范围为 $180^{\circ}$ 间距 $0.25^{\circ}$ 的激光线。作者将其连接到一个旋转电机上，使其可以在竖直方向做 $180^{\circ}/s$ 的旋转，一秒钟可以生成40条扫描线，因此在竖直方向上的分辨率为 $4.5^{\circ}$ 。原文中，scan代指水平方向上的一条扫描线，sweep代指雷达完成一次半球旋转后得到的扫描面。

将第k次扫描sweep记作k， $k\in Z^{+}$ 。第k次扫描内获取的点云数据记为 $\mathcal{P}_{k}$ 。雷达坐标系满足右手坐标系,坐标系原点位于雷达的几何中心，x轴正方向向左，y轴向上，z轴向前，表示为 $\left \{ L \right \}$ 。第k次扫描的雷达观测坐标系表示为 $\left \{ L_{k} \right \}$ ， $\mathcal{P}_{k}$ 中的一个点 $i\in \mathcal{P}_{k}$ ，在 $\left \{ L_{k} \right \}$ 中表示为 $\textbf{X}_{\left ( k,i \right )}^{L}$ 。世界坐标系表示为 $\left \{ W \right \}$ ，与初始位置的观测坐标系 $\left \{ L \right \}$ 重合， $\mathcal{P}_{k}$ 中的一个点 $i\in \mathcal{P}_{k}$ ，在 $\left \{ W_{k} \right \}$ 中可以表示为 $\textbf{X}_{\left ( k,i \right )}^{W}$ 。

那么，要求解的问题可以表述为：

给定一个雷达点云序列 $\mathcal{P}_{k}$ ， $k\in Z^{+}$ ，计算雷达在k次扫描内的运动，并利用 $\mathcal{P}_{k}$ 构建已遍历到的场景的地图。

一、算法流程

图2 LOAM算法流程图

算法整体可以分为三个部分：

Point Cloud Registration: 可以理解为点云的一个预处理步骤，区分采集点云的通道（竖直方向方位角）和时刻（水平方向方位角），并进行特征点提取；
Lidar Odometry: 雷达里程计，对同一帧（sweep）内不同时刻采集的点云(scan)做运动变形（motion distortion）的补偿，得到无变形的单帧点云数据；
Lidar Mapping: 雷达建图，将每一帧的点云(sweep)配准到全局坐标系下，并进行体素抽稀，得到三维点云地图。

对于每帧输入的点云 $\hat{\mathcal{P}}$ ，（1）首先，经过点云预处理和特征提取步骤（Point Cloud Registration），得到 $\mathcal{P}_{k}$ ；（2）其次，通过雷达里程计（Lidar Odometry）对第k帧点云 $\mathcal{P}_{k}$ 做变形补偿，得到无运动变形的点云 $\mathcal{P}_{k}$ ，该步骤的更新频率为10Hz;（3）再次，通过雷达建图（Lidar Mapping），将每一帧的无变形点云 $\mathcal{P}_{k}$ 配准到全局坐标系 $\left \{ W \right \}$ 下，并进行体素抽稀，得到三维点云地图，该步骤更新频率为1Hz；（4）最后，集成前两个步骤发布的姿态变换，生成相对于地图的当前激光雷达姿态变换，输出频率为10Hz。

二、雷达里程计

雷达里程计包括三个主要模块：特征提取、特征匹配和运动估计。下面对这几个模块分别进行介绍。

1. 特征提取

1.1 平滑度计算

在进行特征提取之前，需要对当前帧点云 $\mathcal{P}_{k}$ 中的每个点i计算其平滑度，平滑度计算公式：

其中，当前点i的坐标为 $\textbf{X}_{\left ( k,i \right )}^{L}$ ， $\mathcal{S}$ 为与当前点i相邻的前后n个点组成的集合， $\textbf{X}_{\left ( k,j \right )}^{L}$ 为邻域集合 $\mathcal{S}$ 中的一个点j的坐标。

    for (int i = 5; i < cloudSize - 5; i++)
    { 
        float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x 
			+ laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x 
			+ laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x 
			+ laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
        float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y 
			+ laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y 
			+ laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y 
			+ laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
        float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z 
			+ laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z 
			+ laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z 
			+ laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;
		//前后5个点与当前点的差的平均值*10
        cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
        cloudSortInd[i] = i;
        cloudNeighborPicked[i] = 0;
        cloudLabel[i] = 0;
    }

A-LOAM平滑度计算对应代码段

1.2 特征提取

LOAM算法将点云特征分为两类：平面点和边缘点。

平面点(planar points)：在三维空间中处于平滑平面上的点，其和周围点的大小差距不大，曲率较低，平滑度较低。
边缘点(edge points)：在三维空间中处于尖锐边缘上的点，其和周围点的大小差距较大，曲率较高，平滑度较高。

对当前帧 $\mathcal{P}_{k}$ 内的点进行排序，找到c 最小的点作为平面点，c 最大的点作为边缘点。为了使特征值点分布均匀，将一帧扫描（sweep）内的一条扫描线（scan）分成四段，每一段提取最多两个边缘点，四个平面点。此外，如下图所示这两种不可靠点也不会被选为特征点。

图3 不可靠点示意图。（a）A点所在平面与雷达发出的激光束呈一定夹角，B点所在平面与激光束几乎平行，B点为不可靠点；（b）B点与A点的间隔较大，且线段AB与激光线束平行，B点对A点造成了遮挡，A点为不可靠点。

特征点选取的准则总结如下：

不能超过设定的size，每个集合平面点4个，边缘点2个；
已选取的点周围不能有点，使得点可以分布的更加均匀；
选取的平面点不能与激光扫描束平行。

    for (int i = 0; i < N_SCANS; i++)//通道数
    {
        if( scanEndInd[i] - scanStartInd[i] < 6)
            continue;
        pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
        for (int j = 0; j < 6; j++)//每条扫描线分为六段
        {
            int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; //起点ID
            int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;//终点ID

            TicToc t_tmp;
            std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);//用cloudCurvature排序
            t_q_sort += t_tmp.toc();

            int largestPickedNum = 0;
            for (int k = ep; k >= sp; k--)
            {
                int ind = cloudSortInd[k]; 

                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] > 0.1)//C值较大为边缘点
                {

                    largestPickedNum++;
                    if (largestPickedNum <= 2)//C值最大的两个点构成边缘点小集合Fe，参考LeGO-LOAM
                    {                        
                        cloudLabel[ind] = 2;
                        cornerPointsSharp.push_back(laserCloud->points[ind]);
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else if (largestPickedNum <= 20)//C值较大的20个点构成边缘点大集合Fme，参考LeGO-LOAM
                    {                        
                        cloudLabel[ind] = 1; 
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else
                    {
                        break;
                    }

                    cloudNeighborPicked[ind] = 1; //取过的点不再被取

                    for (int l = 1; l <= 5; l++)
                    {
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
                            break;//间隔很大的邻近点仍然可以被选
                        }

                        cloudNeighborPicked[ind + l] = 1;//当前点的后5个点不再被取
                    }
                    for (int l = -1; l >= -5; l--)
                    {
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
                            break;//间隔很大的邻近点仍然可以被选
                        }

                        cloudNeighborPicked[ind + l] = 1;//当前点的前5个点不再被取
                    }
                }
            }

            int smallestPickedNum = 0;
            for (int k = sp; k <= ep; k++)
            {
                int ind = cloudSortInd[k];

                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] < 0.1)//C值较小为平面点
                {

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

                    smallestPickedNum++;
                    if (smallestPickedNum >= 4) // C值最小的两个点构成平面点小集合Fp，参考LeGO-LOAM
                    { 
                        break;
                    }

                    cloudNeighborPicked[ind] = 1; //取过的点不再被取
                    for (int l = 1; l <= 5; l++)
                    { 
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
                            break;//间隔很大的邻近点仍然可以被选
                        }

                        cloudNeighborPicked[ind + l] = 1; // 当前点的后5个点不再被取
                    }
                    for (int l = -1; l >= -5; l--)
                    {
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
                            break;//间隔很大的邻近点仍然可以被选
                        }

                        cloudNeighborPicked[ind + l] = 1;//当前点的前5个点不再被取
                    }
                }
            }

            for (int k = sp; k <= ep; k++)
            {
                if (cloudLabel[k] <= 0) // C值小于条件阈值的其余点构成平面点大集合Fmp，参考LeGO-LOAM
                {
                    surfPointsLessFlatScan->push_back(laserCloud->points[k]);
                }
            }
        }

        pcl::PointCloud<PointType> surfPointsLessFlatScanDS;//平面点大集合Fmp做体素抽稀
        pcl::VoxelGrid<PointType> downSizeFilter;
        downSizeFilter.setInputCloud(surfPointsLessFlatScan);
        downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
        downSizeFilter.filter(surfPointsLessFlatScanDS);

        surfPointsLessFlat += surfPointsLessFlatScanDS;//每段的平面点大集合Fmp存到当前帧的总集合中
    }

A-LOAM特征点选取对应代码段

您可能感兴趣的与本文相关内容