3D激光SLAM:A-LOAM :前端lidar点预处理部分代码解读
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
int main(int argc, char **argv)
{
//节点 名称:scanRegistration
ros::init(argc, argv, "scanRegistration");
ros::NodeHandle nh;//ros 句柄
//从配置参数中 读取 scan_line 参数, 多少线的激光雷达 在launch文件中配置的
nh.param<int>("scan_line", N_SCANS, 16);
//从配置参数中 读取 minimum_range 参数, 最小有效距离 在launch文件中配置的 踢出雷达上的载体出现在视野里的影响
nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);
从main函数开始
首先是ros的基本操作,初始化节点和 声明句柄
然后从参数服务器中读取两个参数
- scan_line 多少线的激光雷达 在launch文件中配置的
- minimum_range 最小有效距离 在launch文件中配置的 踢出雷达上的载体出现在视野里的影响
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//打印线束
printf("scan line number %d \n", N_SCANS);
//做一个线束的判断
if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)
{
printf("only support velodyne with 16, 32 or 64 scan line!");
return 0;
}
做一个线束的判断
目前仅支持 16线 32线 64线的 机械式lidar
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//订阅 velodyne 的 lidar消息 收到一个消息包 则进入 laserCloudHandler 回调函数一次
ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);
//定义要发布的消息
pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);
pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);
pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);
pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);
pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100);
pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_remove_points", 100);
本节点订阅和发布的消息
从这里可以看出来,这个节点是接收lidar的原始数据
- “/velodyne_points”
然后发布处理后的数据
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
下面来看这个节点的主要工作,接收到lidar数据后的处理,和特征提取部分,在回调函数laserCloudHandler 中
void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
// 判断系统是否进行初始化, 如果没有 则缓冲几帧 目前 systemDelay为0,自己用可以设置
if (!systemInited)
{
systemInitCount++;
if (systemInitCount >= systemDelay)
{
systemInited = true;
}
else
return;
}
首先是进行一个初始化的判断,前几帧可以不要
源码的systemDelay为0
实际使用的时候可以设置个大小
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//将ros点云转为pcl点云格式
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;//声明pcl点云
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);//将ros点云转为pcl点云格式
将ros点云转为pcl点云格式
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
std::vector<int> indices;//这个变量保存了下面去除nan点的序号
//去除点云中的nan点
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
//去除点云中的距离小于阈值的点
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);
去除点云中的nan点
去除点云中的距离小于阈值的点
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//计算起始点和结束点的水平角度,与x轴的夹角,由于激光雷达是顺时针旋转,这里取反就相当于转成了逆时针
int cloudSize = laserCloudIn.points.size();
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
//atan2的范围是 [-Pi,Pi] ,这里加上2Pi是为了保证起始到结束相差2PI,符合实际
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
laserCloudIn.points[cloudSize - 1].x) +
2 * M_PI;
// 总会有一些例外, 转换到合理的范围内
if (endOri - startOri > 3 * M_PI)
{
endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI)
{
endOri += 2 * M_PI;
}
计算起始点和结束点的水平角度,与x轴的夹角
为了给后面计算相对起点的时间戳用的
这里要说下 机械式lidar的坐标系
所以在求与x轴的夹角的时候是 arctan(y/x)
结束点的水平角加上了2pi,主要的目的是 将角的范围转为 0~360度,因为结束点大部分为负值,比如起点,30度,结束点为-90,转完即为[30,270]
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//遍历每一个点
for (int i = 0; i < cloudSize; i++)
{
point.x = laserCloudIn.points[i].x;
point.y = laserCloudIn.points[i].y;
point.z = laserCloudIn.points[i].z;
//计算俯仰角 单位是角度 目的是用来判断是第几个线束
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
int scanID = 0;//线束的id
//计算是第几个线束 scan
if (N_SCANS == 16)
{
//垂直是30度 每个线之间的夹角是2度
scanID = int((angle + 15) / 2 + 0.5);
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
else if (N_SCANS == 32)
{
scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
if (scanID > (N_SCANS - 1) || scanID < 0)
{
count--;
continue;
}
}
else if (N_SCANS == 64)
{
if (angle >= -8.83)
scanID = int((2 - angle) * 3.0 + 0.5);
else
scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);
// use [0 50] > 50 remove outlies
if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
{
count--;
continue;
}
}
else
{
printf("wrong scan number\n");
ROS_BREAK();
}
//printf("angle %f scanID %d \n", angle, scanID);
遍历每一个点
通过计算的俯仰角度 来判断 这个点 在哪个线的scan上面
float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
计算俯仰角的代码就是根据上面这个图
scanID = int((angle + 15) / 2 + 0.5);
根据俯仰角度求对应的线束就是 根据 上面这图 ，从最下面的线束算1，然后大约每2°，一根线，所以是 (angle + 15) / 2。最后加的0.5是为了进一位，因为是从1开始数的嘛
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//计算水平角
float ori = -atan2(point.y, point.x);
/* 把计算的水平角 放到 开始角度和结束角度 合理 的区间之内 */
if (!halfPassed)
{
if (ori < startOri - M_PI / 2)
{
ori += 2 * M_PI;
}
else if (ori > startOri + M_PI * 3 / 2)//这种情况不会发生
{
ori -= 2 * M_PI;
}
//如果超过了180,就说明过一半了
if (ori - startOri > M_PI)
{
halfPassed = true;
}
}
else
{
ori += 2 * M_PI;
if (ori < endOri - M_PI * 3 / 2)
{
ori += 2 * M_PI;
}
else if (ori > endOri + M_PI / 2)
{
ori -= 2 * M_PI;
}
}
计算水平角
主要有 -pi 到 pi 的区间, 分成两个半圆算的,
主要就是保证把计算的水平角 放到 开始角度和结束角度 合理 的区间之内
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//角度的计算是为了计算相对的起始时刻的时间 为点云畸变补偿使用
float relTime = (ori - startOri) / (endOri - startOri);//计算当前点 在起始和结束之间的比率
//整数部分是sacn的id ,小数部分是相对起始时刻的时间
point.intensity = scanID + scanPeriod * relTime;
//根据scan的ID存入各自数组
laserCloudScans[scanID].push_back(point);
}
角度的计算是为了计算相对的起始时刻的时间 为点云畸变补偿使用
计算当前点 在起始和结束之间的比率
整数部分是sacn的id ,小数部分是相对起始时刻的时间
根据scan的ID存入各自数组
以上从回调函数开始 整个 很多行的代码 就在 完成一个功能,求点 相对的起始时刻的时间
现在有的lidar是把每个点的时间戳自带了
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//当前有效的点云的数目
cloudSize = count;
printf("points size %d \n", cloudSize);
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());//缓存叠加的每条scan的点云
//处理每个scan 每条scan上面的 点的起始 id 为 前5个点不要 结束的id 为后6个点不要
for (int i = 0; i < N_SCANS; i++)
{
scanStartInd[i] = laserCloud->size() + 5;
*laserCloud += laserCloudScans[i];
scanEndInd[i] = laserCloud->size() - 6;
}
处理每个scan 每条scan上面的 点的起始 id 为 前5个点不要 结束的id 为后6个点不要
点很多的,这10个点无所谓,主要是求曲率方便
+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
以上为A-LOAM中点的预处理部分代码内容