自动驾驶视觉感知：相机标定

一、概述

相机实现如下转换：世界坐标系-> 相机坐标系->[（畸变矫正>）图像坐标系->] 像素坐标系

  坐标系定义（如上图所示）：

• 世界坐标系 可以任意定义；
• 相机坐标系 原点在光心（小孔成像的中心），z轴与相机光轴平行；
• 图像坐标系 为2维平面，平面在相机焦平面，原点在图像中心；
• 像素坐标系 与图像坐标系在同一平面，但原点在图像左上角（对应传感器阵列）。

坐标变换的基本原理和主要参数如下：

• 世界坐标系->相机坐标系：通过刚体的旋转和平移矩阵实现。其中，旋转矩阵R可由3个控制参数推出（Rodrigues旋转公式）、平移矩阵T有3个参数，组合在一起R|T形成外参矩阵（Extrinsic matrix），共6个参数。Transform:

其中，下标w表示世界坐标系，c表示相机坐标系。

• 相机坐标系->像素坐标系：通过内参控制,

.

其中，内参矩阵K（Intrinsic matrix)，包括5个控制参数（很多时候简化为4个）。该过程实际上是由多步合并而来，即相机坐标系->图像坐标系、图像坐标系->像素坐标系。另外，对于鱼眼相机和工业相机等畸变较大的相机，还需要在中间穿插畸变矫正的操作。由于相机建模是为了获得从世界坐标系到真实像素坐标系之间的转换，畸变矫正是指在理想图像上添加畸变（注意：不是去畸变，去畸变是指为畸变的图像去除畸变，获得无畸变的图像），畸变分为径向畸变和横向畸变。其中，径向去畸变依据的原理是泰勒级数展开，可以根据精度需要选择2个或3个参数；切向畸变一般取2个参数。畸变系数D （Distortion coefficient）。

二、坐标变换

2.1 世界坐标系 <=> 相机坐标系

从世界坐标到相机坐标系的变换关系：

其中，(Xw,Yw,Zw)为世界坐标，（xc,yc,zc）为相机坐标，给出的即是外参矩阵。其中，使用符号表示world to camera的变换。

变换分为主动变换(alibi or active transformation) V.S. 被动变换(alias or passive transformation)：一个vector坐标的变换可以是由于主动变换导致的，也有可能是由于被动变换导致的。

• 主动变换：vector自己发生旋转和平移。
• 被动变换：坐标系发生旋转和平移，导致vector的坐标在旋转后的坐标系下发生变换。

参考：主动变换和被动变换_FSKEps的博客-CSDN博客_主动变换

Rotation Matrix -- from Wolfram MathWorld

左乘（pre-multiplication）V.S. 右乘(post-multiplication)

• 左乘: Rv(v是column vector)，坐标系不动，点动，则左乘。
• 右乘: vR(v是row vector)，点不动，坐标系动，则右乘。右乘由于坐标系动，所以每次旋转需要基于上一次旋转后后得到的坐标系的轴来看本次旋转。

参考：三维旋转矩阵 左乘和右乘分析_狼逍豪的博客-CSDN博客_旋转矩阵左乘右乘的区别

双目立体视觉：一（坐标系变换、左乘右乘、旋转矩阵）_三眼二郎-CSDN博客_坐标变换

左手坐标系和右手坐标系：本文统一使用右手坐标系。

参考：Transformation Ambiguities and Conventions — pytransform3d 1.11.0 documentation

下面着重介绍旋转。旋转有多种表示方法，常用的包括旋转矩阵（9个数，但实际只有3个自由度）、四元数、欧拉角（roll-pitch-yaw）。欧拉角是最直观的，使用3个参数表示3个旋转方向的自由度：

所绕旋转轴	旋转术语	欧拉角
X	Roll
Y	Pitch
Z	Yaw

其中，方向轴的定义符合右手定则，逆时针旋转为正。

图片引自 https://math.stackexchange.com/questions/363652/understanding-rotation-matrices

