3D激光SLAM--A-LOAM :前端lidar点特征提取部分代码解读
3D激光SLAM--A-LOAM :前端lidar点特征提取部分代码解读
- A-LOAM代码的结构
- Code
A-LOAM代码的结构
A-LOAM的cpp有四个,其中 kittiHelper.cpp 的作用是将kitti数据集转为rosbag
剩下的三个是作为 slam 的 部分,分别是:
- laserMappin.cpp ++++ 当前帧到地图的优化
- laserOdometry.cpp ++++ 帧间里程计
- scanRegistration.cpp ++++ 前端lidar点预处理及特征提取
本片主要解读 前端lidar点特征提取部分的代码
之前要做点的预处理,这部分的代码在之前分析过了
链接
Code
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//计算曲率
for (int i = 5; i < cloudSize - 5; i++)
{
//计算x维度的曲率
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;
//保存 原来点的索引 因为后面要排序
cloudSortInd[i] = i;
//这俩个是标志位
cloudNeighborPicked[i] = 0;//此标志位置1时代表这个点被选位特征点了
cloudLabel[i] = 0;//特征点的标签
}
计算每个点的曲率
并将每个点的曲率保存在一个向量中
将每个点的索引保存在一个向量中
初始化的点标签向量
这里的计算曲率就是对应的论文中的这个公式
在代码里就是分别求一个轴维度的曲率,以x轴为例,就是
前五个点x的值+后五个点x的值-10*当前点x的值
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//声明 特征点的点云
pcl::PointCloud<PointType> cornerPointsSharp; //角点特征 点云
pcl::PointCloud<PointType> cornerPointsLessSharp; //弱角点特征 点云
pcl::PointCloud<PointType> surfPointsFlat;//面点特征 点云
pcl::PointCloud<PointType> surfPointsLessFlat;//弱面点特征 点云
声明 特征点的点云
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
注意:下面代码是一个大的嵌套循环
按每个scan一个循环
每个scan等分6份,每份一个循环
//声明 特征点的点云
pcl::PointCloud<PointType> cornerPointsSharp; //角点特征 点云
pcl::PointCloud<PointType> cornerPointsLessSharp; //弱角点特征 点云
pcl::PointCloud<PointType> surfPointsFlat;//面点特征 点云
pcl::PointCloud<PointType> surfPointsLessFlat;//弱面点特征 点云 经过体素滤波处理
声明 特征点的点云
//遍历每个scan
for (int i = 0; i < N_SCANS; i++)
{
// 没有有效的点了,就continue
if( scanEndInd[i] - scanStartInd[i] < 6)
continue;
//用来存储不太平整的点 不经过体素滤波
pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
遍历每个scan
当这个scan上面的 起始点和结束点的id差小于6 ,也就是 有效点过少,则不处理这个scan直接continue
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
注意这个循环是嵌套在上面的那个里面的
// 将每个scan等分成6等分 分别计算里面的特征点
for (int j = 0; j < 6; j++)
{
// 计算等分 的 每个起始点 和 结束点 的 id
int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; //起始点的id
int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;//结束点的id
TicToc t_tmp;//算时间用的
//对点云按照曲率进行排序,小的在前,大的在后
std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
t_q_sort += t_tmp.toc();//计算时间
int largestPickedNum = 0;//挑选最大的曲率特征点的个数 即角点的个数
这个地方涉及到数组的处理了,要好好看下
A-LOAM算法为了让特征点分部均匀,将每一条scan分成6份,在每一份中分部提取曲率最大的和最小的几个点作为角点和面点
在这里要把前面的数据给总结下了,要不基本是乱的
laserCloud 是依次放的由 scan1 到 scan16的点云
cloudCurvature 存放的上面的每个点对应的 曲率值
cloudSortInd 存放的点对应的id ,一开始 每个元素是由 5~n,后面会根据曲率大小重新排
scanStartInd[i] 是第i个scan的起始点的id scanEndInd[i] 是第i个scan的结束点的id
sp和ep分别是一条scan上面等分6份,中一份的 起始点id和结束点id
所以计算sp的时候是这样的:
sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6
计算ep的时候是这样的:
ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;
然后下面的代码 sort那部分,是利用std的排序方法,将cloudSortInd 对应的那部分 将点云的id进行了重新排序,曲率小的在前面,曲率大的在后面
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
int largestPickedNum = 0;//挑选最大的曲率特征点的个数 即角点的个数
//挑选曲率比较大的部分
for (int k = ep; k >= sp; k--)
{
//排序后顺序就乱了, 用之前存的 索引值 取到 点的id
int ind = cloudSortInd[k];
重新排序后,cloudSortInd 上的ep位置就是存放的曲率最大点的id,取出来这个id
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//1 看这个点是否是有效点,同时曲率是否大于阈值
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] > 0.1)
{
//2 满足要求,则选择的 角点个数 加1
largestPickedNum++;
// 每段选择2个曲率最大的点
if (largestPickedNum <= 2)
{
//3 label 为2 是曲率大的标记
cloudLabel[ind] = 2;
//4 cornerPointsSharp存放曲率大的点
cornerPointsSharp.push_back(laserCloud->points[ind]);
//cornerPointsLessSharp 存放曲率稍微大的点
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
//5 以及20个曲率稍微大些的点
else if (largestPickedNum <= 20)
{
// 6 label 为1 是曲率稍微大的标记
cloudLabel[ind] = 1;
//7
cornerPointsLessSharp.push_back(laserCloud->points[ind]);
}
//超过20个的就不要了
else
{
break;
}
//8 这个点被选中后 pick标志位置 1
cloudNeighborPicked[ind] = 1;
//9 为了保证特征点不过度集中,将选中的点周围5个点都置1 避免后续会选到
for (int l = 1; l <= 5; l++)
{
//查看相邻点距离是否差异过大,如果差异过大,说明点云在此不连续,是特征边缘,就是新的特征,就不置位了
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
上面的代码对照注释依次做了如下事情,注释里都标了对应的序号:
- cloudNeighborPicked[ind] == 0 代表这个点可以挑选,然后曲率大于0.1 则满足要求
- 满足要求则选择的点个数+1
- 将最大的两个曲率点标签置为2
- 然后将这两个点存入cornerPointsSharp
- 然后再选较大的20个点,
- 标签置位1
- 然后存入cornerPointsLessSharp
- 被选为 角点和弱角点后,点的选取标志位置为1,则这个点就不能在后面被选取了,因为一开始有那个判断
- 为了保证特征点不过度集中,将选中的点周围5个点都置1 避免后续会选到
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
int smallestPickedNum = 0;
for (int k = sp; k <= ep; k++)
{
int ind = cloudSortInd[k];
if (cloudNeighborPicked[ind] == 0 &&
cloudCurvature[ind] < 0.1)
{
cloudLabel[ind] = -1;
surfPointsFlat.push_back(laserCloud->points[ind]);
smallestPickedNum++;
//选择4个面点
if (smallestPickedNum >= 4)
{
break;//够数了则跳出循环
}
cloudNeighborPicked[ind] = 1;
for (int l = 1; l <= 5; l++)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
for (int l = -1; l >= -5; l--)
{
float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
{
break;
}
cloudNeighborPicked[ind + l] = 1;
}
}
}
选择面点的时候和角点基本一致,不再细写.
有区别的地方就是:
只选4个面点,没有像角点一样选几个弱面点,这部分在下面
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//除了角点和弱角点的都是弱面点
for (int k = sp; k <= ep; k++)
{
if (cloudLabel[k] <= 0)
{
surfPointsLessFlatScan->push_back(laserCloud->points[k]);
}
}
弱面点的处理就在接着的下面
通过标签判断,前面的标签已将打上了 角点为-2 ,弱角点为-1 ,面点为1 .剩下的均为弱面点
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//将弱面点 进行体素滤波 因为剩下的都是弱面点了,点就很多了
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);
surfPointsLessFlat += surfPointsLessFlatScanDS;
将弱面点 进行体素滤波 因为剩下的都是弱面点了,点就很多了
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//发布所有的点云
sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "/camera_init";
pubLaserCloud.publish(laserCloudOutMsg);
//发布角点
sensor_msgs::PointCloud2 cornerPointsSharpMsg;
pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsSharpMsg.header.frame_id = "/camera_init";
pubCornerPointsSharp.publish(cornerPointsSharpMsg);
//发布弱角点
sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsLessSharpMsg.header.frame_id = "/camera_init";
pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);
//发布面点
sensor_msgs::PointCloud2 surfPointsFlat2;
pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsFlat2.header.frame_id = "/camera_init";
pubSurfPointsFlat.publish(surfPointsFlat2);
//发布弱面点
sensor_msgs::PointCloud2 surfPointsLessFlat2;
pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsLessFlat2.header.frame_id = "/camera_init";
pubSurfPointsLessFlat.publish(surfPointsLessFlat2);
将上面整理的特征点转为ros的格式,然后发布topic出去
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
以上为A-LOAM 特征点提取部分的代码解读
之前要做点的预处理,这部分的代码在之前分析过了
链接