lego-loam代码分析(1)-地面提取和点云类聚
概述
目前最新开源的3d slam算法lego-loam,为loam的改进版。同时另有高人进行了工程优化。
其原作者github
其工程优化的github
个人对其工程优化后的代码和原理进行了基本分析;
包含:
- imageProjecion
- FeatureAssociation
- mapOptmization
- transformFusion
这四个文件
imageProjecion.cpp
imageProjecion.cpp为独立的线程,接收3D 点云数据,主要针对3D激光器采集的激光点云数据,因为代码中间所采用策略原理基本依赖于3d激光雷达扫描测距方式而来。
本线程主要工作是对3d 点云进行预处理,主要将其地面分割、聚类、非聚类、无序点云变有效点云等功能;
点云预处理基本流程:
- 接收新的一帧cloud数据;
- 初始化临时变量数据;
- 将ros cloud 转换成pcl point格式;
- 采用pcl库剔除无效值;
- 统计雷达数据扫描起始角度和终止角度,包括角度差范围;
- 将无序的点云数据,根据3d雷达(如16线 velodyne)的基本扫描原理,转换为有序点云,包括水平index和垂直index。可认为将3d点云转换为2维平面图像存储。
- 地面和非地面分割;
- 点云聚类
- 发布点云
获取点云角度范围 (findStartEndAngle())
先提供一张图,为vlp雷达驱动中计算点云坐标的方法,同时也是依据此还原序列的点云和距离信息。
从上图可看出,和一般单线激光雷达还有ros中scan的格式有一定差距,主要是水平扫描角度
α
\alpha
α 的表示意义差距较大。因此后续的许多坐标转换都和此有关系,其中
α
\alpha
α为vlp16驱动中提供的扫描角度,因此后续畸变矫正等操作涉及扫描时刻和扫描方向也与此有关;
// 查找整个点云数据起始角度和终止角度
void ImageProjection::findStartEndAngle() {
// start and end orientation of this cloud
auto point = _laser_cloud_in->points.front();
_seg_msg.startOrientation = -std::atan2(point.y, point.x); // 起始角度
point = _laser_cloud_in->points.back();
_seg_msg.endOrientation = -std::atan2(point.y, point.x) + 2 * M_PI; // 终止角度 + 360
if (_seg_msg.endOrientation - _seg_msg.startOrientation > 3 * M_PI) { // 起始角度和终止角度 放在0~360之间
_seg_msg.endOrientation -= 2 * M_PI;
} else if (_seg_msg.endOrientation - _seg_msg.startOrientation < M_PI) {
_seg_msg.endOrientation += 2 * M_PI;
}
_seg_msg.orientationDiff = // 终止与起始角度差
_seg_msg.endOrientation - _seg_msg.startOrientation;
}
由于一个激光器是360度扫描,且VLP16其旋转速度和分辨率可调。存在一周所获取的点云个数可能会有一定跳动,且终点有可能大于此次起点的角度,也有可能未超过起点角度,故起始和终点角度在360左右,即可能超出360度,而不是一个准确常数。因此终点时刻的角度需要考虑是否进行2*PI翻转。
注:_seg_msg.startOrientation = -std::atan2(point.y, point.x); 此处角度求解形式与常见不同,主要根据传感器驱动有关。loam源码仅适合于VLP系列,若更换其他雷达,需要改正坐标系相关代码才可使用。其中取负号,猜测应该方便后续的计算,即逆时针旋转与笛卡尔坐标系角度方向一致,而VLP16是顺时针旋转
无序的点云变为有序(projectPointCloud())
由于点云数据内存储每个点的信息为x,y,z为笛卡尔坐标,但是无法获知每个点云之间的相互关系。由于16线激光雷达实际上是16个激光探头同时旋转360度获取的距离信息。因此原数据为16组和单线激光雷达组成。根据所使用雷达已知参数(如16线雷达,包含16组,起始角度为-15度,终止的角度为15度,;每组中包含1800激光点);
因此变为有序点云方法如下。
- 获取此点到达激光头的距离
loat range = sqrt(thisPoint.x * thisPoint.x + // 反推点的距离
thisPoint.y * thisPoint.y +
thisPoint.z * thisPoint.z);
- 获取此点与x,y轴平面的夹角
float verticalAngle = std::asin(thisPoint.z / range); // 获取z轴的角度
- 获取垂直方向上的索引
int rowIdn = (verticalAngle - _ang_bottom) / _ang_resolution_Y; // 获取 扫描线中的索引号,_ang_bottom为起始角度,_ang_resolution_Y为垂直角度分辨率
4.获取此点与x,y轴平面内与x轴的夹角,并获取水平方向上的索引
float horizonAngle = std::atan2(thisPoint.x, thisPoint.y); // x/y ,范围为-PI~ PI, pi/2 表明为x轴方向
int columnIdn = -round((horizonAngle - M_PI_2) / _ang_resolution_X) + _horizontal_scans * 0.5; // ??? 不知道为什么这么绕。 表明x轴方向为中间索引号
如此可获取16个在不同垂直(即俯仰角度)下的16组1维激光数据,并按顺序存储;
注意:理论上3d激光雷达与深度摄像机原始数据本身为有序的2维矩阵,但是ros驱动一般发布的为点云格式数据,因此此操作可认为是将点云格式还原回原始数据。如果针对使用的传感器,可修改相应的ros驱动相应直接发布有序序列。如此可利用激光扫描原理和有序性进行后续的分类和特征提取
地面分割groundRemoval()
for (size_t j = 0; j < _horizontal_scans; ++j) {
for (size_t i = 0; i < _ground_scan_index; ++i) { // 仅遍历_ground_scan_index
size_t lowerInd = j + (i)*_horizontal_scans;
size_t upperInd = j + (i + 1) * _horizontal_scans;
if (_full_cloud->points[lowerInd].intensity == -1 || // 垂直方向上相邻的两个点有一个存在无效值,????????没看到哪里赋值为无效值,不起任何作用
_full_cloud->points[upperInd].intensity == -1) {
// no info to check, invalid points
_ground_mat(i, j) = -1; // 表明此点无法判断
continue;
}
float dX =
_full_cloud->points[upperInd].x - _full_cloud->points[lowerInd].x;
float dY =
_full_cloud->points[upperInd].y - _full_cloud->points[lowerInd].y;
float dZ =
_full_cloud->points[upperInd].z - _full_cloud->points[lowerInd].z;
float vertical_angle = std::atan2(dZ , sqrt(dX * dX + dY * dY + dZ * dZ)); // 存在bug,我觉的应该是sqrt(dX * dX + dY * dY)
// TODO: review this change, 判断前后两点的角度变化在10度内
if ( (vertical_angle - _sensor_mount_angle) <= 10 * DEG_TO_RAD) {
_ground_mat(i, j) = 1;
_ground_mat(i + 1, j) = 1;
}
}
}
- 由于目前存储的是按序存储,由于激光雷达主要为水平安装一定高度,因此16组雷达中,第7组则一般为水平雷达,因此第7组雷达以下的点云数据有可能为地面点云;注:源码思想是假设地面为平的,同时激光雷达安装也应平行与地面,因此在使用时需要严格按照假设使用,否则无法获得预期效果。同时也是此算法的一个缺点
- 按序存储的16组雷达,实际为可认为是16个上下固定角度间隔布置的单线激光雷达;
- 判断是否为地面的方法为,找到第1组雷达(即朝下的第一组激光点)中每个点,找到对应相邻组中同一水平索引的点。如果此两点的俯仰角变化在一范围内,则认为为同一平面,则为地面上点;
注:水平索引方向相同,从第一行开始判断相邻的点。
4. 提取地面数据,提取非地面和非无效数据;
点云聚类(cloudSegmentation)
点云聚类的目的主要是将相邻较近的点认为为同一物体表面,主要用于后续的特征提取;认为为同一物体的原理,则遍历每个未被分类标记的点进行检测。以当前点开始将其上下,左右四个点分别列入待判断的buffer中,判断一点与其相邻点满足一条件。直到所有点都被分类;
这里的点云类聚仅考虑非无效值和非地面上的其他点云
float d1 = std::max(_range_mat(fromInd.x(), fromInd.y()), // 获取当前点和相邻点,距离较大值
_range_mat(thisIndX, thisIndY));
float d2 = std::min(_range_mat(fromInd.x(), fromInd.y()), // 获取较小值
_range_mat(thisIndX, thisIndY));
float alpha = (iter.x() == 0) ? _ang_resolution_X : _ang_resolution_Y; // 根据相邻方向获取水平或垂直方向的角度分辨率
float tang = (d2 * sin(alpha) / (d1 - d2 * cos(alpha))); // 实际为短线向长线做垂直线, 长线端点离垂线位置,越近,表明越平坦
if (tang > segmentThetaThreshold) { // 越大表明越平坦,表明为同一分类,放入queue,继续扩展分类
queue.push_back( {thisIndX, thisIndY } );
_label_mat(thisIndX, thisIndY) = _label_count; // 将其标记为同一分类
lineCountFlag[thisIndX] = true; // 垂直方向的此行,已分类过
all_pushed.push_back( {thisIndX, thisIndY } ); // 是此分类的,均放入放入all pushed
}
同一类原理如图所示;
水平方向和垂直方向alpha夹角不同,其中tang的角度越大,表明相邻的两个point越接近在一个平面上,故可认为是同一类;
在分类时还需考虑此物体大小限制,过小则不进行具体分类。其原理:
1.同一类别点个数需超过30个;
2.个数超过5个并且在垂直方向上跨过3个区域(因为垂直方向角度分辨率较大);
点云发布
- 发布已分类的点云数据,包含地面和非地面数据;其中地面点云在水平方向上被降采样;
- 发布未被分类的点云(即小物体,孤立的点云簇),未被分类的在水平方向上被降采样;
注意:目前lego-loam假设匀速运动,未考虑点云的畸变处理