IROS 2023 港科大
文章链接:http://arxiv.org/abs/2307.11653
该方法仅依赖于单个摄像头和里程计(如VIO、LIO)来生成基于样条的地图。将车道关联过程建模为利用二分图的分配问题,并通过结合Chamfer距离、位姿不确定性和横向序列一致性为边赋予权重。
本文贡献
- 在线单目车道建图系统,包括车道跟踪和地图优化子系统,直接输出由Catmull-Rom样条曲线表示的轻量级实例级车道地图,无需离线矢量化。
参考点
- 用控制点的方式代替catmull-Rom样条。
- 用二分图进行关联。
系统架构
本系统由两个子系统组成:车道跟踪和地图优化,
在车道跟踪中,神经网络直接基于输入图像预测3D车道标记,然后这些预测结果经过进一步处理以满足后续需求,随后处理后的车道标记与地图中的车道标记进行关联,并结合里程计提供的姿态信息,最后根据关联结果更新姿态信息。
在地图优化中,样条曲线首先从零开始进行初始化,或者根据新获得的检测结果对原始样条曲线进行扩展,最后应用增量优化框架iSAM2,将新的观测结果添加到地图中,以逐步更新地图中的样条曲线,同时不丢失过去观测的信息。
技术点
3D车道线检测
HDMapNet和MapTR或者其他直接从BEV图像中检测车道的算法,不能恢复高度信息。
本文使用Persformer (开源算法)3D的检测算法,通过创建3D车道的anchors来恢复offset和高度。
车道线表示
每个分段的样条曲线有N segment,每2个segments共享3个控制点,每个segment有4个相邻的控制点,这4个控制点用来计算曲线上的点。
上式中,右侧为4个控制点,τ控制曲线的弯曲程度,经常设置为0.5。
如何生成spline的控制点?均匀采样?
文中设置chord length为3m,。
车道线关联
二分图,KM算法,关键点是如何选择edge和他们的权重。
- 车道线有相同的类型
- 从spline上采样点,计算两个点云的Chamfer距离
Chamfer距离计算方法【1】
如上图中的匹配示例,有4个新的感知结果、5个地图元素。
1. 先根据欧式距离筛选一下,得到图中的7个匹配关系,红色线表示两个匹配关系是错误的,但是由于计算出的距离小于设定值。
2. 每个匹配对作为graph中的vertex,两个vertices的横向顺序作为edge,例如u3的得分通过下式计算:
因子图优化
文中提到使用GTSAM中的ISAM2进行优化。
Factor 1
Factor 2
结果
本文提议方法在OpenLane车道基准测试上进行了评估,该基准测试建立在Waymo数据集上,本文选择这个数据集,因为它具有3D车道标记和实例级跟踪标注,OpenLane包含1000个带标注的道路段,其中798个用于训练车道检测,202个用于评估映射质量和姿态估计。
开源代码部分
LaneMapping
继承关系为:
LaneMapping==》LaneOptimizer==》LaneTracker==》LaneUI
初始化
load_data函数
将rosbag内的数据读取出来,将lanes_gt和lanes_predict对其到gt_pose_wc的时间,其中需要调用process_lanes处理预测和真值车道线点。将数据保存到frames_data中。
LaneUI::preprocess_lanes
将感知的点按照等间隔(pp_ds)抽样。把所有的点按照弦的方向(首点到尾点到方向)进行旋转,按照3阶函数拟合f_yx()和f_zx()。如果需要平滑,x轴按照间隔进行采样,生成新的点;否则,剔除拟合点与原点距离大的原点(注意拟合点只有来删除原始点中的毛点)。最后将点进行反旋转返回。
process
LaneTracker::odometry()
- 在gt_pose中增加噪声模拟里程计3D位姿。
- 预测的车道线点只保留兴趣区域内的点。
- 将预测的位姿、车道线点、时间戳封装到Frame——cur_frame中。
LaneTracker::lane_association()
KnnASSOC类中进行检测点和地图点到匹配关联。
LaneOptimizer::map_update()
使用kd-tree方法在地图中的控制点队列中查找4个控制点,LinkedPoints::find_footpoint
两种求解器使用方法:
当use_isam为true时,一直向isam(ISAM2)求解器中添加vaules和factors,不进行删除。
否则,创建了一个key frame的滑窗去维护,每次把滑窗内的所有key frame重新创建graph使用GaussNewtonOptimizer优化,而不是使用gtsam中的滑窗优化器。
Factors:
- chordal_factor:lane 控制点间的factor,BetweenFactorPoint3,观测信息是创建控制点时相邻两个控制点的距离
-
lane_factor:车道线检测点与4个控制点构成的catmull-rom样条间的factor,检测点在catmull-rom样条中的参数u是根据上次的控制点、当前位姿、检测点坐标计算出来的,只在创建graph之前计算一次。
其他内容
catmull_rom_interpolate
git开源代码中有一个example/demo_vurve_fitting.py,直接运行这个程序,比较几种BSpline拟合算法(chord_length、xyz_norm、iterative、grid方法)的效果和Catmull-Rom Spline方法的效果。
相对其他方法,Catmull-Rom Spline的效果最好,但需要选择合适的,从图中看
=0.5的效果相对
=0.1较好。
Catmull-Rom Spline方法拟合效果:
BSpline拟合效果chord_length方法:(绿点为感知的车道线点,红色小点为拟合的点,大点为控制点)
BSpline拟合效果xyz_norm方法:
BSpline拟合效果iterative方法:
BSpline拟合效果grid方法:
引用:
1. https://blog.csdn.net/weixin_42894692/article/details/106148094