主动变换中（rotation of object relative to fixed axes），把vector绕坐标轴旋转的矩阵为：

,

,

被动变换（rotation of the axes）矩阵中给的是固定的vector在旋转后的坐标系中的坐标（参考Rotation Matrix -- from Wolfram MathWorld）。

2.1.1 世界坐标系=>相机坐标系

工业上一般选择Z-Y-X的顺序，三个旋转角的名字分别称为yaw，pitch，roll。把一个vector从世界坐标系旋转到相机坐标系的旋转矩阵为：

PS: 代码

已知欧拉角，求世界坐标系到相机坐标系的旋转矩阵Euler Angles to Rotation Matrix

// Calculates rotation matrix given euler angles.
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
    // Calculate rotation about x axis
    Mat R_x = (Mat_(3,3) <<
               1,       0,              0,
               0,       cos(theta[0]),   -sin(theta[0]),
               0,       sin(theta[0]),   cos(theta[0])
               );
     
    // Calculate rotation about y axis
    Mat R_y = (Mat_(3,3) <<
               cos(theta[1]),    0,      sin(theta[1]),
               0,               1,      0,
               -sin(theta[1]),   0,      cos(theta[1])
               );
     
    // Calculate rotation about z axis
    Mat R_z = (Mat_(3,3) <<
               cos(theta[2]),    -sin(theta[2]),      0,
               sin(theta[2]),    cos(theta[2]),       0,
               0,               0,                  1);
     
     
    // Combined rotation matrix
    Mat R = R_x * R_y * R_z;
     
    return R;
 
}

已知世界坐标系到相机坐标系的旋转矩阵，根据以上关系求欧拉角： 

 Vec3f rotationMatrixToEulerAngles(Mat &R)
    {
     
        assert(isRotationMatrix(R));
         
        float sy = sqrt(R.at(0,0) * R.at(0,0) +  R.at(0,1) * R.at(0,1) );
     
        bool singular = sy < 1e-6; // If cos\beta=0, \beta=90度
     
        float x, y, z;
        if (!singular)
        {
            x = atan2(R.at(1,2) , R.at(2,2));
            y = atan2(-R.at(0,2), sy);
            z = atan2(R.at(0,1), R.at(0,0));
        }
        else
        {
            x = atan2(-R.at(2,1), R.at(1,1));
            y = atan2(-R.at(0,2), sy);
            z = 0;
        }
        return Vec3f(x, y, z);
    }

2.1.2 世界坐标系<=相机坐标系

从世界坐标系经过Z，Y，X的旋转以及平移T后得到相机坐标系，那么从相机坐标系变换到到世界坐标系是以上过程的反变换过程，即依次经过X, Y, Z反旋，将相机坐标反旋转过来。即：

X-Y-Z旋转的旋转矩阵为：

可参考：旋转矩阵(Rotate Matrix)的性质分析_jhope的专栏-CSDN博客_旋转矩阵

http://eecs.qmul.ac.uk/~gslabaugh/publications/euler.pdf

PS: 代码

已知欧拉角，求相机坐标系到世界坐标系的旋转矩阵Euler Angles to Rotation Matrix

// Calculates rotation matrix given euler angles.
Mat eulerAnglesToRotationMatrix(Vec3f &theta)
{
    // Calculate rotation about x axis
    Mat R_x = (Mat_(3,3) <<
               1,       0,              0,
               0,       cos(theta[0]),   -sin(theta[0]),
               0,       sin(theta[0]),   cos(theta[0])
               );
     
    // Calculate rotation about y axis
    Mat R_y = (Mat_(3,3) <<
               cos(theta[1]),    0,      sin(theta[1]),
               0,               1,      0,
               -sin(theta[1]),   0,      cos(theta[1])
               );
     
    // Calculate rotation about z axis
    Mat R_z = (Mat_(3,3) <<
               cos(theta[2]),    -sin(theta[2]),      0,
               sin(theta[2]),    cos(theta[2]),       0,
               0,               0,                  1);
     
     
    // Combined rotation matrix
    Mat R = R_z * R_y * R_x;
     
    return R;
 
}

