基础知识
本文主要是个人在学习过程中的笔记和总结，如有错误欢迎留言指出。也欢迎大家能够通过我的邮箱与博主进行交流或者分享一些文章和技术博客。
首先我们来学习一下在自动驾驶领域中常见的坐标系之间的关系，如图所示：
自动驾驶中坐标系的一般定义如图所示
默认摄像头的坐标系对应于车辆的“右”、“下”和“前”方向
这里首先描述一下如何在世界坐标和相机坐标之间进行变换，但上述任意两个参考帧之间的变换也适用相同的数学原理，世界坐标系中的点（Xw，Yw，Zw）T 通过旋转矩阵R（更精确地说，R∈SO（3））和平移向量t映射到相机坐标系中的点∈R3×3
再介绍旋转的偏航角（yaw）、俯仰角(pitch)和滚动(roll)的概念：实际的相机帧和默认相机帧之间的一般旋转可以通过以下方式描述：
首先围绕Zd轴旋转一个角度（称为滚动角度），然后旋转另一个角度（称为俯仰角度）围绕Xd轴，最后是围绕Yd轴的另一个角度（称为偏航角）。如下图所示：
如果我们将滚动角（roll）的余弦和正弦分别定义为cp和cr，则滚动旋转矩阵为
定义俯仰角(pitch)的余弦和正弦为cp和sp，偏航角(yaw)的余弦和正弦为cy和sy，俯仰和偏航旋转矩阵为
那么最终的旋转矩阵则可以通过横滚、俯仰和偏航矩阵相乘表示为
消失点计算俯仰角和偏航角
我们知道，车辆行驶轨道或车道线基本上是平行的，但是，如果我们用相机拍摄轨道或道路的图像，我们会发现图像中的轨道线或车道并不平行。这些线在图像中相交的点称为消失点。
使用这种消失点方法来计算相机位姿，实际上我们只能恢复相机的偏航和俯仰,从直觉上讲，消失点方法无法恢复横滚角和平移，因为消失点不受这两种方法的影响！
默认的车辆俯仰角和偏航角横滚角的定义
不同roll角的图像可视化：
roll =20度
roll =0度
roll =-20度
我们知道在世界坐标系中，这些平行线永远不会相交，所以我们说消失点在无穷远处，假设汽车的前进方向与车道线对齐，我们可以说车道线的相交点的坐标的Z=∞, 其中（X，Y，Z）是道路参考坐标系中一点的坐标。
我们刚刚所做的假设非常非常重要：车辆与车道对齐，车道笔直，然后，图像中车道线的交点（消失点）将为我们提供有关摄像头安装的位姿信息，即摄像头相对于车辆的方向，否则，它只能告诉我们车辆相对于车道线的方向。
公式推导
首先根据相机的投影方程
因为这是齐次坐标中的一个方程，我们可以将两边乘以1/Z，并将这个数字换算到左边的λ中：
如果让Z为无穷大，则得到图像空间中消失点的坐标（u，v）：
我们定义p∞=（u，v，1）T作为消失点，我们将变换矩阵的分量表示为
现在，将变换矩阵与（0,0,1,0）T相乘将消除第1、2和4列，只剩下第3列，其值为Rxz、Ryz和Rzz。让我们用r3表示该列。然后
旋转矩阵的列始终是长度为1的向量（单位向量），因此∥r3∥=1.因此，λ等于
最终得到r3 表达式
用于相机位姿估计的消失点方法的思想如下：首先确定图像中的消失点（u，v），因为该点是车道线相交的点，这就产生了p∞=（u，v，1）T，由于我们知道内参矩阵K，我们可以使用上面的公式计算r3。当然，r3只是旋转矩阵三列中的一列，但正如计算结果r3显示的，其包含足够的信息来确定旋转的偏航角和俯仰角，如果我们假设滚动角为零（当然是近似值），我们是可以计算整个旋转矩阵的。
我们对r3进行进一步的推导和分解
根据横滚、俯仰和偏航矩阵表达相机的旋转矩阵
此旋转矩阵的第三列为
如果我们确定图像中的消失点（u，v），我们就知道p∞=（u，v，1）T，因此我们可以计算r3=（Rxz，Ryz，Rzz）T的值，根据上面r3的公式，对于α和β，通过求解等式r3，我们得到：
因此，我们从消失点推导出了俯仰角和偏航角！
