一、概述

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

坐标系定义：

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

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

世界坐标系->相机坐标系：通过刚体的旋转和平移矩阵实现。其中，旋转矩阵R可由3个控制参数推出（Rodrigues旋转公式）、平移矩阵T有3个参数，组合在一起R|T形成外参矩阵（Extrinsic matrix），共6个参数。Transform: xc=[R|t]Xw
相机坐标系->图像坐标系：利用小孔成像原理。主要控制参数为焦距f。Projection: w*x=xc。
畸变校正：对于鱼眼相机和工业相机等畸变较大的相机，需要进行畸变校正。畸变校正是在图像坐标系上完成，分为径向畸变和横向畸变。其中，径向去畸变依据的原理是泰勒级数展开，可以根据精度需要选择2个或3个参数；切向畸变一般取2个参数。参数D （Distortion coefficient）。
图像坐标系->像素坐标系：是平面内坐标平移和角度变换。控制参数为5个，即内参矩阵K（Intrinsic matrix)。u=Kx。

二、坐标变换

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

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

$\begin{bmatrix}x_{c} \\ y_{c} \\ z_{c} \end{bmatrix}=_{c}^{w}\textrm{R}\begin{bmatrix}X_{w}\\ Y_{w} \\ Z_{w}} \end{bmatrix}+_{c}^{w}\textrm{T}$

即：

$\begin{bmatrix}x_{c} \\ y_{c} \\ z_{c} \\1 \end{bmatrix}=\begin{bmatrix} _{c}^{w}\textrm{R}_{3\times3} & _{c}^{w}\textrm{T}_{3\times 1}\\ 0 & 1 \end{bmatrix}\begin{bmatrix}X_{w}\\ Y_{w} \\ Z_{w}}\\1 \end{bmatrix}=\begin{bmatrix} r_{11} & r_{12} & r_{13} &t_{x} \\ r_{21} & r_{22} & r_{23} &t_{y} \\r_{31} & r_{32} & r_{33} &t_{z}\\ 0 & 0 & 0 & 1 \end{bmatrix}\begin{bmatrix}X_{w}\\ Y_{w} \\ Z_{w}}\\1 \end{bmatrix}$

其中， $\begin{bmatrix} _{c}^{w}\textrm{R}_{3\times3} & _{c}^{w}\textrm{T}_{3\times 1}\\ 0 & 1 \end{bmatrix}$ 给出的即是外参矩阵。

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

所绕旋转轴	旋转术语	欧拉角
X	Roll	$\alpha$
Y	Pitch	$\beta$
Z	Yaw	$\theta$

单独的绕坐标轴旋转欧拉角到旋转矩阵的转化关系：

$Rx=\begin{bmatrix} 1& 0 &0 \\ 0& cos\alpha & -sin\alpha\\ 0& sin\alpha & cos\alpha \end{bmatrix}$ ,

$Ry=\begin{bmatrix} cos\beta & 0 &sin\beta \\ 0& 1 & 0\\ -sin\beta& 0 & cos\beta \end{bmatrix}$ ,

$Rz=\begin{bmatrix} cos\theta & -sin\theta &0 \\ sin\theta & cos\theta & 0\\ 0 & 0 & 1 \end{bmatrix}$

2.1.1 世界坐标系到相机坐标系

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

$_{c}^{w}\textrm{R}=Rx(\alpha)Ry(\beta)Rz(\theta)=\begin{bmatrix} 1& 0 &0 \\ 0& cos\alpha & -sin\alpha\\ 0& sin\alpha & cos\alpha \end{bmatrix}\begin{bmatrix} cos\beta & 0 &sin\beta \\ 0& 1 & 0\\ -sin\beta& 0 & cos\beta \end{bmatrix}\begin{bmatrix} cos\theta & -sin\theta &0 \\ sin\theta & cos\theta & 0\\ 0 & 0 & 1 \end{bmatrix} =\begin{bmatrix} cos\beta cos\theta & cos\beta sin\theta &-sin\beta \\ -cos\alpha sin\theta + sin\alpha sin\beta cos\theta & cos\alpha cos\theta + sin\alpha sin\beta sin\theta& sin\alpha cos\beta\\ sin\alpha sin\theta + cos\alpha sin\beta cos\theta & -sin\alpha cos\theta + cos\alpha sin\beta sin\theta & cos\alpha cos\beta \end{bmatrix}$

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_<double>(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_<double>(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_<double>(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<double>(0,0) * R.at<double>(0,0) +  R.at<double>(0,1) * R.at<double>(0,1) );
     
        bool singular = sy < 1e-6; // If cos\beta=0, \beta=90度
     
        float x, y, z;
        if (!singular)
        {
            x = atan2(R.at<double>(1,2) , R.at<double>(2,2));
            y = atan2(-R.at<double>(0,2), sy);
            z = atan2(R.at<double>(0,1), R.at<double>(0,0));
        }
        else
        {
            x = atan2(-R.at<double>(2,1), R.at<double>(1,1));
            y = atan2(-R.at<double>(0,2), sy);
            z = 0;
        }
        return Vec3f(x, y, z);
    }

2.1.2 相机坐标系到世界坐标系

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

$\begin{bmatrix}X_{w}\\ Y_{w} \\ Z_{w}} \end{bmatrix}=_{w}^{c}\textrm{R}\begin{bmatrix}x_{c} \\ y_{c} \\ z_{c} \end{bmatrix}+_{w}^{c}\textrm{T}$

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

