AVM环视系统在自标定中的应用及原理分析

前言

AVM环视系统中相机参数通常是汽车出厂前在标定车间中进行的离线阶段标定。很多供应商还提供了不依赖于标定车间的汽车自标定方法。自标定指的是：汽车在马路上慢速行驶一段路，利用车道线等先验信息标定出相机的外参。

笔者在算法实现的开始阶段没有从头推导公式，而是直接使用论文中的结论公式，导致自标定出的外参矩阵与车间标定的结果（车间标定结果当作真值）总是存在一个正负号的差别。最终的拼接结果表现为图像与预想的结果存在反转、旋转等问题。笔者从头梳理了一下左右手坐标系、坐标系表征、坐标转换、欧拉角等基本原理，发现网上很多作者对这部内容都是抄来抄去文档很混乱。在实现自标定算法前，需要把这些都搞清楚。

在本帖中，笔者1.2.3节从基础原理讲起，最后第4节讲这些原理在自标定中的应用。

自标定相机外参生成鸟瞰图

1. 左右手坐标系

1.1 左右手坐标系判定方式

右、左手坐标系

右手坐标系：右手大拇指指向Z轴，其余四指握向x->y（90°而不是270°）

左手坐标系：左手大拇指指向Z轴，其余四指握向x->y（90°而不是270°）

1.2 旋转角正、负判定方式

方法一：

右手坐标系：从旋转轴上方看下去，逆时针方向为旋转正方向

左手坐标系：从旋转轴上方看下去，顺时针方向为旋转正方向

方法二：

右手坐标系：右手大拇指指向旋转轴正方向，四指握向的方向为正方向（90°而不是270°）

左手坐标系：左手大拇指指向旋转轴正方向，四指握向的方向为正方向（90°而不是270°）

方法二中四指握向的方向始终是x->y->z->x，即角度为90°，而不是270°。

本节的内容先当作一个先验知识，在后面自标定的实际应用中我们可以一起体会一下。

2. 坐标系表征与坐标转换

2.1 坐标系表征

什么是坐标系的表征

“坐标系表征”表达的物理意义用通俗的话解释就是：在A坐标系下观察B坐标系时，所看到的B坐标系的X、Y轴的方向。再通俗一点举个简单的例子：我们观察别人头部的时候，可以很容易知道这个人在看哪里，这就是因为我们获取到了对方人头坐标系的x、y、z轴在我们自己坐标系下的朝向。

 

可以理解为在XOY坐标系下观察xoy坐标系的坐标轴方向。

举个简单的视觉任务中的头姿例子：

如图所示，红色的XYZ坐标系为标准人头坐标系，即人头没有任何角度正朝前方的姿态。黄色的xyz为人头有一些旋转时当前姿态的坐标系。“头姿”指的就是在XYZ坐标系下xyz坐标系的表征，即在标准XYZ坐标系下观察当前人头的姿态：

其中 ,, 分别表示XYZ坐标系下x轴、z轴、y轴方向的单位向量。这就叫做在XYZ坐标系下xyz坐标系的表征。

2.2坐标转换

对XOY坐标系下的点（1，0）进行坐标转换也分为两种情况，对应两种不同的物理意义

使用2.1中的坐标系表征矩阵

这种情况对应的物理意义为：XOY坐标系下的点（1，0）跟随坐标轴做了  旋转后，在原XOY坐标系下的坐标：

显然XOY坐标系下的点（1，0）跟随坐标轴做了  旋转后，在原XOY坐标系下的坐标为 (cos,sin)

使用2.1中的坐标系表征矩阵的逆

这种情况对应的物理意义为：XOY坐标系下的点（1，0）在xoy坐标系下的坐标：

在我们通常说的相机坐标转换中，更多的用到的是这第二种，下式中  表示kinect相机到ir相机的坐标转换。

因此我们可以得出一个结论：我们通常说的相机坐标系之间的坐标转换矩阵 R ，实际上是相机坐标系表征矩阵的逆。即对于如图AB这两个相机，将某点 P 的坐标从A坐标系转换到B坐标系的坐标转换矩阵，实际上等于A相机坐标系下B相机坐标系的表征矩阵的逆。这个结论对于各种视觉任务的理解非常重要。后面我们再说“坐标转换”说的就是这第二种情况。

