A-LOAM

本文最后更新于：May 7, 2023 pm

[TOC]

Overview

A-LOAM is an Advanced implementation of LOAM (J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time), which uses Eigen and Ceres Solver to simplify code structure.

Code: A-LOAM 注释版

ROS Graph

Pipeline

Lidar Hardware

Hokuyo UTM-30LX

  • Vertical
    • sweep: \(180^\circ / s\), a rotation from \(-90^\circ\) to \(90^\circ\) or in the inverse direction (lasting for 1s)
    • FOV: \(180^\circ\)
    • scan rate: 40 lines/sec
    • resolution: \(180^\circ / 40 = 4.5^\circ\)
  • Horizontal (a scan plane)
    • resolution: \(0.25^\circ\) within a scan
  • angular speed: \(180^\circ\) between \(-90^\circ\) and \(90^\circ\) with the horizontal orientation of the laser scanner as zero

VLP-16

  • Time of flight distance measurement with calibrated reflectivities
  • 16 channels
  • Measurement range up to 100 meters
  • Accuracy: +/- 3 cm (typical)
  • Dual returns
  • Field of view (vertical): 30° (+15° to -15°)
  • Angular resolution (vertical): 2°
  • Field of view (horizontal/azimuth): 360°
  • Angular resolution (horizontal/azimuth): 0.1° - 0.4°
  • Rotation rate: 5 - 20 Hz

Scan Registration

数据预处理

  • 数据清洗 

    1
    2
    pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
    removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);

  • 按线数保存的点云集合 

    1
    2
    3
    float relTime = (ori - startOri) / (endOri - startOri);
    point.intensity = scanID + scanPeriod * relTime;
    laserCloudScans[scanID].push_back(point);

  • 曲率计算 (使用每个点的前后五个点) 

    1
    2
    3
    4
    5
    6
    7
    for (int i = 5; i < cloudSize - 5; i++) {
      // ...
      cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
      cloudSortInd[i] = i;
      cloudNeighborPicked[i] = 0;
      cloudLabel[i] = 0;
    }

特征提取

根据曲率进行点云特征提取，将每条线上的点分入相应的类别：边沿点和平面点

  • sharp edges
  • planar surface patches

对于每条线

  • 将每个scan的曲率点分成6等份处理,确保周围都有点被选作特征点

对于每一份，曲率大于0.1的点

  • 挑选曲率最大的前2个点放入sharp点集合 cornerPointsSharp，同时 cloudLabel[ind] = 2
  • 挑选曲率最大的前20个点放入less sharp点集合 cornerPointsLessSharp，同时 cloudLabel[ind] = 1
  • 点的前后各5个连续距离比较近的点筛选出去，防止特征点聚集，使得特征点在每个方向上尽量分布均匀

对于每一份，曲率小于0.1的点

  • 放入flat点集合 surfPointsFlat，同时 cloudLabel[ind] = -1
  • 点的前后各5个连续距离比较近的点筛选出去，防止特征点聚集，使得特征点在每个方向上尽量分布均匀

对于每一份，将剩余的点 cloudLabel[k] <= 0（包括之前被排除的点）全部归入平面点 surfPointsLessFlatScan

Odometry（高频率，粗定位）

运动畸变矫正

运动畸变示意图如下

Reprojecting point cloud to the end of a sweep

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
void TransformToStart(PointType const *const pi, PointType *const po) {
  double s;
  if (DISTORTION)
    s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD;
  else
    s = 1.0;

  Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);
  Eigen::Vector3d t_point_last = s * t_last_curr;
  Eigen::Vector3d point(pi->x, pi->y, pi->z);
  Eigen::Vector3d un_point = q_point_last * point + t_point_last;

  po->x = un_point.x();
  po->y = un_point.y();
  po->z = un_point.z();
  po->intensity = pi->intensity;
}

void TransformToEnd(PointType const *const pi, PointType *const po) {
  // undistort point first
  pcl::PointXYZI un_point_tmp;
  TransformToStart(pi, &un_point_tmp);

  Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);
  Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);

  po->x = point_end.x();
  po->y = point_end.y();
  po->z = point_end.z();

  po->intensity = int(pi->intensity);
}

特征匹配 (Scan-Scan)

correspondence for corner features

当前点 curr_point 与 线段 匹配，找到线段的两个端点

  • last_point_a: KDTree 搜索最近的点
  • last_point_b: 在scan增长和下降的方向上分别搜索，不在同一scan但处于一定阈值scan范围内，距离最小的点