已知相机坐标系到世界坐标系的旋转矩阵，求欧拉角 Rotation Matrix to Euler AnglesRotation Matrix to Euler Angles

// Checks if a matrix is a valid rotation matrix.
bool isRotationMatrix(Mat &R)
{
    Mat Rt;
    transpose(R, Rt);
    Mat shouldBeIdentity = Rt * R;
    Mat I = Mat::eye(3,3, shouldBeIdentity.type());
     
    return  norm(I, shouldBeIdentity) < 1e-6;
     
}
 
// Calculates rotation matrix to euler angles
// The result is the same as MATLAB except the order
// of the euler angles ( x and z are swapped ).
Vec3f rotationMatrixToEulerAngles(Mat &R)
{
 
    assert(isRotationMatrix(R));
     
    float sy = sqrt(R.at(0,0) * R.at(0,0) +  R.at(1,0) * R.at(1,0) );
 
    bool singular = sy < 1e-6; // If
 
    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at(2,1) , R.at(2,2));
        y = atan2(-R.at(2,0), sy);
        z = atan2(R.at(1,0), R.at(0,0));
    }
    else
    {
        x = atan2(-R.at(1,2), R.at(1,1));
        y = atan2(-R.at(2,0), sy);
        z = 0;
    }
    return Vec3f(x, y, z);
}

参考：Rotation Matrix To Euler Angles | LearnOpenCV #

三维空间坐标系变换-旋转矩阵_fireflychh的博客-CSDN博客_坐标变换矩阵

世界坐标系 <=> 相机坐标系  外参矩阵的转换关系：

world2camera和camera2world两个旋转矩阵和满足：

即世界坐标系到相机坐标系的旋转矩阵等与相机坐标系到世界坐标系的旋转矩阵的转置；并且，旋转矩阵是正交阵，即。

世界坐标系 <=> 相机坐标系 平移矩阵的转换关系：

world2camera和camera2world两个平移矩阵满足： 

2.2 相机坐标系 <=> 像素坐标系

相机坐标系与像素坐标系之间的转换关系依赖于相机模型，常见的相机模型包括针孔成像模型pinhole camera model（即透视模型perspective camera model)、鱼眼相机模型（fisheye camera model）。

2.2.1 相机坐标系 => 像素坐标系

物理意义上是按照3个步骤完成相机坐标系到图像坐标系的变换：

a.相机坐标系->图像坐标系：利用小孔成像原理。主要控制参数为焦距f。

b. 畸变矫正：畸变矫正是在图像坐标系上完成，分为径向畸变和横向畸变。其中，径向去畸变依据的原理是泰勒级数展开，可以根据精度需要选择2个或3个参数；切向畸变一般取2个参数。

c. 图像坐标系->像素坐标系：是平面内坐标平移和角度变换。

    但是，实际操作中，内参矩阵是由上面的步骤a和步骤c合并而来，为了方便使用内参矩阵，畸变矫正在归一化的相机坐标系上完成，随后，利用内参直接从归一化的相机坐标变换到像素坐标系。颠倒步骤a和步骤b就是先乘焦距f再畸变校正还是先畸变校正后乘f的区别，不影响最终结果。下面，先在无畸变矫正的条件下推导内参矩阵，在下一节在添加畸变矫正的操作。

下面以针孔相机模型为例进行推导，针孔相机模型为：

其中，为主轴与射入光线之间的夹角，r是图像上点与主点之间的距离，f是焦距。

参见：Pinhole camera model相机模型_1lang-CSDN博客

从相机坐标系到图像坐标系，利用小孔成像原理和三角形相似变换：

,

其中，（xi,yi）为图像坐标，为相机焦距。

从图像坐标系到像素坐标系，若像素坐标系中两轴不垂直，即其夹角，则：

,

