【阅读笔记】Pitch angle estimation using a Vehicle-Mounted monocular camera for range measurement

Li, B., Zhang, X., & Sato, M. (2014). Pitch angle estimation using a Vehicle-Mounted monocular camera for range measurement. International Conference on Signal Processing Proceedings, ICSP, 2015–January(October), 1161–1168. https://doi.org/10.1109/ICOSP.2014.7015183

贡献

本文提出了一种不发生累积误差的俯仰角估计方法，仅用车载单目摄像机即可估计初始俯仰角。 该方法通过计算Harris角点和用金字塔Lucas-Kanade方法检测单目相机相邻帧间特征点的光流，利用SFM方法检测的光流估计相机的自运动参数，包括旋转矩阵和平移向量，再使用高斯-牛顿法对估计的参数进行优化

本文还提出了一种利用平移矢量估计路面倾角的方法。 采用平均传递法，对相邻帧旋转矩阵分解得到的俯仰角和俯仰角改变速度进行组合，实现了高精度俯仰角估计

俯仰角计算的必要性

车载单目相机通过俯仰角估计距离的误差和俯仰角测量的精度有很大的关系，并且这个关系是非线性的

[外链图片转存失败(img-bs1JOZO9-1567392116359)(file:///C:/Users/Avins/AppData/Local/Temp/msohtmlclip1/01/clip_image002.png)]

当距离大于等于20m时，俯仰角估计误差再0.5度时，距离估计的误差就会超过15%，而且，误差会随着距离的增大而增大，呈一个非线性正相关

俯仰角估计步骤

1. 首先，从当前帧和前一帧检测Harris角
2. 然后，对图像中每个区域的邻帧角的亚像素精度光流进行估计
3. 利用SFM和RANSAC算法对相邻帧的旋转矩阵和平移向量进行分解
4. 利用运动立体法从旋转矩阵和平移向量的不同组合中选择合适的自运动参数对
5. 利用高斯-牛顿法对自运动参数进行了优化
6. 利用旋转矩阵分解俯仰角速率，利用平移矢量计算俯仰角速率
7. 利用平均传递法将俯仰角与俯仰角速度结合起来，得到了高精度的俯仰角

流程图如下，接下来讲解每一部分是如何运行的。

角点和光流的检测

Harris角点检测方法很成熟，Opencv有定义好的函数，故不在这里展开。推导过见[这里]

相机运动估计

采用SFM方法估计相机运动时，先要车载单目相机坐标系转换成标准的相机坐标系，也就是将焦距置为1。$p_1$和$p_2$ 代表现在帧和前一帧对应Harris角点的其次坐标，那么存在以下约束

$$p_2^{T}E{p}_1=0$$

式子中EE表示本质矩阵，本质矩阵可以用随机的8对Harris角点求解出来，而得到这些角点可以使用RANSAC算法。

本质矩阵可以分解成旋转矩阵****RR 和 平移向量tt的叉乘矩阵**[T]_x[T]x******，如下式

平移向量tt的叉乘矩阵[T]_x[T]x**表示如下

如果没有相机帧率和车速的信息，则无法使用SFM方法获取平移向量tt的尺度信息。在这种情况下，平移向量被归一化为|| t || = 1。此外，由于旋转矩阵R是正交矩阵，本质矩阵E的Frobenius范数可以归一化为

因为旋转矩阵R是一个正交矩阵,因为平移向量tt的叉乘矩阵[T]_x[T]x**是一个斜对称的矩阵，所以可以通过对本质矩阵使用SVD的到上面的两个矩阵。

下面的图片和公式所示，采用运动立体(motion-stereo)法，利用每个候选解对计算三维空间中对应角点的深度Z。 因为从图像中检测到的所有角都在相机前面，所以角的深度Z应该大于相机的焦距。 因此，使用不同的候选解对计算角的深度Z，选择角点个数最大且深度合理的解对作为相机自运动的合适参数。

[外链图片转存失败(img-sN1lensX-1567392116373)(file:///C:/Users/Avins/AppData/Local/Temp/msohtmlclip1/01/clip_image008.png)]

[外链图片转存失败(img-THtyggY6-1567392116374)(file:///C:/Users/Avins/AppData/Local/Temp/msohtmlclip1/01/clip_image010.png)]

使用高斯-牛顿法进行优化

为了从精度上优化运动方法对结构运动参数的估计，采用高斯-牛顿法使角点的投影误差最小

估计俯仰角和俯仰角变化速率

如式(12)所示，基于SFM和高斯-牛顿法得到的旋转矩阵R，并用罗德里格斯旋转公式可以分解相机自运动的俯仰角、偏航角和滚转角。其中r的长度表示旋转的角度。

[外链图片转存失败(img-umlUffEo-1567392116375)(file:///C:/Users/Avins/AppData/Local/Temp/msohtmlclip1/01/clip_image012.png)]

实验结果

不同距离时通过俯仰角估计距离的误差。

[外链图片转存失败(img-n4kvmahF-1567392116377)(file:///C:/Users/Avins/AppData/Local/Temp/msohtmlclip1/01/clip_image014.png)]

总结

本文提出估计高精度俯仰角估计的方法，以提高车载单目摄像头距离估计的精度。首先提取Harris角点，再对相邻2帧进行特征点匹配，并用RANSAC剔除离群点，然后用8点法估计出本质矩阵，并用高斯牛顿法进行优化。最后通过罗德里格公式得出俯仰角，从而取得了较高的距离估计。