@[toc]

语义特征实例跟踪

两个连续帧中的点分别为p和p’，它们之间满足如下投影关系：

其中z_p是p的深度，$$\pi(\cdot)$ $是相机投影模型 ，\pi’(\cdot)是相机逆投影模型。G=[\theta, \phi, d]是相机坐标系下的平面参数。
G \cdot X = [cos\theta cos\phi, cos\theta sin\phi, sin\theta]^TX+d = 0

Hungarian matching strategy

1、实例关联 instance-wise association
我们在常规对象的多边形polygons和车道的 5.0 像素宽度折线上计算IoU。
2、像素关联 pixel-wise association
我们计算其索引关键点的重投影像素距离。
3、outlier
IoU 百分比 < 50% 和像素距离 > 5.0 的匹配将被忽略。

对于在垂直物体（例如杆）中检测到的关键点，我们使用光流方法在帧之间进行跟踪。在特征跟踪期间，我们使用经典的 GFTT 提取器 和 FREAK 描述符来提取和描述关键点

道路车道的表示和初始化

我们使用分段三次 Catmull-Rom 样条曲线通过一系列控制点C_k \in R_3来表示每个 3D 车道的左右轮廓。每4个连续控制点将确定曲线的形状，用C(t’) \in R_3表示这条曲线。如：

其中张力 τ = 0.5 描述了这种曲线在其控制点处的弯曲程度。t′ ∈[0,1]表示 在两个中点 Ck 和 Ck+1 之间的点。为了简化分段样条曲线的符号，我们定义C(t)为一个由M+2个点C_k,k \in [1, M+2]控制的M段分段曲线。第一个和最后一个控制点 $$C_1$ $和 C_{M+2}是 off-the-curve点，用于在其端点C_2和 C_{M+1}处调整曲线的方向。

控制点生成

通过公式（1）求得点集P_{cj}，并在其中随机采样N个点来作为点集C_k, k \in [2,N+1]来评估损失：

• 其中第一个Mahalanobis 项是3D采样点到曲线的残差。为了计算距离，我们通过方程 C′(t_{cj} )(C(t_{cj} ) −P_{cj} ) = 0 逐段显式求解参数t_{cj}。五次方程的实根代表可能的驻点，然后将其与 [2,N + 1] 之间的所有整数进行比较以获取全局最小点到曲线残差。

• 第二个正则化项确保对这些控制点进行均匀采样。我们使用参数 λ_1 = 40m 来控制密度，{}^xΩ = (0.1m)^2I_3, {}^yΩ = (0.25 ·λ_1)^2I_1来平衡它们的权重。

通过求解上述<c_1,β_1></c_1,β_1> 的方程来初始化off-the-curve点C_1（C_{N+2} 类似。N_2 是 C_2 的 3D 法线，通过搜索与地面相交的像素邻域计算得出。这样的方程意味着解决球体和线的可能交叉点。如果我们找到了多个解，我们接受位于C_3 对面的候选 C_1

或者在这样的样本一致性初始化过程中重复挑选 N 个连续样本作为中间控制点，我们首先随机挑选一个样本，然后通过随机挑选距离在 [0.5λ_1,1.5λ_1] 之间的另一个样本在其两侧递归扩展。我们为每个样条生成最多 500 个随机控制点序列，并评估方程中的损失。 3 挑选最佳顺序。与使用 B 样条 [24] 或 polyb ́eziers [25, 26] 的现有作品相比，Catmull-Rom 样条穿过所有中间控制点，以便我们使用这种随机挑选策略进行初始化。

状态估计

变量

1、P_{a^i} : {a^i}的3D坐标： 三角化得到
2、G：地面参数，将每一帧的可观测区域近似为一个平面。使用球面局部参数来更新表示，θ ∈[0,π],\phi∈[0,2π],d ∈R3
3、全局坐标中a 的垂直平面表示为:Va(φ,e) ·X =[cos φ,sin φ,0]^Tx +e = 0, φ ∈[0,2π],e ∈R_3
4、样条 b 的控制点 C_{b^k} ∈R_3，描绘了道路车道的左侧或右侧轮廓
5、动态关联参数 t^c_{b^j} ∈ R 用于关联在帧 c 上检测到的样条 b 的像素p^c{b_j}。为清楚起见，我们使用 j ∈{m,n} 分别索引采样点和像素

约束因子

1、点约束（重投影误差）

2、车道样条曲线约束

t^c_{b^j}:动态联合参数，也加入联合优化。
方差\Omega由检测精度决定。
3、共面约束