其中，（u,v）为像素坐标，(cx, cy)表示相机主点(principal point)，即相机畸变中心，一般在图像中心点附近；dx和dy是像元尺寸。例如，1280*960的分辨率的图像，（cx,cy）一般在（640,480）附近，若CMOS senosr的尺寸为64.0mm ×48.0 mm，则dx= 64.0mm/1280px, dy=48.0mm/960px。

参考：最详细、最完整的相机标定讲解_a083614的专栏-CSDN博客_相机标定方法

结合上述两个过程，即可获得相机坐标系到像素坐标系的完整计算公式：

即内参矩阵为：

内参共5个变量。

一般情况下，，上式简化为：

其中，f/dx和f/dy是以像素为单位表示的焦距，可直接简化表示为fx,fy。若假设相机焦距为50mm，上面dx=64.0mm/1280px，则fx=f/dx=50mm*1280px/64.0mm=1000 （px）。

即简化的4个变量的内参矩阵为：

即：

内参投影即是相机模型

由上面投影方程可得：

,

,

对以上两公式分别进行平方后相减得：

取开方即得：

可见从相机坐标系到像素坐标系的变换实质就是针孔相机模型。

下面简要扩展推导下鱼眼相机模型：

鱼眼相机模型，根据等距投影：

，

其中，为主轴与射入光线之间的夹角。

即：

,

（展开上式后，第一行和第二行平方加和后开放，可以发现等效于等距投影）

参考：

相机模型-鱼眼模型（fisheye camera model）_幻想成为大牛的小不牛的博客-CSDN博客

2.2.1 相机坐标系 <= 像素坐标系

通过反投影，可以求解像素坐标对应的相机坐标。

由以上公式：

推得：

注意：反投影由于尺度因子Zc的不确定性，在世界坐标系中实际上对应的是一条线，需要补充方程才能确定唯一解。例如，假设像素坐标所在的一个平面，通过补充平面方程，获得像素坐标对应的线与该像素坐标所在平面的交点，即像素坐标对应的世界坐标。

2.3 总结：世界坐标系到像素坐标系

2.3.1 无畸变时

综合2.1, 2.2得到世界坐标系到像素坐标系的变换为：

或写为：

其中， x' 和 y'代表归一化的相机坐标，再乘以焦距f才是图像坐标（x,y）。

上面2.2.1中先从相机坐标系到图像坐标系后再归一化得到（x,y），随后再从图像坐标系变换到像素坐标。这里为了将两步合并为内参矩阵（综合考虑焦距f和cx, cy），先对相机坐标系进行了归一化（x', y'），接着用内参矩阵一步从相机坐标系变换到像素坐标系。在图像坐标系做归一化和相机坐标系做归一化都是可以的。

2.3.2 添加畸变矫正时

对于摄像头包含畸变的相机，还需要在相机坐标系转化到像素坐标系的过程中添加畸变，畸变分为径向畸变和切向畸变。

径向畸变：由于透镜自身导致的（透镜是圆形的，光在远离透镜中心位置弯曲更显著，即“鱼眼”效应），不可避免，在光学中心畸变为0，沿径向增加。可以通过泰勒级数展开校正：

其中，。取的阶数越高越精确，当然，一般好的镜头只需要取前两个高阶项k1,k2即可达到满意的去畸变效果。

切向畸变：由于透镜制造误差或安装误差导致的：

PS: 对于好的相机，切向畸变可以忽略，相当于p1=p2=0.

综合添加径向畸变和切向畸变后的结果为：

即：

参考：最详细、最完整的相机标定讲解_a083614的专栏-CSDN博客_相机标定方法

鱼眼添加畸变：

鱼眼相机成像模型_sylvester的博客-CSDN博客_鱼眼相机模型

鱼眼镜头的成像原理到畸变矫正（完整版）_Sual-CSDN博客_鱼眼相机去畸变

三、相机标定参数的应用

相机标定参数有诸多应用：

3.1 相机重投影

