一、对应源文件
add_executable(featureAssociation src/featureAssociation.cpp)
二、原理
1. 点云特征提取
1.1 单个像素平滑度计算
选择同一行左右分别 个点云
1.2 切分 sub-images
1800 x 16 分割为 6 个 300x16 的子图
1.3 每个子图上进行操作
(1). 平滑度阈值
平面点:
边缘点:
(2). 初始筛选特征点
每行选择前 个
最大的边缘点(不能是地面点云)
每行选择前 个
最小的边缘点(可以是地面点云)
1.4 二次特征点筛选
6 个子图的所有特征点汇总,得到: 和
从 中,每行选择
个
最大的边缘点(不能是地面点云),组成集合:
从 中,每行选择
个
最小的边缘点(必须是地面点云),组成集合:
1.5 结果可视化
c: 和
d: 和
2. 特征关联
2.1 关联原则
边缘特征:第 帧的
和 第
帧的
平面特征:第 帧的
和 第
帧的
关联方法和 LOAM 基本一致,除了下面的标签匹配
2.2 标签匹配
(1)对于平面点,仅仅选择 第 帧的
中标记为地面点的点云进行匹配
(2)对于边缘点, 仅仅选择 第 帧的
中 标记为非地面点的点云进行匹配
3. Two-step Odometry Optimization
3.1 残差项构造
边缘特征:点到直线的距离
平面特征:点到平面的距离
3.2 优化方法
Levenberg-Marquardt (L-M) 优化方法
(1)待优化变量:
相邻帧之间的相对变换
(2)step1
依据平面特征,优化
(3)step2
依据边缘特征,优化
,其中 step1 优化出来的
视为常量
注意:虽然 step1 也可以优化出 ,但是精度不高,所以 step2 里需要重新优化
3.3 优势
效率高
By using the proposed two-step optimization method, we observe that similar accuracy can be achieved while computation time is reduced by about 35%
三、ROS interface
1.1 subscribe topic
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);
subLaserCloudInfo = nh.subscribe<cloud_msgs::cloud_info>("/segmented_cloud_info", 1, &FeatureAssociation::laserCloudInfoHandler, this);
subOutlierCloud = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud", 1, &FeatureAssociation::outlierCloudHandler, this);
subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &FeatureAssociation::imuHandler, this);
1.2. publish topic
pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 1);
pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 1);
pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 1);
pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 1);
pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);
pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
pubOutlierCloudLast = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2);
pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);
四、算法 pipeline
1. adjustDistortion
1.1 作用
将所有点云的速度、位置变换至第一个点云时刻坐标系下
1.2 处理流程
for (int i = 0; i < cloudSize; i++) {
point.x = segmentedCloud->points[i].y;
point.y = segmentedCloud->points[i].z;
point.z = segmentedCloud->points[i].x;
// 调整ori大小,满足start<ori<end
float ori = -atan2(point.x, point.z);
if (!halfPassed) {
if (ori < segInfo.startOrientation - M_PI / 2)
ori += 2 * M_PI;
else if (ori > segInfo.startOrientation + M_PI * 3 / 2)
ori -= 2 * M_PI;
if (ori - segInfo.startOrientation > M_PI)
halfPassed = true;
} else {
ori += 2 * M_PI;
if (ori < segInfo.endOrientation - M_PI * 3 / 2)
ori += 2 * M_PI;
else if (ori > segInfo.endOrientation + M_PI / 2)
ori -= 2 * M_PI;
}
// intensity 小数部分保存相对时间
float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;
point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;
// imu rpy, 速度,位置 插值或者补偿
// 更新:imuRollCur, imuPitchCur, imuYawCur
// imuVeloXCur, imuVeloYCur, imuVeloZCur
// imuShiftXCur, imuShiftYCur, imuShiftZCur
if (i == 0) {
// 计算正余弦量
updateImuRollPitchYawStartSinCos()
} else {
// 速度变换至 i=0时刻
VeloToStartIMU();
// 点云位置变换到初始i=0时刻
TransformToStartIMU(&point);
}
}
2. calculateSmoothness
2.1 作用
计算点云的平滑度
2.2 相关变量
// 存储平滑度
cloudCurvature = new float[N_SCAN*Horizon_SCAN];
// 邻域点云被选择标记
cloudNeighborPicked = new int[N_SCAN*Horizon_SCAN];
// 点云标签
// 初始化为0,-1 代表 surfPointsFlat,<=0 代表surfPointsLessFlatScan
// 2代表cornerPointsSharp,标记为1代表 cornerPointsLessSharp
cloudLabel = new int[N_SCAN*Horizon_SCAN];
// 存储平滑度信息及其索引
cloudSmoothness.resize(N_SCAN*Horizon_SCAN);
2.3 处理流程
void calculateSmoothness() {
int cloudSize = segmentedCloud->points.size();
// 取左右相邻5个点云
for (int i = 5; i < cloudSize - 5; i++) {
float diffRange = segInfo.segmentedCloudRange[i-5] + segInfo.segmentedCloudRange[i-4]
+ segInfo.segmentedCloudRange[i-3] + segInfo.segmentedCloudRange[i-2]
+ segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i] * 10
+ segInfo.segmentedCloudRange[i+1] + segInfo.segmentedCloudRange[i+2]
+ segInfo.segmentedCloudRange[i+3] + segInfo.segmentedCloudRange[i+4]
+ segInfo.segmentedCloudRange[i+5];
cloudCurvature[i] = diffRange*diffRange;
cloudNeighborPicked[i] = 0;
cloudLabel[i] = 0;
cloudSmoothness[i].value = cloudCurvature[i];
cloudSmoothness[i].ind = i;
}
}
3. markOccludedPoints
3.1 作用
标记遮挡点云,后续不用于特征提取
3.2 处理流程
选择扫描角度近似,但是深度差值很大的点云
选择深度变化突变的点云
4. extractFeatures
4.1 作用
特征点提取,分为 6 个 subimages,逐行提取边缘点和平面点
4.2 处理流程
// 逐行提取特征点
for (int i = 0; i < N_SCAN; i++) {
// 滤波前原始点云 lessflat
surfPointsLessFlatScan->clear();
for (int j = 0; j < 6; j++) { // 分为 6 个 subimages
// sp, ep 代表第 i 行 第 j 个 subimag 点云起始、结束索引
int sp = (segInfo.startRingIndex[i] * (6 - j) + segInfo.endRingIndex[i] * j) / 6;
int ep = (segInfo.startRingIndex[i] * (5 - j) + segInfo.endRingIndex[i] * (j + 1)) / 6 - 1;
if (sp >= ep)
continue;
// 按照 smoothness 从小到大排序
std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());
// 选取边缘点(一定不能是地面点云)
int largestPickedNum = 0;
for (int k = ep; k >= sp; k--) {
int ind = cloudSmoothness[k].ind;
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > edgeThreshold &&
segInfo.segmentedCloudGroundFlag[ind] == false) {
largestPickedNum++;
// 平滑度最大的 2 个点云属于 cornerPointSharp
// 平滑度最大的 20 个点云属于 cornerPointsLessSharp
if (largestPickedNum <= 2) {
cloudLabel[ind] = 2; // cornerPointSharp
cornerPointsSharp->push_back(segmentedCloud->points[ind]);
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
} else if (largestPickedNum <= 20) {
cloudLabel[ind] = 1; // cornerPointsLessSharp
cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
} else {
break;
}
// 标记邻域点云,避免特征点集中
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
....
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
....
cloudNeighborPicked[ind + l] = 1;
}
}
}
// 选取平面点(一定是地面点云,和论文有出入?)
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++) {
int ind = cloudSmoothness[k].ind;
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < surfThreshold &&
segInfo.segmentedCloudGroundFlag[ind] == true) {
// flat pts
cloudLabel[ind] = -1;
surfPointsFlat->push_back(segmentedCloud->points[ind]);
smallestPickedNum++;
if (smallestPickedNum >= 4) {
break;
}
// 标记邻域点云,避免特征点集中
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++) {
....
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--) {
....
cloudNeighborPicked[ind + l] = 1;
}
}
}
// 保存 LessFlatPts
for (int k = sp; k <= ep; k++) {
if (cloudLabel[k] <= 0) {
surfPointsLessFlatScan->push_back(segmentedCloud->points[k]);
}
}
} // end for j
// surfPointsLessFlatScan 进行下采样
surfPointsLessFlatScanDS->clear();
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.filter(*surfPointsLessFlatScanDS);
*surfPointsLessFlat += *surfPointsLessFlatScanDS;
} // end for i
5. publishCloud
结果可视化,
发布点云:cornerPointsSharp,cornerPointsLessSharp,surfPointsFlat,surfPointsLessFlat
6. updateInitialGuess
todo
7. updateTransformation
todo
8. integrateTransformation
todo
9. 结果可视化
publishOdometry
publishCloudsLast