3. 欧拉角与旋转矩阵

笔者在根据github项目[1]实现基于消失点的自标定算法中计算坐标转换矩阵的时候，总是差一个正负号。当时就觉得大概率是因为欧拉角与旋转矩阵的关系没搞懂，而且笔者发现csdn、某乎甚至github上的老哥都是抄来抄去，给出的公式经常会相差一个正负号，估计大部分人都没搞懂最基础的原理是怎么回事。于是笔者从头推导了一遍，终于搞清楚这个东西是怎么用的。

下面我们一起来推导一遍。

3.1 什么是欧拉角

欧拉角是坐标系旋转的表示，针对于相机坐标系可定义为（相机坐标系如下）：

相机坐标系

绕相机x轴旋转，得到俯仰角pitch

绕相机y轴旋转，得到航偏角yaw

绕相机Z轴旋转，得到滚转角roll

注意：这样定义的pitch、yaw、roll（x轴pitch，y轴yaw，z轴roll）只能适用于这种相机坐标系，对于其他坐标系，例如飞机坐标系，那pitch、yaw和roll表示的含义就不同了，例如下图（引用自一文详解四元数、欧拉角、旋转矩阵、轴角如何相互转换 (qq.com)）：

飞机坐标系

当然，我们只讨论相机坐标系的情况，别的不考虑。相机坐标系也是一种右手系。其中z轴为相机光轴，朝向相机正前方；x轴朝向相机右侧；y轴朝向正下方。因此绕x轴旋转为俯仰角，绕y轴旋转为航偏角，绕z轴旋转为滚转角。

我们已经了解欧拉角在相机坐标系中的物理含义，知道了roll、yaw、pitch分别表示什么含义，那么如何通过欧拉角来表示两个不同的坐标系之间的关系呢？继续往下看。

3.2 欧拉角与坐标系表征

我们先思考一个问题：如果相机绕着某一个轴转动了一个角度，那么我们如何使用数学来表示这个变换呢？本节的示意图，先看xyz，XYZ两个三维的相机坐标系，然后再观察相机坐标系绕某个轴做旋转。每个三维坐标系都表示相机坐标系，也是右手系。

相机绕z轴旋转（roll）

相机绕Z轴旋转与2.1节很像，为了更好地推导数学公式，我们先从上图这个角度来看相机坐标系。图中Z轴方向为垂直于XY平面朝外，先脑补一下这个坐标系是不是和前面3.1中的坐标系是一模一样的（肯定是一样的啊，只不过从哪个角度来观察相机坐标系是不同的），XYZ相机坐标系绕找Z轴旋转了 roll 角度转换到了xyz坐标系，根据第1章的内容不难知道相机坐标系是一种右手系，且根据1.2中的结论可知上图中的旋转角 roll 为正。

进一步地，根据2.1节中坐标系表征的结论，在XYZ坐标系下xoy坐标系的表征如下，式中 r 表示roll角

相机绕y轴旋转(yaw)

上图依然是从某个角度看的相机坐标系，其中Y轴为垂直于XOZ平面朝里。根据右手坐标系定理可知图中相机坐标系XYZ绕着Y轴旋转到xyz坐标系，这个方向的旋转角 yaw 为正。进一步地，在原始XYZ相机坐标系下观察旋转后的xyz坐标系的表征如下，其中 y 表示yaw角

相机绕x轴旋转（pitch）

上图中相机坐标系XYZ绕X轴旋转pitch角，其中X轴为垂直与YOZ平面朝外，按照右手原则上图中的pitch为正。进一步地，在原始XYZ坐标系下观察xyz坐标系的表征如下，其中 p 表示pitch角。

3.3 欧拉角与坐标变换

上一节中讨论的是欧拉角与坐标系表征之间关系的问题，更多地是用在头姿任务中。在很多视觉任务中，我们需要知道的是：当坐标系做了某种旋转之后，同一个点在前后两个坐标系下的映射关系。