根据标定的内参和外参，可以把给定的世界坐标系中的点投影到图像上。世界坐标系投影到像素坐标系，实际就是上面世界坐标系-> 相机坐标系->（畸变矫正>）图像坐标系-> 像素坐标系的流程。opencv封装的API为：

cv::projectPoints(InputArrary objectPoints,InputArrary rvec,InputArrary tvec,InputArrary cameraMatrix,InputArrary distCoeffs, OutputArray imagePoints, OutputArray jacobian=noArray(), double aspectRatio=0)

3.2 图像去畸变

图像去畸变需要内参矩阵和畸变系数作为输入，步骤（K->D->K的过程）：

a) 利用内参从无畸变的像素坐标系转换为图像坐标系

b) 利用畸变系数在图像坐标系做畸变矫正（加畸变）

c) 再利用内参将畸变后的图像坐标转换回像素坐标

这样就可以确定无畸变的像素坐标对应的畸变图像上的值，把对应值赋给无畸变的像素坐标生成一幅新的图像（这个过程称为重映射remap），也就间接实现了去畸变。详细过程可参考：相机畸变校正详解_hanxiaoyong_的博客-CSDN博客_相机畸变校正 

可以直接调用Opencv的图像去畸变函数完成整个步骤：

整幅图像去畸变方法一：

void initUndistortRectifyMap(Mat cameraMatrix, Mat distCoeffs, Mat Retification, Mat newCameraMatrix, Size size, int m1type, Mat map1, Mat map2);
void remap(Mat src, Mat dst, Mat map1, Mat map2, int interpolation, int borderMode=BORDER_CONSTANT, const Scalar& borderValue=Scalar());

其中，initUndistortRectifyMap输出的map1和map2即为无畸变图像经过畸变矫正后对应的畸变后的坐标。remap函数的作用是通过查找新图像像素（去畸变的图像）在原始图像内的位置（输入的map1和map2，即畸变后的位置）来构建新图像。

注意：cv::initUndistortRectifyMap是使用针孔模型pinhole；如果是鱼眼相机，需要使用 fisheye camera model，即cv::fisheye::initUndistortRectifyMap. 

参考：opencv自带图像去畸变remap和initUndistortRectifyMap_最爱方方的博客-CSDN博客

整幅图像去畸变方法二：

void undistort(Mat src, Mat dst, Mat cameraMatrix, Mat distCoeffs, Mat newCameraMatrix=noArray())

其中，src是输入的带有畸变的图像；

dst是输出的校正的图像，与src尺寸和类型一致。注意：不能和src图像同名，这也resize中src和dst不同；

cameraMatrix即内参矩阵K；

distCoeffs即畸变系数D，，可以选取4,5或者8项。disCoeffs可以是Nx1的Mat，也可以是1xN的Mat。

newCameraMatrix,默认与内参矩阵K相同，如果需要额外的缩放或平移也可以自定义设置。

该方法undistort()是方法一中initUndistortRectifyMap()的参数Rectification使用单位矩阵和remap()的插值方法使用bilinear interpolation的组合。

参考：Geometric Image Transformations — OpenCV 2.4.13.7 documentation

稀疏点集去畸变：

void undistortPoints(InputArray src, OutputArray dst, InputArray cameraMatrix, InputArray distCoeffs, InputArray R=noArray(), InputArray P=noArray())

四、相机标定的方法

相机标定就是确定相机外参和内参的过程。先标定内参，再标定外参。

标定相机内参：棋盘格ChArUco法，每张图片可以得到8个方程，其中有6个未知数。通过移动、选择角度，形成一个三维体的效果。

标定相机外参：在相机内参已知的基础上，再采集地面棋盘格或已知坐标点，getPerspectiveTransform，获得鸟瞰图的projection信息，再warpPerspective()。

给出图像坐标及其对应的世界坐标，并输入内参矩阵和畸变系数，从而计算外参矩阵（旋转和平移）。Finds an object pose from 3D-2D point correspondences.

bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int flags=ITERATIVE )

参考：Camera Calibration and 3D Reconstruction — OpenCV 2.4.13.7 documentation