[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
数据预处理
- 数据清洗
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices); removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE); - 按线数保存的点云集合
float relTime = (ori - startOri) / (endOri - startOri); point.intensity = scanID + scanPeriod * relTime; laserCloudScans[scanID].push_back(point); - 曲率计算 (使用每个点的前后五个点)
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
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)
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_alast_point_b
correspondence for plane features
当前点 curr_point 与 面 匹配，找到面的法向量
KDTree 搜索最近的5个点（最远点距离小于1米），计算面的法向量
运动估计 ICP
残差度量方式
- 点到线段距离
- 点到面距离
计算出的位姿修正Odometry的位姿
地图增长
获得 laserCloudCornerArray 和 laserCloudSurfArray，并降采样；当地图逐渐累加时，珊格之外的部分就被舍弃，这样可以保证内存空间不会随着程序的运⾏⽽爆掉，同时保证效率。
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);
}
}