消失点位姿估计思路

消失点位姿估计思路 参考文章：Online Extrinsic Camera Calibration for Temporally Consistent IPM Using Lane Boundary Observations with a Lane Width Prior（cvpr2020）

最近在想着外参修正的问题，基本的思想是利用车道线的消失点来实现当前的位姿估计。参考了很多文献目前两种方法：
1、利用消失点和内参来计算

np.arctan((vp[1] - v) / fx) * 180 / pi
np.arctan((vp[0] - u) / fy) * 180 / pi

vp是消失点。
这是最简单的方法，可以计算出 俯仰角pitch 和 偏航角yaw
2、即参考文章Online Extrinsic Camera Calibration for Temporally Consistent IPM Using Lane Boundary Observations with a Lane Width Prior实现方法
核心思想是高斯球上两个great circle的交点。great circle是光心与像平面平行线段的单位圆。算法主要思想如下：
1）通过文中的算法1计算出理想的消失点vp和理想的线段集合Lc
2）计算Lc中每条线段的great circle的法向量
3）这些法向量均与VD垂直，所以构成超定方程求解VD
4）依据VD求解pitch和yaw
注意求解的pitch和yaw是弧度，需要转换为角度。
初始化的思路就如上
后面不论是方法1还是2都可以利用KEF来进行帧序列的位姿求解。
具体代码由于种种原因就不放出来了，有问题欢迎打扰。