$_{w}^{c}\textrm{R}=RzRyRx=\begin{bmatrix} cos\theta & -sin\theta &0 \\ sin\theta & cos\theta & 0\\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} cos\beta & 0 &sin\beta \\ 0& 1 & 0\\ -sin\beta& 0 & cos\beta \end{bmatrix}\begin{bmatrix} 1& 0 &0 \\ 0& cos\alpha & -sin\alpha\\ 0 & sin\alpha & cos\alpha \end{bmatrix} =\begin{bmatrix} cos\beta cos\theta & -cos\alpha sin\theta + sin\alpha sin\beta cos\theta & sin\alpha sin\theta + cos\alpha sin\beta cos\theta \\cos\beta sin\theta& cos\alpha cos\theta + sin\alpha sin\beta sin\theta&-sin\alpha cos\theta + cos\alpha sin\beta sin\theta\\ -sin\beta& sin\alpha cos\beta & cos\alpha cos\beta \end{bmatrix}$

两个旋转矩阵 $_{c}^{w}\textrm{R}$ 和 $_{w}^{c}\textrm{R}$ 满足：

$_{c}^{w}\textrm{R}=_{w}^{c}\textrm{R}^{T}$

即世界坐标系到相机坐标系的旋转矩阵等与相机坐标系到世界坐标系的旋转矩阵的转置；并且，旋转矩阵是正交阵，即 $R^{T}=R^{-1}$ 。

两个平移矩阵满足： $_{c}^{w}\textrm{T}=-_{w}^{c}\textrm{R}^{T} \cdot _{w}^{c}\textrm{T}$

可参考：https://blog.csdn.net/jhope/article/details/83067956

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_<double>(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_<double>(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_<double>(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<double>(0,0) * R.at<double>(0,0) +  R.at<double>(1,0) * R.at<double>(1,0) );
 
    bool singular = sy < 1e-6; // If
 
    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
        y = atan2(-R.at<double>(2,0), sy);
        z = atan2(R.at<double>(1,0), R.at<double>(0,0));
    }
    else
    {
        x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
        y = atan2(-R.at<double>(2,0), sy);
        z = 0;
    }
    return Vec3f(x, y, z);
}

参考：https://www.learnopencv.com/rotation-matrix-to-euler-angles/

https://blog.csdn.net/fireflychh/article/details/82352710

2.2 相机坐标系到图像坐标系

利用小孔成像原理和三角形相似变换：

$z_{c}\begin{bmatrix}x \\ y \\ 1 \end{bmatrix}=\begin{bmatrix} f & 0 & 0 & 0 \\ 0 & f & 0 & 0 \\ 0 & 0 & 1 & 0 \end{bmatrix}\begin{bmatrix}x_{c} \\ y_{c} \\ z_{c} \\1 \end{bmatrix}$

其中， $f$ 为相机焦距。

2.3 图像坐标系到像素坐标系

若像素坐标系中两轴不垂直，其夹角 $\gamma \neq 90^{o}$ ，则：

$\begin{bmatrix} u\\v \\1 \end{bmatrix}=\begin{bmatrix} \frac{1}{dx}& -\frac{cot\gamma }{dx} & u_{0} \\ 0 & \frac{1}{dy \cdot sin\gamma } & v_{0} \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix}x \\ y \\ 1 \end{bmatrix}$

参考：https://blog.csdn.net/a083614/article/details/78579163

结合2.2和2.3，得到:

$z_{c}\begin{bmatrix} u\\v \\1 \end{bmatrix}=\begin{bmatrix} \frac{f}{dx}& -\frac{f \cdot cot\gamma }{dx} & u_{0} & 0\\ 0 &\frac{f}{dy \cdot sin\gamma } & v_{0} & 0\\ 0 & 0 & 1 & 0\end{bmatrix} \begin{bmatrix}x_{c} \\ y_{c} \\ z_{c} \\1 \end{bmatrix}$

即内参矩阵为：

