精华内容
下载资源
问答
  • Lego-Loam算法
    千次阅读
    2022-01-13 20:26:47

    摘录:
    https://zhuanlan.zhihu.com/p/386449627

    目录
    参数

    参数

    extern const int N_SCAN = 16;     //  线数
    extern const int Horizon_SCAN = 1800;  // 旋转一周采样次数
    extern const float ang_res_x = 0.2;     // 水平分辨率
    extern const float ang_res_y = 2.0;     // 垂直分辨率
    extern const float ang_bottom = 15.0+0.1;   // lidar 底部线束的角度偏移
    extern const int groundScanInd = 7;         // lidar 判定为地面的线数
    // 是否需要回环检测
    extern const bool loopClosureEnableFlag = false;
    // 建图时间间隔(周期)
    extern const double mappingProcessInterval = 0.3;
    // 激光雷达扫描频率
    extern const float scanPeriod = 0.1;
    // 系统延迟
    extern const int systemDelay = 0;
    // imu缓存大小
    extern const int imuQueLength = 200;
    // 过滤小于1m之内的点云
    extern const float sensorMinimumRange = 1.0;
    // 雷达安装角度????(可能存在非水平安装)
    extern const float sensorMountAngle = 0.0;
    // 点云分割每个方向的角度大小
    extern const float segmentTheta = 60.0/180.0*M_PI; // decrese this value may improve accuracy
    // 点云分割有效点和有效线的大小
    extern const int segmentValidPointNum = 5;
    extern const int segmentValidLineNum = 3;
    // 水平方向最小分辨率弧度
    extern const float segmentAlphaX = ang_res_x / 180.0 * M_PI;
    // 垂直方向最小分辨率弧度
    extern const float segmentAlphaY = ang_res_y / 180.0 * M_PI;
    // 线特征数量
    extern const int edgeFeatureNum = 2;
    // 面特征数量
    extern const int surfFeatureNum = 4;
    // 没有使用到的参数
    extern const int sectionsTotal = 6;
    // 曲率阈值
    extern const float edgeThreshold = 0.1;
    extern const float surfThreshold = 0.1;
    // 特征查找半径
    extern const float nearestFeatureSearchSqDist = 25;
    
    // Mapping Params
    // 点云地图搜索半径
    extern const float surroundingKeyframeSearchRadius = 50.0; // key frame that is within n meters from current pose will be considerd for scan-to-map optimization (when loop closure disabled)
    // 点云地图搜索大小
    extern const int   surroundingKeyframeSearchNum = 50; // submap size (when loop closure enabled)
    // history key frames (history submap for loop closure)
    // 回环检测首尾距离
    extern const float historyKeyframeSearchRadius = 7.0; // key frame that is within n meters from current pose will be considerd for loop closure
    // 回环点云查找范围[-25, 25]
    extern const int   historyKeyframeSearchNum = 25; // 2n+1 number of hostory key frames will be fused into a submap for loop closure
    // icp匹配分数,平均距离小于0.3
    extern const float historyKeyframeFitnessScore = 0.3; // the smaller the better alignment
    // 可视化点云范围
    extern const float globalMapVisualizationSearchRadius = 500.0; // key frames with in n meters will be visualized
    
    更多相关内容
  • Loam算法详解(配合开源代码aloam)

    千次阅读 2021-08-07 19:44:24
    LOAM算法是激光slam中一个经典的开源框架,结合qin tong 博士的开源代码a-loam,对其原理及代码实现简要介绍如下: LOAM算法的总体框架如下图所示: 除了左侧的硬件采集数据之外,LOAM算法本身主要包括图中标记的...

    参考论文:LOAM: Lidar Odometry and Mapping in Real-time
    代码:A-LOAM
    Ubuntu 18.04 + ROS Melodic + eigen 3.3.4 + PCL 1.8.1 + ceres 2.0 + A-LOAM 配置

    LOAM算法是激光slam中一个经典的开源框架,结合qin tong 博士的开源代码a-loam,对其原理及代码实现简要介绍如下:

    LOAM算法的总体框架如下图所示:

    在这里插入图片描述
    除了左侧的硬件采集数据之外,LOAM算法本身主要包括图中标记的-四个部分。该算法的关键想法是把实时定位与测图这一同时寻找优化大量变量的复杂问题进行区分,通过两个算法来进行解决。首先,使用一个高频、低精度的里程计来估计LiDAR的位姿,并以此结果去除实验过程中产生的运动畸变(假设为匀速运动)。另外,使用一个低频、高精度的里程计算法来改善LiDAR位姿的精度,并输出全局坐标系下的地图。

    1 LiDAR硬件

    LiDAR硬件在采集过程中不断发布点云数据(10 HZ),以符号 P k P_k Pk表示在第K帧接收到的点云数据。
    L L L是局部激光坐标系,坐标原点在Lidar的几何中心,以符号 X ( k , i ) L X_{(k,i)}^L X(k,i)L表示在第 K K K帧中的 i i i 点在局部坐标系下的坐标。
    W W W 是全局坐标系,坐标原点在Lidar的起始位置,以符号 X ( k , i ) W X_{(k,i)}^W X(k,i)W表示在第 K K K帧中的 i i i 点在全局坐标系下的坐标。

    2 Point Cloud Registration

    这一部分中从LiDAR硬件中接收原始点云数据,接着进行预处理(去除噪声点、无效点)并按照扫描线号进行区分,存储在一个vector中。随后,对点云数据进行处理,发布sharp、less_sharp、flat、less_flat四种类型的特征点,如下图中所示:
    在这里插入图片描述
    分别介绍LiDAR点云预处理与特征点提取两部分内容:

    2.1 LiDAR点云预处理

    传感器获得的数据可能在一些点的坐标中为NaN(不是数)值, 一个NaNs表明测量传感器距离到该点的距离值是有问题的,可能是因为传感器太近或太远,或者因为表面反射。那么当存在无效点云的NaNs值作为算法的输入的时候,可能会引起很多问题,使用PCL自带的函数进行去除。
    在这里插入图片描述
    随后,去除原始数据中距离很近的点,即相邻点间小于仪器测量的分辨率,这里设置MINIMUM_RANGE为0.1 m。
    在这里插入图片描述
    进行上述处理后,计算每个点的scanID并存储在对应的laserCloudScans变量中。
    在这里插入图片描述
    在这里插入图片描述
    此外,还计算了当前点在一帧数据中的相对时间(时间比例),存放在intensity中(后续修正运动畸变时使用)。

    2.2 提取特征点

    在文章中描述了通过公式(1)来描述局部表面的平滑程度,其中S表示选取的一个周围点集, X ( k , i ) L X_{(k,i)}^L X(k,i)L表示当前点, X ( k , j ) L X_{(k,j)}^L X(k,j)L表示近邻点。
    在这里插入图片描述
    若c值较大,则代表当前点与周围点的差距较大,曲率较高,代表为一个边缘点(也就是代码中的sharp点)。
    若c值较小,则代表当前点与周围点的差距较小,曲率较低,代表为一个平面点(也就是代码中的flat点)。
    通过如下代码计算公式(1),当前点的前后5个点组成点云集合 S S S
    在这里插入图片描述
    在选择特征点的过程中,我们还想让其均匀分布,所以文章中计划把每个扫描分为4个部分(代码中分为了6个部分),同时每个子集区域中选取2个sharp点和4个flat点。
    在这里插入图片描述
    此外,为了避免选中不稳定的特征点,作者还讨论了下图中的两种情况要进行避免。1)左图(a)中的B点在一个几乎与激光束平行的表面上。2)右图(b)中的A点在被遮挡区域的边缘。
    在这里插入图片描述

    3 Lidar odometry

    3.1 寻找特征点对应关系

    假设一个扫描k的开始时间戳为 t k t_k tk,在第 k k k帧扫描结束接收到的点云为 P k P_k Pk(左侧第一条蓝线),其被投影到 t ( k + 1 ) t_{(k+1)} t(k+1)时间戳,用 P ‾ k \overline{P}_k Pk表示(即中间的绿线),同时在下一个扫描 k + 1 k+1 k+1中获取的数据为 P ( k + 1 ) P_{(k+1)} P(k+1)(图中的橙线)。
    在这里插入图片描述

    (注意:由于假设为匀速运动,所以上图中可以用直线来表示每一帧扫描的点云数据。)

    现在,假设我们有了 P ‾ k \overline{P}_k Pk P ( k + 1 ) P_{(k+1)} P(k+1)两个点云数据集,基于此来寻找两个点云数据之间的对应关系。
    首先,对 P ( k + 1 ) P_{(k+1)} P(k+1)采用2.2节中提取特征点的方法找到两个特征点集,边缘点 E ( k + 1 ) E_{(k+1)} E(k+1)和平面点 H ( k + 1 ) H_{(k+1)} H(k+1)。其次,对 P ‾ k \overline{P}_k Pk进行kd-tree索引,并在 P ‾ k \overline{P}_k Pk中找到每个边缘点对应的边缘线(两个点,左图(a))与每个平面点对应的面片(三个点,右图(b))。
    在这里插入图片描述
    利用向量叉乘,获取点 i i i到直线 i j ij ij的距离,以及点 i i i到平面 l j m ljm ljm的距离。计算公式分别为:
    在这里插入图片描述在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    前述假设LiDAR在一个扫描中角速度和线速度均为匀速运动,所以能够对计算出的位姿进行线性插值。假设 T ( k , i ) L T_{(k,i)}^L T(k,i)L为[ t ( k + 1 ) t_{(k+1)} t(k+1), t t t]两帧点云数据之间的位姿变换(6-DoF),给定一个点i , i∈ P ( k + 1 ) P_{(k+1)} P(k+1),其获取的时间戳为 t i t_i ti,[ t ( k + 1 ) t_{(k+1)} t(k+1), t i t_i ti]之间的变换为 T ( k + 1 , i ) L T_{(k+1,i)}^L T(k+1,i)L并可以通过下式进行计算。
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    在每一次迭代过程中均把 P ( k + 1 ) P_{(k+1)} P(k+1)中的点i投影到该帧数据的起始点 t ( k + 1 ) t_{(k+1)} t(k+1),然后再与 P k P_k Pk中的点进行匹配。
    假设 X ( ( k + 1 , i ) ) L X_{((k+1,i))}^L X((k+1,i))L为投影之前的i点坐标, X ~ ( ( k + 1 , i ) ) L \widetilde{X} _{((k+1,i))}^L X ((k+1,i))L为投影到起始点 t ( k + 1 ) t_{(k+1)} t(k+1)之后的i点坐标,则两者之间存在如下关系。在这里插入图片描述
    综合考虑前述的公式2与公式3,得到以下公式9与公式10:
    在这里插入图片描述
    在这里插入图片描述
    综合公式9与公式10,得到:
    在这里插入图片描述
    式11中每一行均为一个特征点及对应的边缘线或平面面片,对公式11中的非线性函数进行优化,不断迭代使得 d d d最小。
    在这里插入图片描述

    3.2 Lidar odometry算法总结

    Lidar odometry算法如下所示,以上一帧点云 P ‾ k \overline{P}_k Pk,逐渐增长的点云 P ( k + 1 ) P_{(k+1)} P(k+1),以及上一次迭代计算的位姿参数 T ( k + 1 ) L T_{(k+1)}^L T(k+1)L作为输入(如果是一个扫描的开始则设置为0)。接着,从 P ( k + 1 ) P_{(k+1)} P(k+1)中提取边缘点 E ( k + 1 ) E_{(k+1)} E(k+1)和平面点 H ( k + 1 ) H_{(k+1)} H(k+1),对于每个边缘点/平面点,在上一帧点云 P ‾ k \overline{P}_k Pk中找到其对应的边缘线/面片,建立公式11中的非线性函数,并进行优,若迭代达到收敛则输出 T ( k + 1 ) L T_{(k+1)}^L T(k+1)L,若达到了扫描的终点则输出 T ( k + 1 ) L T_{(k+1)}^L T(k+1)L P ‾ ( k + 1 ) \overline{P}_{(k+1)} P(k+1)
    在这里插入图片描述

    4 Lidar Mapping

    第3节中的里程计高频、低精度运行,其输出的位姿参数还需要进一步在后续的mapping中进行优化。
    Mapping算法在每个扫描结束才运行一次,如在第 k + 1 k+1 k+1次扫描结束后,lidar odometry产生了一个没有畸变的点云 P ‾ ( k + 1 ) \overline{P}_{(k+1)} P(k+1)和一个位姿变换参数 T ( k + 1 ) L T_{(k+1)}^L T(k+1)L(代表[ t ( k + 1 ) t_{(k+1)} t(k+1), t ( k + 2 ) t_{(k+2)} t(k+2)]之间的运动)。Mapping算法把点云 P ‾ ( k + 1 ) \overline{P}_{(k+1)} P(k+1)配准到全局坐标系 W W W下,如下图中所示,假设 Q k Q_k Qk为前 k k k次扫描积累的数据所形成的点云地图, T k W T_k^W TkW为lidar在地图中最后一次扫描 k k k k + 1 k+1 k+1处的变换位姿。随着lidar odometry的输出,Mapping算法不断扩展 T k W T_k^W TkW T ( k + 1 ) W T_{(k+1)}^W T(k+1)W(即扫描时间[ t ( k + 1 ) t_{(k+1)} t(k+1), t ( k + 2 ) t_{(k+2)} t(k+2)]范围的全局变换,并把 P ‾ ( k + 1 ) \overline{P}_{(k+1)} P(k+1)投影到全局坐标系下,得到 Q ‾ ( k + 1 ) \overline{Q}_{(k+1)} Q(k+1)。接着,通过不断优化 T ( k + 1 ) W T_{(k+1)}^W T(k+1)W Q ‾ ( k + 1 ) \overline{Q}_{(k+1)} Q(k+1)匹配到地图 Q k Q_k Qk中。
    在这里插入图片描述
    特征点提取方法与上述的2.2节相同(数量增加10倍,即为代码中的less_sharp点与less_flat点)。为了找到特征点之间的对应关系且提高运行效率,使用边长为10m的立方体代替全局地图(代码中是10105)。
    在这里插入图片描述
    取立方体中与 Q ‾ ( k + 1 ) \overline{Q}_{(k+1)} Q(k+1)相交的部分,建立3D kd-tree索引。在 Q k Q_k Qk的特征点中选取点集S’,针对边缘点只保留S’中边缘线上的点,针对平面点,只保留S’中面片上的点。然后,计算点集S’的协方差矩阵M,并分解M得到特征值记为V,特征向量E。

    如果S’分布在一条线段上,那么V中一个特征值就会明显比其他两个大, E中与较大特征值相对应的特征向量代表边缘线的方向。(一大两小,大方向)
    如果S’分布在一个平面上,那么V中一个特征值就会明显比其他两个小, E中与较小特征值相对应的特征向量代表平面的朝向。(一小两大,小方向)
    边缘线或平面块的位置通过点集S’的几何中心确定。

    边缘线或平面块的位置通过点集S’的几何中心确定。
    在这里插入图片描述

    在这里插入图片描述
    在这里插入图片描述
    为了计算边缘点/平面点与其对应边缘线/面片之间的距离,在一个边缘线上选取两个点,在面片上选取三个点,分别通过公式2、3进行计算,并转换为公式9、10,组合为公式11中所示的非线性函数,进行优化。
    边缘特征:

    在这里插入图片描述平面特征:
    在这里插入图片描述
    求解:
    在这里插入图片描述

    最后,把 Q ‾ ( k + 1 ) \overline{Q}_{(k+1)} Q(k+1)匹配到地图 Q k Q_k Qk中时,考虑到点云的均匀分布,还对其进行了一个下采样。

    展开全文
  • LOAM算法原理和代码的理解

    千次阅读 多人点赞 2019-12-28 22:44:06
    LOAM在KITTI上的排名一直是前两名。 LOAM在KITTI上的排名 原始LOAM代码的地址:https://github.com/cuitaixiang/LOAM_NOTED 作为教学用的A-LOAM代码地址:https://github.com/HKUST-Aerial-Robotics/A-LOAM LOAM_...

    LOAM概况

    V-LOAM和LOAM在KITTI上的排名一直是前两名。
    LOAM在KITTI上的排名
    原始LOAM代码(带中文注释)的地址:https://github.com/cuitaixiang/LOAM_NOTED
    作为教学用的A-LOAM代码地址:https://github.com/HKUST-Aerial-Robotics/A-LOAM使用ceres库来做非线性优化。代码里默认一帧激光数据内是匀速运动的,直接使用时间进行线性插值得到去畸变的点云位姿

    LOAM使用两轴的激光雷达进行SLAM建图。其具体的结构如下图所示:
    在这里插入图片描述
    因为一帧完整的激光数据所经历的时间比较长,在该段时间内激光器的位移已不能忽略。所以每束激光测量的距离会在激光器的不同位置获得。LOAM首先面临的问题就是点云运动畸变的去除。另外,LOAM将SLAM问题分成了两部分,使用两个算法并行运行来处理这两部分问题,以达到实时构建低漂移的地图。其中一个算法以10Hz的频率运行,得到一个 较为粗糙的里程计信息。该算法使用基于特征点的scan-to-scan match方法,可以快速计算得到里程计信息。另一个算法以1Hz的频率运行,输出一个更为精确的里程计信息。因为它以一个更低的频率运行,所以可以获取更多的特征点,同时也可以进行更多次迭代。总的来说,LOAM结合了scan-to-scan match方法和scan-to-map match方法的优势,很好地控制了运算速度和精度。

    LOAM主要算法流程

    在这里插入图片描述
    上图为LOAM算法的主要执行流程。下面分别说明。
    Point Cloud Registration
    1.使用IMU数据进行点云运动畸变去除

    利用IMU去除激光传感器在运动过程中非匀速(加减速)部分造成的误差(运动畸变)。为什么这样做呢?因为LOAM是基于匀速运动的假设,但实际中激光传感器的运动肯定不是匀速的,因此使用IMU来去除非匀速运动部分造成的误差,以满足匀速运动的假设。

    imuHandler()函数接受IMU数据。IMU的数据可以提供给我们IMU坐标系三个轴相对于世界坐标系的欧拉角和三个轴上的加速度。但由于加速度受到重力的影响所以首先得去除重力影响。在去除重力影响后我们想要获得IMU在世界坐标系下的运动,因此在 AccumulateIMUShift()中,根据欧拉角就可以将IMU三轴上的加速度转换到世界坐标系下的加速度。 然后利用匀加速度公式 s 1 = s 2 + v t + 1 / 2 a t 2 s1 = s2+ vt + 1/2at^2 s1=s2+vt+1/2at2来计算位移。因此我们可以求出每一帧IMU数据在世界坐标系下对应的位移和速度。代码中实际缓存了200帧这样的数据,为后面插值激光点运动畸变打下了基础。

    这里使用IMU数据进行插值计算点云的中点的位置,消除由于非匀速运动造成的运动畸变。

    //-0.5 < relTime < 1.5(点旋转的角度与整个周期旋转角度的比率, 即点云中点的相对时间)
        float relTime = (ori - startOri) / (endOri - startOri);
        //点强度=线号+点相对时间(即一个整数+一个小数,整数部分是线号,小数部分是该点的相对时间),匀
        //速扫描:根据当前扫描的角度和扫描周期计算相对扫描起始位置的时间
        point.intensity = scanID + scanPeriod * relTime;
     
        //点时间=点云时间+周期时间
        if (imuPointerLast >= 0) {//如果收到IMU数据,使用IMU矫正点云畸变
          float pointTime = relTime * scanPeriod;//计算点的周期时间
          //寻找是否有点云的时间戳小于IMU的时间戳的IMU位置:imuPointerFront
          while (imuPointerFront != imuPointerLast) {
            if (timeScanCur + pointTime < imuTime[imuPointerFront]) {
              break;
            }
            imuPointerFront = (imuPointerFront + 1) % imuQueLength;
          }
     
          if (timeScanCur + pointTime > imuTime[imuPointerFront]) {//没找到,此时
    //imuPointerFront==imtPointerLast,只能以当前收到的最新的IMU的速度,位移,欧拉角作为当前点的
    //速度,位移,欧拉角使用
            imuRollCur = imuRoll[imuPointerFront];
            imuPitchCur = imuPitch[imuPointerFront];
            imuYawCur = imuYaw[imuPointerFront];
     
            imuVeloXCur = imuVeloX[imuPointerFront];
            imuVeloYCur = imuVeloY[imuPointerFront];
            imuVeloZCur = imuVeloZ[imuPointerFront];
     
            imuShiftXCur = imuShiftX[imuPointerFront];
            imuShiftYCur = imuShiftY[imuPointerFront];
            imuShiftZCur = imuShiftZ[imuPointerFront];
          } else {//找到了点云时间戳小于IMU时间戳的IMU位置,则该点必处于imuPointerBack和
    //imuPointerFront之间,据此线性插值,计算点云点的速度,位移和欧拉角
            int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
            //按时间距离计算权重分配比率,也即线性插值
            float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack]) 
                             / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
            float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime) 
                            / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
     
            imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
            imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
            if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI) {
              imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;
            } else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI) {
              imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;
            } else {
              imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
            }
     
            //本质:imuVeloXCur = imuVeloX[imuPointerback] + (imuVelX[imuPointerFront]-imuVelX[imuPoniterBack])*ratioFront
            imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
            imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
            imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;
     
            imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
            imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
            imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
          }
     
          if (i == 0) {//如果是第一个点,记住点云起始位置的速度,位移,欧拉角
            imuRollStart = imuRollCur;
            imuPitchStart = imuPitchCur;
            imuYawStart = imuYawCur;
     
            imuVeloXStart = imuVeloXCur;
            imuVeloYStart = imuVeloYCur;
            imuVeloZStart = imuVeloZCur;
     
            imuShiftXStart = imuShiftXCur;
            imuShiftYStart = imuShiftYCur;
            imuShiftZStart = imuShiftZCur;
          } else {//计算之后每个点相对于第一个点的由于加减速非匀速运动产生的位移速度畸变,并对点云中
    //的每个点位置信息重新补偿矫正
            ShiftToStartIMU(pointTime);
            VeloToStartIMU();
            TransformToStartIMU(&point);
          }
        }
        laserCloudScans[scanID].push_back(point);//将每个补偿矫正的点放入对应线号的容器
    }
    
    
    

    void ShiftToStartIMU(float pointTime)函数中主要计算一帧数据中某点相对于一帧的起始点由于加减速造成的运动畸变。因此我们首先要求出世界坐标系下的加减速造成的运动畸变,然后将运动畸变值经过绕y、x、z轴旋转后得到起始点坐标系下的运动畸变。这里的坐标系一定要搞清楚为什么要放的起始点的坐标系下。

    //计算局部坐标系下点云中的点相对第一个开始点的由于加减速运动产生的位移畸变
    void ShiftToStartIMU(float pointTime)
    {
      //计算相对于第一个点由于加减速产生的畸变位移(全局坐标系下畸变位移量delta_Tg)
      //imuShiftFromStartCur = imuShiftCur - (imuShiftStart + imuVeloStart * pointTime)
      imuShiftFromStartXCur = imuShiftXCur - imuShiftXStart - imuVeloXStart * pointTime;
      imuShiftFromStartYCur = imuShiftYCur - imuShiftYStart - imuVeloYStart * pointTime;
      imuShiftFromStartZCur = imuShiftZCur - imuShiftZStart - imuVeloZStart * pointTime;
     
      /********************************************************************************
      Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Tg
      transfrom from the global frame to the local frame
      *********************************************************************************/
     
      //绕y轴旋转(-imuYawStart),即Ry(yaw).inverse
      float x1 = cos(imuYawStart) * imuShiftFromStartXCur - sin(imuYawStart) * imuShiftFromStartZCur;
      float y1 = imuShiftFromStartYCur;
      float z1 = sin(imuYawStart) * imuShiftFromStartXCur + cos(imuYawStart) * imuShiftFromStartZCur;
     
      //绕x轴旋转(-imuPitchStart),即Rx(pitch).inverse
      float x2 = x1;
      float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
      float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;
     
      //绕z轴旋转(-imuRollStart),即Rz(pitch).inverse
      imuShiftFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;
      imuShiftFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;
      imuShiftFromStartZCur = z2;
    }
    

    接下来就是VeloToStartIMU()函数,这个函数流程和上一个函数大致相同。它的作用就是求当前点的速度相对于点云起始点的速度畸变,先计算全局坐标系下的然后再转换到起始点的坐标系中。

    //计算局部坐标系下点云中的点相对第一个开始点由于加减速产生的的速度畸变(增量)
    void VeloToStartIMU()
    {
      //计算相对于第一个点由于加减速产生的畸变速度(全局坐标系下畸变速度增量delta_Vg)
      imuVeloFromStartXCur = imuVeloXCur - imuVeloXStart;
      imuVeloFromStartYCur = imuVeloYCur - imuVeloYStart;
      imuVeloFromStartZCur = imuVeloZCur - imuVeloZStart;
     
      /********************************************************************************
        Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * delta_Vg
        transfrom from the global frame to the local frame
      *********************************************************************************/
      
      //绕y轴旋转(-imuYawStart),即Ry(yaw).inverse
      float x1 = cos(imuYawStart) * imuVeloFromStartXCur - sin(imuYawStart) * imuVeloFromStartZCur;
      float y1 = imuVeloFromStartYCur;
      float z1 = sin(imuYawStart) * imuVeloFromStartXCur + cos(imuYawStart) * imuVeloFromStartZCur;
     
      //绕x轴旋转(-imuPitchStart),即Rx(pitch).inverse
      float x2 = x1;
      float y2 = cos(imuPitchStart) * y1 + sin(imuPitchStart) * z1;
      float z2 = -sin(imuPitchStart) * y1 + cos(imuPitchStart) * z1;
     
      //绕z轴旋转(-imuRollStart),即Rz(pitch).inverse
      imuVeloFromStartXCur = cos(imuRollStart) * x2 + sin(imuRollStart) * y2;
      imuVeloFromStartYCur = -sin(imuRollStart) * x2 + cos(imuRollStart) * y2;
      imuVeloFromStartZCur = z2;
    }
    
    

    接下来就是TransformToStartIMU(PointType *p)函数作用是将当前点先转换到世界坐标系下然后再由世界坐标转换到点云起始点坐标系下。 然后减去加减速造成的非匀速畸变的值。

    //去除点云加减速产生的位移畸变
    void TransformToStartIMU(PointType *p)
    {
      /********************************************************************************
        Ry*Rx*Rz*Pl, transform point to the global frame
      *********************************************************************************/
      //绕z轴旋转(imuRollCur)
      float x1 = cos(imuRollCur) * p->x - sin(imuRollCur) * p->y;
      float y1 = sin(imuRollCur) * p->x + cos(imuRollCur) * p->y;
      float z1 = p->z;
     
      //绕x轴旋转(imuPitchCur)
      float x2 = x1;
      float y2 = cos(imuPitchCur) * y1 - sin(imuPitchCur) * z1;
      float z2 = sin(imuPitchCur) * y1 + cos(imuPitchCur) * z1;
     
      //绕y轴旋转(imuYawCur)
      float x3 = cos(imuYawCur) * x2 + sin(imuYawCur) * z2;
      float y3 = y2;
      float z3 = -sin(imuYawCur) * x2 + cos(imuYawCur) * z2;
     
      /********************************************************************************
        Rz(pitch).inverse * Rx(pitch).inverse * Ry(yaw).inverse * Pg
        transfrom global points to the local frame
      *********************************************************************************/
      
      //绕y轴旋转(-imuYawStart)
      float x4 = cos(imuYawStart) * x3 - sin(imuYawStart) * z3;
      float y4 = y3;
      float z4 = sin(imuYawStart) * x3 + cos(imuYawStart) * z3;
     
      //绕x轴旋转(-imuPitchStart)
      float x5 = x4;
      float y5 = cos(imuPitchStart) * y4 + sin(imuPitchStart) * z4;
      float z5 = -sin(imuPitchStart) * y4 + cos(imuPitchStart) * z4;
     
      //绕z轴旋转(-imuRollStart),然后叠加平移量
      p->x = cos(imuRollStart) * x5 + sin(imuRollStart) * y5 + imuShiftFromStartXCur;
      p->y = -sin(imuRollStart) * x5 + cos(imuRollStart) * y5 + imuShiftFromStartYCur;
      p->z = z5 + imuShiftFromStartZCur;
    }
    

    这一部分主要引用:https://blog.csdn.net/liuyanpeng12333/article/details/82737181中讲解IMU进行运动畸变的部分

    2.特征点的提取
    LOAM中提取的特征点有两种:Edge Point和Planar Point。两种特征使用曲率来进行区分。曲率最大的为Edge Point,曲率最小的为Planar Point。论文中计算曲率的公式如下:
    在这里插入图片描述
    我对该公式的理解是:
    在点云 i i i的左边和右边分别选取几个点。如下图中所示的 k k k点和 j j j点。
    在这里插入图片描述
    其中向量 i g ig ig的模长就表征了曲率的大小。代码中分别在 i i i点的左侧和右侧选取了5个点来进行曲率计算。

      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;
    
        //曲率计算
    
        cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
    

    3.去除一些不可靠的点。
    主要是下面两种情况。
    在这里插入图片描述
    针对激光光束入射角与障碍物表面夹角小的点,即a图所示的情况,代码中是按下图所示方法来去除的。
    在这里插入图片描述
    A A A B B B为距离大于阈值(代码里为0.1米)的两个相邻点。其与激光器 O O O的连线就是该点的距离值。找到 A O AO AO连线上的一点 B ′ B' B使得 B O = B ′ O BO=B'O BO=BO。因为弧长除以边长就是夹角 θ \theta θ的弧度值。所以 θ = B B ′ / B O \theta=BB'/BO θ=BB/BO θ \theta θ值越小表示 A B AB AB两点的连线越陡。当 θ \theta θ值小于某一个阈值就认为该点是不可靠的。并且将前方或后方的5个点也置成不可选的点(即不会选为特征点)。下面是该部分的代码实现。

    //挑选点,排除容易被斜面挡住的点以及离群点,有些点容易被斜面挡住,而离群点可能出现带有偶然性,这些情况都可能导致前后两次扫描不能被同时看到
    
      for (int i = 5; i < cloudSize - 6; i++) {//与后一个点差值,所以减6
    
        float diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x;
    
        float diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y;
    
        float diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z;
    
        //计算有效曲率点与后一个点之间的距离平方和
    
        float diff = diffX * diffX + diffY * diffY + diffZ * diffZ;
    
    
    
        if (diff > 0.1) {//前提:两个点之间距离要大于0.1
    
    
    
            //点的深度
    
          float depth1 = sqrt(laserCloud->points[i].x * laserCloud->points[i].x + 
    
                         laserCloud->points[i].y * laserCloud->points[i].y +
    
                         laserCloud->points[i].z * laserCloud->points[i].z);
    
    
    
          //后一个点的深度
    
          float depth2 = sqrt(laserCloud->points[i + 1].x * laserCloud->points[i + 1].x + 
    
                         laserCloud->points[i + 1].y * laserCloud->points[i + 1].y +
    
                         laserCloud->points[i + 1].z * laserCloud->points[i + 1].z);
    
    
    
          //按照两点的深度的比例,将深度较大的点拉回后计算距离
    
          if (depth1 > depth2) {
    
            diffX = laserCloud->points[i + 1].x - laserCloud->points[i].x * depth2 / depth1;
    
            diffY = laserCloud->points[i + 1].y - laserCloud->points[i].y * depth2 / depth1;
    
            diffZ = laserCloud->points[i + 1].z - laserCloud->points[i].z * depth2 / depth1;
    
    
    
            //边长比也即是弧度值,若小于0.1,说明夹角比较小,斜面比较陡峭,点深度变化比较剧烈,点处在近似与激光束平行的斜面上
    
            if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth2 < 0.1) {//排除容易被斜面挡住的点
    
                //该点及前面五个点(大致都在斜面上)全部置为筛选过
    
              cloudNeighborPicked[i - 5] = 1;//赋成1表示不再考虑为特征点
    
              cloudNeighborPicked[i - 4] = 1;
    
              cloudNeighborPicked[i - 3] = 1;
    
              cloudNeighborPicked[i - 2] = 1;
    
              cloudNeighborPicked[i - 1] = 1;
    
              cloudNeighborPicked[i] = 1;
    
            }
    
          } else {
    
            diffX = laserCloud->points[i + 1].x * depth1 / depth2 - laserCloud->points[i].x;
    
            diffY = laserCloud->points[i + 1].y * depth1 / depth2 - laserCloud->points[i].y;
    
            diffZ = laserCloud->points[i + 1].z * depth1 / depth2 - laserCloud->points[i].z;
    
    
    
            if (sqrt(diffX * diffX + diffY * diffY + diffZ * diffZ) / depth1 < 0.1) {
    
              cloudNeighborPicked[i + 1] = 1;
    
              cloudNeighborPicked[i + 2] = 1;
    
              cloudNeighborPicked[i + 3] = 1;
    
              cloudNeighborPicked[i + 4] = 1;
    
              cloudNeighborPicked[i + 5] = 1;
    
              cloudNeighborPicked[i + 6] = 1;
    
            }
    
          }
    
        }
    

    针对前后遮挡的情况,如b所示,是通过计算前后两点间距的平方是否大于该点距离值平方的万分之二来排除的。下面是代码实现。

        float diffX2 = laserCloud->points[i].x - laserCloud->points[i - 1].x;
    
        float diffY2 = laserCloud->points[i].y - laserCloud->points[i - 1].y;
    
        float diffZ2 = laserCloud->points[i].z - laserCloud->points[i - 1].z;
    
        //与前一个点的距离平方和
    
        float diff2 = diffX2 * diffX2 + diffY2 * diffY2 + diffZ2 * diffZ2;
    
    
    
        //点深度的平方和
    
        float dis = laserCloud->points[i].x * laserCloud->points[i].x
    
                  + laserCloud->points[i].y * laserCloud->points[i].y
    
                  + laserCloud->points[i].z * laserCloud->points[i].z;
    
    
    
        //与前后点的平方和都大于深度平方和的万分之二,这些点视为离群点,包括陡斜面上的点,强烈凸凹点和空旷区域中的某些点,置为筛选过,弃用
    
        if (diff > 0.0002 * dis && diff2 > 0.0002 * dis) {
    
          cloudNeighborPicked[i] = 1;
    
        }
    

    遍历点云计算好曲率后会对曲率进行从小到大的排序。为了保证特征点的均匀获取,会将点云分成6个部分,每个部分选取一定数量的特征点。

    Lidar Odometry
    1.激光帧的时间同步
    在这里插入图片描述
    一帧激光帧经历的时间比较长。一个sweep间隔里,激光点云数据会由于激光器的运动产生畸变。所以作者将前后两帧激光数据都转换到同一时刻,然后再进行激光点云匹配。如上图所示,将完整的一帧激光点云 P k P_k Pk投影到 t t + 1 t_{t+1} tt+1时刻。投影过程会去除点云的运动畸变。在上一个模块(Point Cloud Registration)中,已经用IMU的数据去除了非匀速运动的畸变。在本模块(Lidar Odometr)中,使用上一模块中得到的一个sweep内的平均速度去除匀速运动引起的畸变。最终得到的效果就好像,激光器在 t t + 1 t_{t+1} tt+1时刻静止扫出来的点云。对于新接受的数据 P k + 1 P_{k+1} Pk+1使用同样的方法投影到 t t + 2 t_{t+2} tt+2时刻。注意这里的投影过程是为了后面的点云匹配。所以只进行了特征点云的投影。而当匹配结束后会将当前帧的特征点云和完整的点云数据都投影到该帧最后一个时刻。

    2.点云匹配与误差函数

    在这里插入图片描述
    对于Edge Point,是寻找离当前帧的Edge Point最近的上一帧里的两个Edge Point。同时需要保证这两个Edge Point不能在同一层激光里,正如上图(a)中的点 l l l j j j分别处于不同的层。而误差函数就是描述当前帧的 i i i点到上一帧中 l l l j j j连线的距离。方程公式如下:

    在这里插入图片描述
    上图的公式表示点到直线的距离。在二维空间中,向量 a a a叉乘向量 b b b的模长等于由向量a和向量b构成的平行四边形的面积。
    在这里插入图片描述
    在这里插入图片描述

    图片引用于https://huangwang.github.io/2019/01/05/%E5%90%91%E9%87%8F%E7%82%B9%E7%A7%AF%E5%8F%89%E7%A7%AF%E5%8F%8A%E5%85%B6%E5%87%A0%E4%BD%95%E6%84%8F%E4%B9%89/

    方程中的分子相当于向量 a a a和向量 b b b围成的平行四边形面积。分母就是向量 a b ab ab的模长。平行四边形的面积除以边长 a b ab ab就是向量 a a a和向量 b b b共同端点到直线 a b ab ab的距离。
    在这里插入图片描述

    对于Planar Point,如Fig7图中(b)所示,找到离当前帧中点 i i i最近的上一帧的3个Planar Point。需要保证这3个Planar Point不在同一层中。这样3个点就形成了一个面。而误差函数就是描述当前帧的 i i i点到上一帧中 l l l j j j m m m点所在平面的距离。方程公式如下:
    在这里插入图片描述
    在三维几何中,向量a和向量b的叉乘结果是一个向量,有个更通俗易懂的叫法是法向量,该向量垂直于a和b向量构成的平面。方程中点乘的下面那部分会得到单位化的法向量。而向量 i j ij ij点乘单位法向量就相当于向量 i j ij ij在法向量上的投影,即点 i i i到平面的距离。

    最终的误差函数是两项误差的和。最后用LM方法迭代(代码中最大迭代25次)求解出前后两帧激光的位姿差。

    Lidar Mapping
    在这里插入图片描述
    上图中, Q k Q_{k} Qk表示到第 k k k次sweep为止累积的点云地图。 Q ˉ k + 1 \bar{Q}_{k+1} Qˉk+1表示投影到map坐标系下的第 k k k次sweep的激光数据。 T k W \boldsymbol{T}_{k}^{W} TkW表示map坐标系下激光器的位姿。 T k + 1 L \boldsymbol{T}_{k+1}^{L} Tk+1L表示 T k W \boldsymbol{T}_{k}^{W} TkW坐标系下激光器的位姿。

    从Lidar Odometry模块中,我们可以得到一帧已经去除畸变的激光点云数据和相对于前一帧的粗糙位姿变换。Lidar Mapping模块的任务就是用 Q ˉ k + 1 \bar{Q}_{k+1} Qˉk+1与地图 Q k Q_{k} Qk进行匹配得到一个更为精确的帧间位姿变换,然后建图,为下次匹配做好准备。这里使用的匹配方法与Lidar Odometry模块中是一致的。不同的地方在于这里提取的特征点数量是Lidar Odometry模块的10倍。特征点的匹配不是找对应的2个或者3个特征点。而是相对于当前帧,在地图 Q k Q_{k} Qk中对应位置附近10m10m10m的Cubic中提取所有的特征点。筛选出最近的5个特征点,然后计算协方差。对协方差进行特征值分解,最大特征值对应的向量就是需要匹配的Edge line方向向量,最小特征值对应的向量就是需要匹配的Planar patch方向向量。分别在Edge line上选取两个点,在Planar patch上选取三个点,按照点到线的距离公式和点到面的距离公式构建误差方程。同样使用LM方法迭代(最多迭代10次)求解,得到一个更为精确的帧间位姿变换。

    Transform Integration
    Transform Integration模块主要是融合了Lidar Mapping得到的位姿变换和Lidar Odometry得到的位姿变换。最终发布一个频率与Lidar Odometry发布频率一致的位姿变换。

    LOAM的相关材料

    LOAM_velodyne学习该专栏对LOAM代码的的4个模块都做了分析
    LOAM 论文及原理分析这篇文章对论文内容做了较为详细的解释
    LOAM SLAM代码解析之一:scanRegistration.cpp 点云及IMU数据处理节点这篇文章对IMU数据处理和特征点选取的代码作了分析解释
    收集的材料放到自己的github上了:https://github.com/shoufei403/loam_leanring.git

    针对LOAM和A-LOAM在KITT上排名差别很大的几点看法

    1.LOAM认为一帧激光点云的间隔时间内激光器是匀加减速运动的,并且在点云畸变去除时使用了IMU数据来去除非匀速部分的畸变,然后在Lidar Mapping模块中还利用IMU数据求出平均速度来去除匀速运动引起的畸变。而A-LOAM只认为一帧激光点云的间隔时间内激光器是匀速运动的。然后直接根据时间进行线性插值来进行点云运动畸变去除。很显然LOAM对激光器的运动描述的更为准确,所以效果会更好。

    2.A-LOAM使用ceres进行非线性优化求解。而LOAM是作者手写的非线性求解器。这一部分我看不太懂,但猜测代码的工程实现上会有优化,代码执行效率可能更高。

    关注公众号《首飞》回复“机器人”获取精心推荐的C/C++,Python,Docker,Qt,ROS1/2,机器人学等机器人行业常用技术资料。

    展开全文
  • LeGO-LOAM算法详解

    千次阅读 2021-08-16 11:33:16
    LeGO-LOAM算法详解 整体框架 LeGO-LOAM算法的总体框架如下图所示: 图中新增加了绿框中的Segmentation环节,同时对后续的特征提取、Odometry以及Mapping部分均有一定的修改,主要包括: 增加Segmentation操作,把...

    LeGO-LOAM算法详解

    整体框架

    LeGO-LOAM算法的总体框架如下图所示:
    在这里插入图片描述
    图中新增加了绿框中的Segmentation环节,同时对后续的特征提取、Odometry以及Mapping部分均有一定的修改,主要包括:

    1. 增加Segmentation操作,把点云投影为距离图像,分离出地面点与非地面点(分割点,segmented point)。
    2. 平滑度计算公式不同,原始LOAM中为使用点集中的坐标相减,而LeGO-LOAM中为使用点集中的欧式距离作差。
    3. 特征点选取中原始LOAM为按照平滑度的值及已有特征点数量分为平面点/边缘点两类,而LeGO -LOAM中不仅考虑到平滑度的值还同时考虑到点的类型为“地面点/分割点”,分别进行提取不同的特征点集。
    4. Lidar Odometry模块相邻帧之间特征点的对应关系也分别按照“地面点/分割点” 进行寻找,使得寻找匹配特征点的效率、精度更高。
    5. Lidar Mapping模块中LeGO-LOAM提供了基于传感器视野范围与基于图优化获取的两种获取特征对应点方法。此外,还加入了iSAM2进行后端优化(闭环检测)。

    综上,LeGO-LOAM的总体思路与LOAM基本相同,1)把原始点云投影为距离图像,并在进行特征提取之前区分为“地面点/分割点”。2)对具有相同类别的特征点进行匹配。3)加入iSAM2进行闭环检测。这三点是其主要创新。

    1 Segmentation模块

    文章中采用Velodyne VLP-16激光雷达采集的数据进行实验,首先把原始点云重投影为一个距离图像,分辨率为1800*16(因为VLP-16水平分辨率为0.2°,360/0.2 = 1800,同时垂直方向上位16线数据)。重投影之后,三维点云变为二维图像,以像素点到传感器之间的距离作为像素值。

    以VLP-16竖直维度的特性来进行标记地面点和非地面点,其在垂直方向的扫描范围为[-15°, 15°],认为地面点出现在[-15°, -1°]之间的扫描线上,而被标记的地面点可以不用进行后续的分割。

    随后,将距离图像分割为很多个聚类,同一个聚类的点被标记上唯一的标识。点数较少的聚类(少于30点)被作为噪声去除,这一步可以减少室外环境噪音点的干扰,如随风飘动的树叶,地面上的杂草等不稳定的特征,这样的处理就可以保留原始的地面点和相对较大的静态物体(树干,楼房等)来进行后续的特征提取工作了。如下图中 (a)为原始点云,(b)为进行分割处理后的点云。
    在这里插入图片描述

    2 Feature Extraction模块

    这一模块在计算平滑度时与经典的LOAM相似,考虑到特征点在各个方向分布均匀,把距离图像水平均分为若干个子图像(360°划分为六等分)。随后对t时刻点云 P t P_t Pt中的每一个点 P i P_i Pi左右各选取5个点组合成点集 S S S,通过下式计算平滑度。
    在这里插入图片描述
    这里采用了距离图像中激光点到传感器之间的距离,与原始LOAM中直接使用两点坐标不同。
    A \mathbb{A} A
    随后,从子图像中的地面点中选取 n F p n_{F_p} nFp个平面点 F p F_p Fp,从分割点中选取 n F e n_{F_e} nFe个边缘点 F e F_e Fe
    此外,从子图像中的地面点与分割点中选取 n F p n_{\mathbb{F}_p} nFp个平面点 F p \mathbb{F}_p Fp, 从子图像中的非地面点中选取 n F e n_{\mathbb{F}_e} nFe个边缘点 F e \mathbb{F}_e Fe,即存在以下关系, F p F_p Fp F p \mathbb{F}_p Fp以及 F e F_e Fe F e \mathbb{F}_e Fe
    如下图中所示,( c ) 中为有类别约束的平面点 F p F_p Fp及边缘点 F e F_e Fe,(d) 中为子图像中所有的平面点 F p \mathbb{F}_p Fp与边缘点 F e \mathbb{F}_e Fe
    在这里插入图片描述

    3 LiDAR Odometry

    特征提取时,我们得到了四种特征点集{ F p F_p Fp, F e F_e Fe, F p \mathbb{F}_p Fp, F e \mathbb{F}_e Fe}。为了更好的寻找相邻两帧点云数据之间的对应特征点对,采用如下图所示的思路进行优化:
    在这里插入图片描述
    上图中的流程可以总结为:

    • 对于平面点:在 F p ( t − 1 ) \mathbb{F}_p^{(t-1)} Fp(t1)中具有分割点标签的点云中寻找 F p t F_p^t Fpt的对应的关联点。
    • 对于边缘点:在 F e ( t − 1 ) \mathbb{F}_e^{(t-1)} Fe(t1)中具有分割点标签的点云中寻找 F e t F_e^t Fet的对应的关联点。

    这可以使缩小对应点的候选范围,同时提高匹配的精度和效率。有了对应的特征点对之后,需要对其进行优化求解六个姿态变换参数,LeGO-LOAM使用两步LM优化方法进行处理。

    首先,采用平面点 F p ( t − 1 ) \mathbb{F}_p^{(t-1)} Fp(t1) F p t F_p^t Fpt对应的约束,优化计算得到了{ t z t_z tz, t r o l l t_{roll} troll, t p i t c h t_{pitch} tpitch}。随后,基于 F e ( t − 1 ) \mathbb{F}_e^{(t-1)} Fe(t1) F e t F_e^t Fet对应的约束,以及之前优化得到的{ t z t_z tz, t r o l l t_{roll} troll, t p i t c h t_{pitch} tpitch},进行优化得到{ t x t_x tx, t y t_y ty, t y a w t_{yaw} tyaw}。最终,两次优化结果融合,得到最终的变换参数{ t x t_x tx, t y t_y ty, t z t_z tz, t r o l l t_{roll} troll, t p i t c h t_{pitch} tpitch , t y a w t_{yaw} tyaw}。

    4 LiDAR Mapping

    作者在这里介绍了两种方法:1)基于传感器视域。 2)基于图优化。分别来把上一步输出的点云与局部位姿变换到全局地图中。

    其中,第一种方法将全局点云地图分割为很多cube,然后根据传感器的有效探测距离(VLP-16为100m)选取一定数量的cube,组合成前 t − 1 t-1 t1 时刻获取的点云地图 Q ( t − 1 ) Q^{(t-1)} Q(t1),然后对当前帧 Q t Q^t Qt与其相交的部分进行匹配,与经典的LOAM算法基本相同。

    第二种方法加入了位姿图和回环检测 (iSAM2),提高了建图效率与精度,但是目前还不太理解,这里就先不做详细的介绍了。

    展开全文
  • LeGO-LOAM算法流程

    2021-03-01 17:15:14
    https://www.cnblogs.com/zhangjcblog/p/10580683.html
  • 目录 引言 引言 LOAM: Lidar Odometry and Mapping in ...本系列文章将为各位同学系统梳理LOAM算法论文和代码中涉及的重点和难点。 LOAM在KITTI上的排名 传送门: 论文:www.ri.cmu.edu/pub_files/2014/7/...
  • loam算法仿真使用数据集 https://github.com/laboshinl/loam_velodyne nsh_indoor_outdoor.bag下载链接 链接:https://pan.baidu.com/s/1lh57y9ua8J6BaINapo4BcQ 提取码:1zip 下载到工作空间后编译 运行: roslaunch...
  • LOAM算法核心解析之特征点的提取(一) 本篇文章接好LOAM中的特征点提取算法,以及其特征点的定义方法。 LOAM算法整体的流程与传统的基于特征点的视觉SLAM很相似: 第一步、提取点云中的特征点; 第二步、使用最近邻...
  • LeGO-LOAM将当前帧扫描到点云特征(线特征和面特征)与当前帧周围的点云地图进行匹配,进行位姿优化。 代码分析 从主函数开始分析,主函数分为2个主要的功能,一是启动单独的线程进行回环检测,二是在循环中执行主...
  • 说明: 简要记录一下速腾16线激光雷达即RS-16在ubuntu18.04系统下跑SC-A-LOAM算法的过程,因为SC-A-LOAM算法源码不支持RS雷达的数据,大致思想就是先将rs雷达发布的激光数据转换为velodyne类型的数据,考虑到接下来...
  • 一、运行环境 环境:ubutu18.04+ros:melodic+pcl:1.8+gtsam+metis 环境配置: Eigen 3.3.4 PCL 1.8.1 (1.11不能用) ...首先,要注意的是SC-LeGo-LOAM和16线雷神激光雷达源码的版本不一样,源码也可能稍有
  • loam算法学习整理

    2020-02-08 18:31:59
    作者研究方向包括3D建图部分,目前个人已知的比较好的三维实时建图算法主要包括loam和cartographer两...这篇博客主要是我学习loam算法整理的一些东西,会随着学习的深入随时增添内容,所以所有的内容都会按照时间顺...
  • SLAM算法 -LOAM框架分析(一)

    千次阅读 2022-02-26 22:21:45
    2 LOAM前端 LOAM的整体思想就是将复杂的SLAM问题分为:1. 高频的运动估计; 2. 低频的环境建图。
  • Loam算法笔记

    2020-09-07 09:44:37
  • 在Ubuntu18.04中使用gazebo配合LOAM算法仿真

    千次阅读 多人点赞 2021-02-07 13:05:29
    目录 写在前面 操作 环境准备 代码准备 LOAM代码 gazebo代码 联合运行 总结 写在前面 我的本科毕设需要使用到LOAM算法,最近因为疫情原因,可能没有小车来操作,故学习了如何在gazebo中进行实物仿真。目前实现了...
  • 由于本人的本科毕业设计需要在Ubuntu环境下实现无人车和无人机的Gazebo与Loam算法实时仿真,因此通过互联网搜索相关教程,在这里感谢两位大佬的分享:无人车的Gazebo与Loam仿真方法和无人机的Gazebo与Loam仿真方法。...
  • 点击上方“3D视觉工坊”,选择“星标”干货第一时间送达作者丨paopaoslam来源丨泡泡机器人SLAM标题:F-LOAM : Fast LiDAR Odometry and Mapping作者:Han Wang, Chen Wang, Chun-Lin Chen, Lihua Xie来源:IROS 2021...
  • The Point Cloud Library (PCL) is a standalone, large scale, open project for 2D/3D image and point cloud processing. PCL is released under the terms of theBSD license, and thus free for commercial and...
  • LOAM 论文详细解读摘要引言相关工作符号和任务描述系统概括A 硬件部分B 算法系统概述激光雷达里程计A 特征点提取B 找特征点的匹配对C 运动估计lidar 建图 摘要 提出了一个实时激光里程计并建图的算法,使用的是一个...
  • 点击上方“3D视觉工坊”,选择“星标”干货第一时间送达一、前言LOAM[1]是Ji Zhang于2014年提出的使用激光雷达完成定位与三维建图的算法,即Lidar Odometry an...
  • LeGo-LOAM是一种轻型、地面优化的激光雷达测程测图方法,用于实时六自由度姿态估计。LeGO-LOAM是轻量级的,因为它可以在低功耗的嵌入式系统上实现实时的姿态估计。 系统接收来自3D激光雷达的输入并输出6个DOF姿势...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 1,579
精华内容 631
关键字:

loam算法

友情链接: Key.rar