第2章中我们得到了结论：我们通常说的相机坐标系之间的坐标转换矩阵 R ，实际上是相机坐标系表征矩阵的逆

相机绕z轴旋转（roll）

图中点P在XYZ与xyz坐标系下的坐标之间的转换关系可以表示如下：

相机绕y轴旋转(yaw)

相机绕x轴旋转(pitch)

这回懂了吧，网上csdn、某户、github上经常看见这个矩阵里面的元素有正有负，大家写的都不一样。因为这两种矩阵根本上代表的就不是一个东西，一个是坐标系表征，一个是坐标变换。

第3章结论如下，如果不想推导可以直接记住，经过笔者验证肯定是对的：

相机坐标系问题中的“欧拉角”“坐标系表征”“坐标系转换”

4. 相机自标定

4.1 消失点原理

消失点原理

上图中 O 为相机光心，Image plane为相机成像平面。地上的两条平行线为汽车行驶在路上时的平行车道线，根据相机透视投影模型不难发现：三维相机坐标系下的平行车道线投影到相机的二维成像平面上会交于一点，这一点我们称之为消失点，它对应的是三维相机坐标系下平行车道线的无穷远点。

4.2 车道线检测

本篇不想讲关于车道线检测的内容，直接看结果吧（帖子写的太辛苦了，后面补吧。。。）：

4.3 消失点的计算

检测到车道线后，需要计算消失点。实际上我们会检测到很多的直线，但是由于噪声原因，它们不会交于一点。因此这是一个求解超定方程的问题，见笔者另一篇文章[2]中的1.3和1.4。

此时，需要我们用数学方法构造一个argmin||||  的问题。

对于图中的每一条车道线，我们都计算出了它对应的方程：x+y+=0  ，其中 (x,y)为消失点， ,,为车道线的直线方程参数，我们需要计算出满足所有直线方程的消失点 (x,y) 。因此问题转换为：

由于噪声的存在，上述理想化等式不可能存在，因此进一步地转换为：，可以通过对矩阵  A进行特征分解，最小特征值对应的特征向量就是这个问题的解，也就是消失点坐标。

消失点求解代码如下：

void RoadCalibrate::VanishPointsFitting(const std::vector& line_params,
                                cv::Point2d& pts) {
  if (int(line_params.size()) < 2) {
    LOG_ERROR("lines detected are not enouth");
    return;
  }

  int h = int(line_params.size());
  int w = 3;
  cv::Mat A = cv::ones(h, w, CV_32F);
  for (int i = 0; i < h; i++) {
    A.at(i, 0) = line_params[i].x;
    A.at(i, 1) = line_params[i].y;
    A.at(i, 2) = line_params[i].z;
  }
  cv::Mat A_T;
  cv::transpose(A, A_T);
  cv::Mat At_A = A_T * (A);

  cv::Mat eigenvalues;
  cv::Mat eigenvectors;

  cv::eigen(At_A, eigenvalues, eigenvectors);
  pts.x = eigenvectors.at(2, 0) / eigenvectors.at(2, 2);
  pts.y = eigenvectors.at(2, 1) / eigenvectors.at(2, 2);
}

opencv中调用eigen可以进行特征分解，特征值大小从大到小排列，特征向量是一行一行写入opencv的mat中的，即mat矩阵的第三行为解，拿到第三行向量做归一化即可。

至于为什么要做归一化，这个尺度是怎么来的，可以参见笔者另一篇知乎文章：单应矩阵+相机标定+ICP—计算机视觉中的数学方法 - 知乎 (zhihu.com)。截取其中一段话，简单来讲就是我们计算出来的解 (x,y,)是包含一个尺度在里面的，而在二维平面中这个  肯定是1，因此不管算出来的解是啥，最后做归一化就可以得到消失点的二维像素坐标了。

4.4 基于消失点标定外参

基于消失点标定外参的方法是通过相机模型推导的，式中 (X,Y,Z,1)为道路坐标系下某一点的齐次坐标，通过外参 (R,t)转换到相机坐标系，然后通过相机内参 K 转移到图像坐标系下。我们要做的就是计算外参 R 。

