Motion Estimation for Self-Driving Cars With a Generalized Camera

Motion Estimation for Self-Driving Cars With a Generalized Camera

摘要

本文提出了基于非完整约束的2-point法计算本质矩阵。提出了至少一对inter-camera匹配即可计算2-point的方法，以及只有intra-camera匹配的特例求解。

通用相机模型

图2为车上的通用相机排布，其中车体坐标系为V，记相机的内参为$K_i$，外参为$T_{C_i}=[R_{C_i},t_{C_i}]$，归一化相机坐标为${\hat{x}}_{ij}=K_i^{-1}{x}_{ij}$。使用6维的Plucker线消除单一相机投影中心描述3D点$X_j$的依赖：

它描述了连接$x_{ij}$和$X_j$的光线，$u_{ij}=R_{C_i}{\hat{x}}_j$为在V系下该光线的单位方向。(1) 将两个图像坐标系的点关联变为两条光线相交，极限约束变为：

其中，$l_{ij,k},l_{ij,k+1}$为第k帧和k+1帧的Plucker线关联。$E_{GC}$为通用本质矩阵，$R$为通用相机系的相对旋转，$E=[t{]}_{\times }R$为原来的本质矩阵。

运动估计

图3为运动估计流程图。2-point RANSAC使用intra-camera关联计算相对运动，对于2-point RANSAC估计出来的yaw角$\theta$接近于0时，使用inter-camera关联通过1-point法计算尺度$\rho$，使用1-point法计算时$\theta$是固定住的。最后，估计得到的$\theta ,\rho$会经过非线性优化和Kalman滤波。

匹配关联

本文的运动估计算法依赖两种关联——intra-camera和inter-camera，前者为同一相机在时序上的关联，后者为不同相机在时序上的关联，如图4所示。在实际中，inter-camera关联会远少于intra-camera，因此我们使用intra-camera关联作为默认case，而在直行case时使用inter-camera关联来恢复尺度。

2-point solution

图5描述了k时刻到k+1时刻的阿克曼转向模型，$V_k$和$V_{k+1}$间的相对运动$R,t$为待估计量，由该模型可得：

其中，$\theta$为yaw角变化量，$\rho$为尺度，$\varphi =\theta /2$。可以看出，两帧之间的相对运动只有2个自由度——尺度和yaw角。将(3) 代入 (2) 的$E_{GC}$，有：

为书写方便，记k时刻的Plucker线为$l=[u^T,(t_C\times u{)}^T{]}^T$，$k+1$时刻为$l^{\prime }=[u^{\prime T},(t_{C^{\prime }}\times u^{\prime }{)}^T{]}^T$，则公式 (2) 可写为：

其中，

式 (5) 为新的GEC约束，需要2组Plucker线关联求解。记Plucker线的系数分别为$(a_1,b_1,c_1,d_1,e_1)$ 和$(a_2,b_2,c_2,d_2,e_2)$，根据三角形半角公式：

我们有：

其中，$\alpha =\cos \frac{\theta }{2}$，$\beta =\sin \frac{\theta }{2}$，联立(7a)(7b)消除$\rho$，我们有：

由于${\alpha }^2+{\beta }^2=1$，将其代入(8)消除$\alpha$，并令$\gamma ={\beta }^2$，则(8)可以简化为：

$A,B,C,D$为由$(a,b,c,d,e)$组成的系数，其中D具有特殊性质：

对于intra-camera关联有$t_C=t_C^{\prime }$，此时最后两项关于a的系数会被消掉，并且有$a=-e$，由此可得$D=0$，因此(10b)变为：

其中一个解为$\gamma =0$，另外两解为$\gamma =\frac{-B\pm \sqrt{B^2-4AC}}{2A}$。将$\gamma$带回$\gamma ={\beta }^2$，可得$\beta$最多有6组解，其中两组为0。最终$\theta$可由$\beta =\sin \frac{\theta }{2}$解出，$\rho$由公式(7)解出。

退化case

当直行时，只有intra-camera关联无法确定尺度，但是至少只要一组inter-camera关联就可以确定尺度。对于inter-camera关联，有$t_C=t_C^{\prime }$，此时$a\ne -e$，因此$\rho$可以由公式(7)确定。这表明，当使用intra-camera关联估计出的内点解为$(\rho =1,\theta =0)$时，可使用inter-camera关联来求解尺度。

鲁棒估计

使用2-point RANSAC剔除外点，对于每个相机，使用估计的R、t和相机外参$T_{C_i}$来计算本质矩阵，通过检查Sampson 误差来计算内点比例。RANSAC的迭代次数为$m=\frac{\ln (1-p)}{\ln (1-v^n)}$，其中$n$为关联对数，$p$为期望内点比例，$v$为所有关联都是内点的概率，假设$p=0.99,v=0.5$，则总共需要16次迭代。

非线性优化

在外点剔除后，进行一次非线性优化来得到准确的$\rho ,\theta$，损失函数为：

其中，$(x_{ij}\leftrightarrow x_{i^{\prime }j})$为前后帧的关联，$X_j$为三角化的3D点，$C$包括了intra-camera和inter-camera的关联。$P_i,P_{i^{\prime }}$为相机投影矩阵：

优化的初值由上一部RANSAC的估计得到。

卡尔曼滤波

我们基于恒速模型实现了对$\rho ,\theta$的卡尔曼滤波，由于车体运动由转向和速度两者控制，因此我们分别对尺度和yaw角实现1D卡尔曼滤波，图6对比了直接估计和卡尔曼滤波的结果，其中蓝色线为直接2-point估计，绿色线为卡尔曼滤波结果。