correspondence for plane features

当前点 curr_point 与 面 匹配，找到面的三个点

  • last_point_a: KDTree 搜索最近的点

  • last_point_b: 在scan增长(intensity<=closestPointScanID)和下降(intensity>=closestPointScanID)的方向上分别搜索，处于一定阈值scan范围内，距离最小的点

  • last_point_c: 在scan增长(intensity>closestPointScanID)和下降(intensity<closestPointScanID)的方向上分别搜索，处于一定阈值scan范围内，距离最小的点

运动估计 ICP

残差度量方式

  • 点到线段距离
  • 点到面距离

Mapping（低频率，精定位）

基于Cube的地图管理

LOAM采用的是栅格（cube）地图的方法，将整个地图分成21×21×11个珊格，每个珊格是⼀个边⻓50m的正⽅体，当地图逐渐累加时，珊格之外的部分就被舍弃，这样可以保证内存空间不会随着程序的运⾏⽽爆掉，同时保证效率。

特征匹配 (Scan-Map)

将当前帧已经消除畸变的点云转换到全局坐标系 transformAssociateToMap()，然后与局部地图（local map或者称为submap，源码中使用的是三维栅格cube做的局部地图管理）做特征匹配

用于特征匹配的局部地图 (local map)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
int laserCloudValidNum = 0;
int laserCloudSurroundNum = 0;
// 在每一维附近5个cube(前2个，后2个，中间1个)里进行查找
for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++) {
  for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++) {
    for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++) {
      if (i >= 0 && i < laserCloudWidth && j >= 0 && j < laserCloudHeight && k >= 0 && k < laserCloudDepth) {
        laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
        laserCloudValidNum++;
        laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
        laserCloudSurroundNum++;
      }
    }
  }
}

// 构建特征点地图，查找匹配使用
for (int i = 0; i < laserCloudValidNum; i++) {
  *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
  *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
}

correspondence for corner features

当前点 curr_point 与 线段 匹配，找到线段的两个端点

KDTree 搜索最近的5个点（最远点距离小于1米），计算其中心点 center，并构建协方差矩阵；如果是线特征，协方差矩阵最大特征值对应的特征向量即为线的方向向量 unit_direction，然后根据中心点和方向向量得到两个端点

  • last_point_a
  • last_point_b

correspondence for plane features

当前点 curr_point 与 面 匹配，找到面的法向量

KDTree 搜索最近的5个点（最远点距离小于1米），计算面的法向量

运动估计 ICP

残差度量方式

  • 点到线段距离
  • 点到面距离

计算出的位姿修正Odometry的位姿

地图增长

获得 laserCloudCornerArray 和 laserCloudSurfArray，并降采样；当地图逐渐累加时，珊格之外的部分就被舍弃，这样可以保证内存空间不会随着程序的运⾏⽽爆掉，同时保证效率。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
for (int i = 0; i < laserCloudCornerStackNum; i++) {
  pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);

  int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
  int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
  int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

  if (pointSel.x + 25.0 < 0) cubeI--;
  if (pointSel.y + 25.0 < 0) cubeJ--;
  if (pointSel.z + 25.0 < 0) cubeK--;

  if (cubeI >= 0 && cubeI < laserCloudWidth && cubeJ >= 0 && cubeJ < laserCloudHeight && cubeK >= 0 &&
      cubeK < laserCloudDepth) {
    int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
    laserCloudCornerArray[cubeInd]->push_back(pointSel);
  }
}

for (int i = 0; i < laserCloudSurfStackNum; i++) {
  pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel);

  int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
  int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
  int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

  if (pointSel.x + 25.0 < 0) cubeI--;
  if (pointSel.y + 25.0 < 0) cubeJ--;
  if (pointSel.z + 25.0 < 0) cubeK--;

  if (cubeI >= 0 && cubeI < laserCloudWidth && cubeJ >= 0 && cubeJ < laserCloudHeight && cubeK >= 0 &&
      cubeK < laserCloudDepth) {
    int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
    laserCloudSurfArray[cubeInd]->push_back(pointSel);
  }
}
SLAM
#LiDAR SLAM #LIO
A-LOAM
https://cgabc.xyz/posts/79cfdce/
Author
Gavin Gao
Posted on
June 19, 2022
Licensed under