相机模型

在我们AVM自标定的应用中，要标定的相机外参主要是俯仰角pitch，一般情况下yaw和roll很小。道路坐标系可以理解为朝向正前方，而我们的相机坐标系有一个俯仰角pitch，以一定角度向下倾斜。

我们已经知道消失点对应的三维坐标点在无穷远处，即 Z 为正无穷，且根据4.2中消失点计算的方法可以求解出消失点的二维坐标。因此我们可以通过上述相机模型+消失点二维图像坐标+消失点三维坐标的Z为无穷大这些先验构建起约束关系，推导如下：

 

那么问题来了，我们得到了旋转矩阵的最后一列 r 有什么用呢？这就又要使用到第3章中的欧拉角知识。首先我们要确定，当前的问题是坐标转换（求解将某点的坐标从道路坐标系转换到相机坐标系的 R ），而不是坐标系表征，因此用到的是第三章表格中的第二行矩阵（这一点必须要明确，否则在算法实现的时候总是差一个正负号，或者你看到最终投影的结果反转之类的，这也是本贴为什么花了前三个章节分别讲了左右手坐标系、坐标系转换、坐标系表征、欧拉角这些东西）。

相机坐标系与道路坐标系的转换关系可以理解成相机坐标系先绕x轴旋转某个pitch角度，再绕y轴旋转某个yaw角度，最后绕z轴旋转某个roll角度；也可以理解成先绕x轴旋转某个pitch角，再绕z轴旋转某个roll角，最后绕y轴旋转......。一共有 =321=6 中不同的顺序组合。假设我们确定了坐标转换矩阵 R ，那么可以求解出6中不同的欧拉角，就是因为旋转轴的组合顺序不同。

实际上我们在基于消失点进行相机外参标定时用的时如下组合：

上式的物理意义是：相机先绕Z轴旋转roll翻滚角，然后绕X旋转pitch俯仰角，最后绕Y旋转航偏角，需要注意的是坐标转换矩阵的连乘顺序为左乘。即假设  将点 p 的坐标从a坐标系转换到b坐标系得到  （即 =p ），  将点  的坐标从b坐标系转换到c坐标系（即 =），那么点 p 从a坐标系转换到c坐标系可表示为：

进一步地，可以得到道路坐标系与相机坐标系之间的坐标转换关系，从下式中可以看到第三列向量非常简洁，而由于我们前面已经算出了第三列  ，因此可以通过反三角函数直接计算出pitch角和yaw角。

实际上roll、yaw、pitch的六种组合式中，每一种组合式的结果 R 必然有一列是非常简洁的，而在4.3节中我们根据消失点深度Z是无穷的+消失点图像坐标+相机内参这三个先验信息可以计算出 R 矩阵中第三列向量，因此我们在基于消失点计算外参的算法中构造这个 R 的时候，选取的是如上式的这种组合方式。

yaw pitch的计算：

void RoadCalibrate::ExtrinsicFitting(const cv::Point2d& pts, float& yaw,
                                     float& pitch) {
  // vanish
  cv::Mat vanish_point = (cv::Mat_(3, 1) << pts.x, pts.y, 1);
  cv::Mat r3 = m_intrinsic_dist_inverse * vanish_point;
  cv::Mat in = m_intrinsic_dist_inverse;
  double L2 = cv::norm(r3, cv::NORM_L2);
  r3 = r3 / L2;
  pitch = asin(r3.at(1, 0));                    
  yaw = atan(-r3.at(0, 0) / r3.at(2, 0));
}

4.5 相机姿态计算

以前置摄像头为例，小车坐标系、道路坐标系、相机坐标系之间的关系如下：

这个碗模型的中心就是汽车中心，中间的长方形区域就是汽车位置。

汽车坐标系：坐标原点在汽车中心（地面上），z轴垂直地面向上，y轴为汽车正前方，x轴为右手方向。这个是AVM 3D算法中设置的，见引用自动驾驶——自动泊车之AVM环视系统算法2 - 知乎 (zhihu.com)。

