Online Monocular Lane Mapping Using Catmull-Rom Spline

 IROS 2023 港科大

文章链接：http://arxiv.org/abs/2307.11653

github：GitHub - HKUST-Aerial-Robotics/MonoLaneMapping: Online Monocular Lane Mapping Using Catmull-Rom Spline (IROS 2023)

该方法仅依赖于单个摄像头和里程计（如VIO、LIO）来生成基于样条的地图。将车道关联过程建模为利用二分图的分配问题，并通过结合Chamfer距离、位姿不确定性和横向序列一致性为边赋予权重。

本文贡献

• 在线单目车道建图系统，包括车道跟踪和地图优化子系统，直接输出由Catmull-Rom样条曲线表示的轻量级实例级车道地图，无需离线矢量化。

参考点

1. 用控制点的方式代替catmull-Rom样条。
2. 用二分图进行关联。

系统架构

本系统由两个子系统组成：车道跟踪和地图优化，

在车道跟踪中，神经网络直接基于输入图像预测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()

1. 在gt_pose中增加噪声模拟里程计3D位姿。
2. 预测的车道线点只保留兴趣区域内的点。
3. 将预测的位姿、车道线点、时间戳封装到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