$K=\begin{bmatrix} \frac{f}{dx}& -\frac{f \cdot cot\gamma }{dx} & u_{0} & 0\\ 0 &\frac{f}{dy \cdot sin\gamma } & v_{0} & 0\\ 0 & 0 & 1 & 0\end{bmatrix}$

内参共5个变量。

一般情况下， $\gamma = 90^{o}$ ，上式分别简化为：

$\begin{bmatrix} u\\v \\1 \end{bmatrix}=\begin{bmatrix} \frac{1}{dx}& 0 & u_{0} \\ 0 & \frac{1}{dy} & v_{0} \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix}x \\ y \\ 1 \end{bmatrix}$

$z_{c}\begin{bmatrix} u\\v \\1 \end{bmatrix}=\begin{bmatrix} \frac{f}{dx}& 0 & u_{0} & 0\\ 0 & \frac{f}{dy} & v_{0} & 0\\ 0 & 0 & 1 & 0\end{bmatrix} \begin{bmatrix}x_{c} \\ y_{c} \\ z_{c} \\1 \end{bmatrix}$

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

$K=\begin{bmatrix} \frac{f}{dx}& 0 & u_{0} & 0\\ 0 & \frac{f}{dy} & v_{0} & 0\\ 0 & 0 & 1 & 0\end{bmatrix}$

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

$z_{c}\begin{bmatrix} u\\v \\1 \end{bmatrix}=K[_{c}^{w}\textrm{R}|_{c}^{w}\textrm{T}]\begin{bmatrix}X_{w}\\ Y_{w} \\ Z_{w}}\\1 \end{bmatrix}$

或写为：

对于畸变的相机：

三、相机标定参数的应用

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

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

根据标定的内参和外参，可以把给定的世界坐标系中的点投影到图像上；也可以通过反投影，求解像素坐标对应的世界坐标（反投影由于尺度因子Zc的不确定性，在世界坐标系中实际上对应的是一条线，需要补充方程才能确定唯一解。例如，假设像素坐标所在的一个平面，通过补充平面方程，获得像素坐标对应的线与该像素坐标所在平面的交点，即像素坐标对应的世界坐标）。
畸变校正

3.1 相机投影

世界坐标系投影到图像坐标系：

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

3.2 图像去畸变（畸变校正）

畸变分为径向畸变和切向畸变。

3.2.1 径向畸变

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

$x_{corrected}=x(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6}+k_{4}r^{8}+k_{5}r^{10}+k_{6}r^{13}+...)$

$y_{corrected}=y(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6}+k_{4}r^{8}+k_{5}r^{10}+k_{6}r^{13}+...)$

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

3.2.2 切向畸变

切向畸变由于透镜制造误差或安装误差导致的。校正公式：

$x_{corrected}=x+[2p_{1}y+p_{2}(r^{2}+2x^{2})]$

$y_{corrected}=y+[2p_{2}x+p_{1}(r^{2}+2y^{2})]$

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

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

$x_{corrected}=x(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6}+k_{4}r^{8}+k_{5}r^{10}+k_{6}r^{13}+...)+[2p_{1}y+p_{2}(r^{2}+2x^{2})]$

$y_{corrected}=y(1+k_{1}r^{2}+k_{2}r^{4}+k_{3}r^{6}+k_{4}r^{8}+k_{5}r^{10}+k_{6}r^{13}+...)+[2p_{2}x+p_{1}(r^{2}+2y^{2})]$

参考：https://blog.csdn.net/a083614/article/details/78579163

3.2.3 畸变校正步骤（K->D->K的过程）：

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

b) 利用畸变系数在图像坐标系校正畸变

c) 再利用内参从图像坐标转换回像素坐标。

即图像去畸变需要内参矩阵和畸变系数，可以直接调用Opencv的图像去畸变函数完成整个步骤：

整幅图像去畸变方法一：

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

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

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

cameraMatrix即内参矩阵K；

distCoeffs即畸变系数D， $(k_{1},k_{2},p_{1},p_{2}[,k_{3}[,k_{4},k_{5},k_{6}]])$ ，可以选取4,5或者8项。disCoeffs可以是Nx1的Mat，也可以是1xN的Mat。

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

参考：https://docs.opencv.org/2.4/modules/imgproc/doc/geometric_transformations.html

整幅图像去畸变方法二：

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());

即先计算map1和map2，随后再remap。

这种方法灵活度更高。其实方法一中的undistort()是initUndistortRectifyMap()的参数Rectification使用单位矩阵和remap()的插值方法使用bilinear interpolation的组合。

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

参考：https://blog.csdn.net/qq_25254777/article/details/79282403

稀疏点集去畸变：

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 )

参考：https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html

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