道路坐标系为：原点在汽车坐标系y轴延长线的地面上某一点，z轴为汽车行驶方向，y轴为垂直于地面朝下，x轴为右手方向。其实就是正朝向汽车行驶方向的虚拟相机坐标系。

相机坐标系：坐标系原点在汽车车头某位置，与道路坐标系相比存在绕x轴的俯仰角pitch。相比之下绕y轴的yaw角和绕z轴的roll角较小。

下图为放大后的坐标系示意图：

如图上图所示，道路坐标系与汽车标系之间的关系非常简单，它们都是有两个轴平行于地面，一个轴垂直于地面，因此很容易用第2章中的坐标系表征方法来相互表示。

我们想得到的是car坐标系->road坐标系的坐标转换关系，因此我们只需要先计算car坐标系下road坐标系的表征，然后对矩阵求逆即可。显然，在car坐标系下road坐标系的表征为：

则坐标转换矩阵为：

示意代码如下：

/*******************************************************************************************************/
void RoadCalibrate::Mat &coor_car2cam ) {
    cv::Mat car_look_road = (cv::Mat_(3, 3) << 1, 0, 0, 0, 0, 1, 0, -1, 0);
    cv::Mat coor_car2road;
    cv::invert(car_look_road, coor_car2road, CV:DECOMP_LU);
    coor_car2cam = coor_road2cam * coor_car2road;
}
/*******************************************************************************************************/

来直接看下基于消失点自标定与在标定车间通过solvePnP标定的差异：

从标定的外参结果上来看大差不差，接下来我们可视化看下生成的鸟瞰图如何。

 

代码如下：

cv::Mat RoadCalibrate::GenerateBev(float yaw, float pitch, const cv::Mat &gray) {

  //coordinate: road->camera
  float roll = 0;
  cv::Mat rotation_yaw = (cv::Mat_(3, 3) << cos(yaw), 0, -sin(yaw), 0,
                          1,
                          0, sin(yaw), 0, cos(yaw));
  cv::Mat rotation_pitch = (cv::Mat_(3, 3) << 1, 0, 0, 0, cos(pitch),
                            sin(pitch), 0, -sin(pitch), cos(pitch));
  cv::Mat rotation_roll = (cv::Mat_(3, 3) << cos(roll), sin(roll), 0,
                           -sin(roll), cos(roll), 0, 0, 0, 1);
  cv::Mat coorR_road2cam = rotation_yaw * rotation_pitch * rotation_roll;

  //coordinate: camera->road
  cv::Mat coorR_cam2road;
  cv::invert(coorR_road2cam, coorR_cam2road, cv::DECOMP_LU);

  //road->bev
  cv::Mat coorR_road2bev = (cv::Mat_(3, 3) << 1, 0, 0, 
                     0, cos(-1.57), sin(-1.57), 
                     0, -sin(-1.57), cos(-1.57));
  cv::Mat coorR_cam2bev = coorR_road2bev * coorR_cam2road;
  cv::Mat H_cam2bev =
      m_intrinsic_dist * coorR_cam2bev * m_intrinsic_dist_inverse;
  cv::Mat img_bev;
  cv::warpPerspective(gray, img_bev, H_cam2bev, gray.size(),
                      cv::INTER_LINEAR);

  imshow("bev", img_bev);

}

自标定生成鸟瞰图

总结

最近的工作中越来越意识到算法底层原理的重要性，就算是调参也要弄清楚算法的逻辑，更不用说像自标定这种没有现成的API需要自己一点一点堆砌的东西。

本文中基于消失点的自标定只是一个糙版demo，目前还只能标定出yaw和pitch默认roll是0，外参矩阵中的t也是使用标定车间的标定结果，还有很多东西需要完善。比如车道线检测中的保护逻辑，标定出前后相机外参后需要在全局做优化等等。最后推荐一篇论文：《Automatic Calibration of an Around View Monitor System Exploiting Lane Markings》有些思路可以借鉴一下。

ToDo List：

车道线检测策略

尝试用网络学习车道线

全局优化

编辑：黄飞