A-LOAM

本文最后更新于:2023年5月7日 下午

[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的位姿

地图增长

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

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);
}
}

A-LOAM
https://cgabc.xyz/posts/79cfdce/
作者
Gavin Gao
发布于
2022年6月19日
许可协议