相机标定 (camera calibration)

自在即为仙收录于学术

2019-08-09 约 3848 字预计阅读 8 分钟

相机标定 (camera calibration)相关理论整理。

Agreement

相关术语

1. 旋转

1.1 内旋和外旋

内旋 / intrinsic rotation

内旋是指多次旋转时绕自身坐标轴旋转。

Intrinsic rotations are elemental rotations that occur about the axes of a coordinate system XYZ attached to a moving body. —— wiki

外旋 / extrinsic rotation

外旋是指多次旋转时绕固定（初始）的坐标轴旋转。

Extrinsic rotations are elemental rotations that occur about the axes of the fixed coordinate system XYZ. —— wiki

逆序的外旋等价于正序的内旋，如 $Z-Y-X$ 内旋等价于 $X-Y-Z$ 外旋。该等效性可以通过如下简易推理证明：

共识：
内旋坐标系为O-X-Y-Z，旋转后坐标系用O’, O" , …指代
外旋坐标系为o-x-y-z
内、外旋坐标系初始位置重叠
内旋采用矩阵形式描述，即按序左乘。如 $R_{X\alpha}\cdot R_{Y\beta}\cdot R_{Z\gamma}$ 表示先绕Z轴旋转 $\gamma$，再绕Y轴旋转 $\beta$，最后绕X轴旋转 $\alpha$
外旋采用*号描述，且自左向右。如 $R_{x\alpha}*R_{y\beta}*R_{z\gamma}$ 表示先绕x轴旋转 $\alpha$，再绕y轴旋转 $\beta$，最后绕z轴旋转 $\gamma$
旋转的逆运算统一以 $^{-1}$ 表示
旋转等效（等价）的含义是原空间的任意一点旋转后到达的位置（坐标）一致
基于以上，证明可重新表述为 $R_{X\alpha}\cdot R_{Y\beta}\cdot R_{Z\gamma}=R_{x\alpha}*R_{y\beta}*R_{z\gamma}$

$\because$ 绕同一轴旋转
$\therefore$ $R_{Z\gamma}=R_{z\gamma}$
$\because$ $R_{Y\beta}$ 绕O’下的Y轴旋转，与先把O’旋回O再绕y轴旋转最后再旋回O’等效
$\therefore$ $R_{Y\beta}=R_{Z\gamma}^{-1}*R_{y\beta}*R_{Z\gamma}$
$\because$ $R_{X\alpha}$ 同理
$\therefore$ $R_{X\alpha}=(R_{Y\beta}\cdot R_{Z\gamma})^{-1}*R_{x\alpha}*(R_{Y\beta}\cdot R_{Z\gamma})$
$\because$ $R_{X\alpha}=(R_{Y\beta}\cdot R_{Z\gamma})^{-1}*R_{x\alpha}*(R_{Y\beta}\cdot R_{Z\gamma})$
$\therefore$ $R_{X\alpha}\cdot R_{Y\beta}\cdot R_{Z\gamma}=((R_{Y\beta}\cdot R_{Z\gamma})^{-1}*R_{x\alpha}*(R_{Y\beta}\cdot R_{Z\gamma}))\cdot R_{Y\beta}\cdot R_{Z\gamma}=R_{x\alpha}*(R_{Y\beta}\cdot R_{Z\gamma})$ 注意该符号描述的旋转含义，最外层旋转顺序按内旋规则，括号内旋转顺序按外旋规则
$\because$ $R_{Y\beta}=R_{Z\gamma}^{-1}*R_{y\beta}*R_{Z\gamma}$
$\therefore$ $R_{X\alpha}\cdot R_{Y\beta}\cdot R_{Z\gamma}=R_{x\alpha}*(R_{Y\beta}\cdot R_{Z\gamma})=R_{x\alpha}*((R_{Z\gamma}^{-1}*R_{y\beta}*R_{Z\gamma})\cdot R_{Z\gamma})=R_{x\alpha}*R_{y\beta}*R_{Z\gamma}$
$\because$ $R_{Z\gamma}=R_{z\gamma}$
$\therefore$ $R_{X\alpha}\cdot R_{Y\beta}\cdot R_{Z\gamma}=R_{x\alpha}*R_{y\beta}*R_{Z\gamma}=R_{x\alpha}*R_{y\beta}*R_{z\gamma}$

1.2 旋转的表示形式

旋转矩阵

以3x3矩阵表示旋转，通常在该矩阵右乘一个列向量。旋转矩阵必须是正交矩阵，且行列式为1。

四元数 / quaternions

四元数是由复数不可交换延伸出来的数学概念。

罗德里格斯旋转向量 / Rodrigues vector

（罗德里格斯）旋转向量是一个三维向量，其单位向量为旋转轴，其模为旋转角度。该向量可紧凑的表示旋转。

欧拉角 / Euler angles

欧拉角是有序的旋转角度组合，任何三维空间旋转均可通过三个绕轴旋转达到。

广义的欧拉角可分为两类：

Proper Euler angles (classic Euler angles)
Tait–Bryan angles (Cardan angles / nautical angles / heading, elevation, and bank / yaw, pitch, and roll)

对三维空间坐标系XYZ，上述两类欧拉角有如下顺序：

Proper Euler angles: $Z-X-Z, X-Y-X, Y-Z-Y, Z-Y-Z, X-Z-X, Y-X-Y$
Tait–Bryan angles: $X-Y-Z, Y-Z-X, z-X-Y, X-Z-Y, Z-Y-X, Y-X-Z$

显然，经典欧拉角仅使用两个坐标轴，而万向节角使用全部三个坐标轴。

1.3 主动旋转和被动旋转

主动旋转是对空间中的点、向量等目标进行旋转，被动旋转是对空间坐标系进行旋转。

从相对运动角度看，旋转目标和旋转坐标系是相对的，因此主动旋转某角度等效于将坐标系反向旋转同样角度。

1.4 旋转实例

右手坐标系
旋转向量遵从右手法则
以旋转矩阵形式描述
主对角线分布 $cos$, 从对角线分布 $sin$

Z-Y-X 主动内旋旋转矩阵 $$ \begin{align} R &= \left[ \begin{array}{ccc} 1 & 0 & 0\\ 0 & cos\theta_x & -sin\theta_x\\ 0 & sin\theta_x & cos\theta_x\\ \end{array} \right] \cdot \left[ \begin{array}{ccc} cos\theta_y & 0 & sin\theta_y\\ 0 & 1 & 0\\ -sin\theta_y & 0 & cos\theta_y\\ \end{array} \right] \cdot \left[ \begin{array}{ccc} cos\theta_z & -sin\theta_z & 0\\ sin\theta_z & cos\theta_z & 0\\ 0 & 0 & 1\\ \end{array} \right] \tag{1} \\ &= \left[ \begin{array}{ccc} cos\theta_ycos\theta_z & -cos\theta_ysin\theta_z & sin\theta_y\\ sin\theta_xsin\theta_ycos\theta_z+cos\theta_xsin\theta_z & -sin\theta_xsin\theta_ysin\theta_z+cos\theta_xcos\theta_z & -sin\theta_xcos\theta_y\\ -cos\theta_xsin\theta_ycos\theta_z+sin\theta_xsin\theta_z & cos\theta_xsin\theta_ysin\theta_z+sin\theta_xcos\theta_z & cos\theta_xcos\theta_y\\ \end{array} \right] \notag \end{align} $$
X-Y-Z 主动内旋旋转矩阵 $$ \begin{align} R &= \left[ \begin{array}{ccc} cos\theta_z & -sin\theta_z & 0\\ sin\theta_z & cos\theta_z & 0\\ 0 & 0 & 1\\ \end{array} \right] \cdot \left[ \begin{array}{ccc} cos\theta_y & 0 & sin\theta_y\\ 0 & 1 & 0\\ -sin\theta_y & 0 & cos\theta_y\\ \end{array} \right] \cdot \left[ \begin{array}{ccc} 1 & 0 & 0\\ 0 & cos\theta_x & -sin\theta_x\\ 0 & sin\theta_x & cos\theta_x\\ \end{array} \right] \tag{2} \\ &= \left[ \begin{array}{ccc} cos\theta_zcos\theta_y & -sin\theta_zcos\theta_x+cos\theta_zsin\theta_ysin\theta_x & sin\theta_zsin\theta_x+cos\theta_zsin\theta_ycos\theta_x\\ sin\theta_zcos\theta_y & cos\theta_zcos\theta_x+sin\theta_zsin\theta_ysin\theta_x & -cos\theta_zsin\theta_x+sin\theta_zsin\theta_ycos\theta_x\\ -sin\theta_y & cos\theta_ysin\theta_x & cos\theta_ycos\theta_x\\ \end{array} \right] \notag \end{align} $$
Z-Y-X 被动内旋旋转矩阵 $$ \begin{align} R &= \left[ \begin{array}{ccc} 1 & 0 & 0\\ 0 & cos\theta_x & sin\theta_x\\ 0 & -sin\theta_x & cos\theta_x\\ \end{array} \right] \cdot \left[ \begin{array}{ccc} cos\theta_y & 0 & -sin\theta_y\\ 0 & 1 & 0\\ sin\theta_y & 0 & cos\theta_y\\ \end{array} \right] \cdot \left[ \begin{array}{ccc} cos\theta_z & sin\theta_z & 0\\ -sin\theta_z & cos\theta_z & 0\\ 0 & 0 & 1\\ \end{array} \right] \tag{3} \\ &= \left[ \begin{array}{ccc} 1 & 0 & 0\\ 0 & cos\theta_x & sin\theta_x\\ 0 & -sin\theta_x & cos\theta_x\\ \end{array} \right] \cdot \left[ \begin{array}{ccc} cos\theta_ycos\theta_z & cos\theta_ysin\theta_z & -sin\theta_y\\ -sin\theta_z & cos\theta_z & 0\\ sin\theta_ycos\theta_z & sin\theta_ysin\theta_z & cos\theta_y\\ \end{array} \right] \notag \\ &= \left[ \begin{array}{ccc} cos\theta_ycos\theta_z & cos\theta_ysin\theta_z & -sin\theta_y\\ -cos\theta_xsin\theta_z+sin\theta_xsin\theta_ycos\theta_z & cos\theta_xcos\theta_z+sin\theta_xsin\theta_ysin\theta_z & sin\theta_xcos\theta_y\\ sin\theta_xsin\theta_z+cos\theta_xsin\theta_ycos\theta_z & -sin\theta_xcos\theta_z+cos\theta_xsin\theta_ysin\theta_z & cos\theta_xcos\theta_y\\ \end{array} \right] \notag \end{align} $$
X-Y-Z 被动内旋旋转矩阵 $$ \begin{align} R &= \left[ \begin{array}{ccc} cos\theta_z & sin\theta_z & 0\\ -sin\theta_z & cos\theta_z & 0\\ 0 & 0 & 1\\ \end{array} \right] \cdot \left[ \begin{array}{ccc} cos\theta_y & 0 & -sin\theta_y\\ 0 & 1 & 0\\ sin\theta_y & 0 & cos\theta_y\\ \end{array} \right] \cdot \left[ \begin{array}{ccc} 1 & 0 & 0\\ 0 & cos\theta_x & sin\theta_x\\ 0 & -sin\theta_x & cos\theta_x\\ \end{array} \right] \tag{4} \\ &= \left[ \begin{array}{ccc} cos\theta_z & sin\theta_z & 0\\ -sin\theta_z & cos\theta_z & 0\\ 0 & 0 & 1\\ \end{array} \right] \cdot \left[ \begin{array}{ccc} cos\theta_y & sin\theta_ysin\theta_x & -sin\theta_ycos\theta_x\\ 0 & cos\theta_x & sin\theta_x\\ sin\theta_y & -cos\theta_ysin\theta_x & cos\theta_ycos\theta_x\\ \end{array} \right] \notag \\ &= \left[ \begin{array}{ccc} cos\theta_zcos\theta_y & sin\theta_zcos\theta_x+cos\theta_zsin\theta_ysin\theta_x & sin\theta_zsin\theta_x-cos\theta_zsin\theta_ycos\theta_x\\ -sin\theta_zcos\theta_y & cos\theta_zcos\theta_x-sin\theta_zsin\theta_ysin\theta_x & cos\theta_zsin\theta_x+sin\theta_zsin\theta_ycos\theta_x\\ sin\theta_y & -cos\theta_ysin\theta_x & cos\theta_ycos\theta_x\\ \end{array} \right] \notag \end{align} $$

1.5 万向锁 / Gimbal lock

万向锁是指在（万向节）欧拉角表示法中丢失旋转自由度的现象。当第二次旋转角度为 $\frac{\pi}{2}+n\pi, n\in\mathbb{Z}$ 时，第一次旋转和第三次旋转轴相同，此时旋转自由度降低。

以Z-Y-X被动内旋旋转为例, $\theta_y=\frac{\pi}{2}$： $$ \begin{align} R &= \left[ \begin{array}{ccc} 1 & 0 & 0\\ 0 & cos\theta_x & sin\theta_x\\ 0 & -sin\theta_x & cos\theta_x\\ \end{array} \right] \cdot \left[ \begin{array}{ccc} cos\theta_y & 0 & -sin\theta_y\\ 0 & 1 & 0\\ sin\theta_y & 0 & cos\theta_y\\ \end{array} \right] \cdot \left[ \begin{array}{ccc} cos\theta_z & sin\theta_z & 0\\ -sin\theta_z & cos\theta_z & 0\\ 0 & 0 & 1\\ \end{array} \right] \tag{5} \\ &= \left[ \begin{array}{ccc} cos\theta_ycos\theta_z & cos\theta_ysin\theta_z & -sin\theta_y\\ -cos\theta_xsin\theta_z+sin\theta_xsin\theta_ycos\theta_z & cos\theta_xcos\theta_z+sin\theta_xsin\theta_ysin\theta_z & sin\theta_xcos\theta_y\\ sin\theta_xsin\theta_z+cos\theta_xsin\theta_ycos\theta_z & -sin\theta_xcos\theta_z+cos\theta_xsin\theta_ysin\theta_z & cos\theta_xcos\theta_y\\ \end{array} \right] \notag \\ &= \left[ \begin{array}{ccc} 0 & 0 & -1\\ -cos\theta_xsin\theta_z+sin\theta_xcos\theta_z & cos\theta_xcos\theta_z+sin\theta_xsin\theta_z & 0\\ sin\theta_xsin\theta_z+cos\theta_xcos\theta_z & -sin\theta_xcos\theta_z+cos\theta_xsin\theta_z & 0\\ \end{array} \right] \notag \\ &= \left[ \begin{array}{ccc} 0 & 0 & -1\\ sin(\theta_x-\theta_z) & cos(\theta_x-\theta_z) & 0\\ cos(\theta_x-\theta_z) & -sin(\theta_x-\theta_z) & 0\\ \end{array} \right] \notag \end{align} $$ 显然，改变 $\theta_x, \theta_z$ 可以达到相同的效果。

2. 形式转换

2.1 旋转矩阵与欧拉角

以 Z-Y-X 被动内旋矩阵与 Z-Y-X Euler角为例

旋转矩阵 -> 欧拉角

Python

# 似乎即使确定了旋转顺序及方式，旋转矩阵到欧拉角的转换也并不是唯一的。 atan2 函数值域内有两个解，经不完全测试所有的角度均选择另一个值时得到的旋转矩阵相同。网络上某些方法直接使用 asin(-rotm[0][2]) 求解 ry，不如使用 atan2 合理，因为 asin 或 acos 天然缺少另一边的正负信息所以总是对应两解「https://stackoverflow.com/questions/29094261/why-are-there-no-asin2-and-acos2-functions-similar-to-atan2」，而 atan2 通过合理的选择参数的正负可在数学上获得唯一解。

def rotatemat_to_euler(rotm):
    cos_ry = math.sqrt(rotm[0][0]**2+rotm[0][1]**2)
    is_singular = cos_ry < 10e-6
    if is_singular:
        rx = math.atan2(rotm[1][2], rotm[2][2])
        ry = math.atan2(-rotm[0][2], cos_ry)
        rz = 0
    else:
        rx = math.atan2(rotm[1][2], rotm[2][2])
        ry = math.atan2(-rotm[0][2], cos_ry)
        rz = math.atan2(rotm[0][1], rotm[0][0])
    return rx, ry, rz

// 似乎即使确定了旋转顺序及方式，旋转矩阵到欧拉角的转换也并不是唯一的。 atan2 函数值域内有两个解，经不完全测试所有的角度均选择另一个值时得到的旋转矩阵相同。网络上某些方法直接使用 asin(-rotm(0, 2)) 求解 euler_rad[1]，不如使用 atan2 合理，因为 asin 或 acos 天然缺少另一边的正负信息所以总是对应两解「https://stackoverflow.com/questions/29094261/why-are-there-no-asin2-and-acos2-functions-similar-to-atan2」，而 atan2 通过合理的选择参数的正负可在数学上获得唯一解。

/**
 * brief: 将旋转矩阵转换到欧拉角
 * param rotm: 按Z-Y-X轴内旋坐标系的旋转矩阵
 * return euler_rad: [rx, ry, rz]
 */
template <typename T = double>
inline std::vector<T> RotateMat2Euler(const Eigen::Matrix<T, 3, 3> &rotm) {
  std::vector<T> euler_rad(3);

  const T cos_ry = std::sqrt(std::pow(rotm(0, 0), 2)+std::pow(rotm(0, 1), 2));
  const bool is_singular = cos_ry < 10e-6;
  if (is_singular) {
    euler_rad[0] = std::atan2(rotm(1, 2), rotm(2, 2));
    euler_rad[1] = std::atan2(-rotm(0, 2), cos_ry);
    euler_rad[2] = 0;
  } else {
    euler_rad[0] = std::atan2(rotm(1, 2), rotm(2, 2));
    euler_rad[1] = std::atan2(-rotm(0, 2), cos_ry);
    euler_rad[2] = std::atan2(rotm(0, 1), rotm(0, 0));
  }
  return euler_rad;
}

欧拉角 -> 旋转矩阵

Python

def euler_to_rotatemat(euler_rad):
    rx_rad, ry_rad, rz_rad = euler_rad

    rx = np.array([[1, 0, 0], [0, math.cos(rx_rad), math.sin(rx_rad)], [0, -math.sin(rx_rad), math.cos(rx_rad)]])
    ry = np.array([[math.cos(ry_rad), 0, -math.sin(ry_rad)], [0, 1, 0], [math.sin(ry_rad), 0, math.cos(ry_rad)]])
    rz = np.array([[math.cos(rz_rad), math.sin(rz_rad), 0], [-math.sin(rz_rad), math.cos(rz_rad), 0], [0, 0, 1]])

    return rx@ry@rz

/**
 * brief: 将欧拉角转换到旋转矩阵
 * param euler_rad: [rx, ry, rz]
 * return rotm: 按Z-Y-X轴内旋坐标系的旋转矩阵
 */
template <typename T = double>
inline Eigen::Matrix<T, 3, 3> EulerRotateMat(const std::vector<T> &euler_rad) {
  assert(euler_rad.size() == 3);
  const T rx_rad = euler_rad[0];
  const T ry_rad = euler_rad[1];
  const T rz_rad = euler_rad[2];

  Eigen::AngleAxis<T> rx(-rx_rad, Eigen::Matrix<T, 3, 1>::UnitX());
  Eigen::AngleAxis<T> ry(-ry_rad, Eigen::Matrix<T, 3, 1>::UnitY());
  Eigen::AngleAxis<T> rz(-rz_rad, Eigen::Matrix<T, 3, 1>::UnitZ());
  return rx.toRotationMatrix()*ry.toRotationMatrix()*rz.toRotationMatrix();
}

3. 相机成像

相机成像原理有小孔成像和透镜成像两种。小孔成像基于光沿直线传播原理，透镜成像基于光的折射现象。

透镜相机与针孔相机的本质区别是用镜头取代了小孔¹。由于基于透镜的相机曝光时间更短，因此随着技术发展逐渐取代了针孔相机。

相机成像原理可用下图表示：

PS: 透镜成像规律表明应该在光心后焦点处倒立成像，这里为便于理解将成像平面基于光心对称挪到与物同侧。

4. 坐标系转换

相机标定的本质就是确定像素坐标 $(u,v)$ 与世界坐标 $(x_w,y_w,z_w)$ 之间的最优映射关系。

$$ \begin{align} \left[ \begin{array}{c} u \\ v \\ 1 \end{array} \right] &= \left[ \begin{array}{ccc} \frac{1}{d_x} & 0 & c_x \\ 0 & \frac{1}{d_y} & c_y \\ 0 & 0 & 1 \\ \end{array} \right] \cdot \left[ \begin{array}{c} x_i \\ y_i \\ 1 \end{array} \right] \Leftarrow \frac{1}{z_c} \left[ \begin{array}{cccc} f & 0 & 0 & 0\\ 0 & f & 0 & 0\\ 0 & 0 & 1 & 0\\ \end{array} \right] \cdot \left[ \begin{array}{c} x_c \\ y_c \\ z_c \\ 1 \end{array} \right] \Leftarrow \left[ \begin{array}{cc} R & T \\ 0 & 1 \end{array} \right] \cdot \left[ \begin{array}{c} x_w \\ y_w \\ z_w \\ 1 \end{array} \right] \notag \\ &= \frac{1}{z_c} \left[ \begin{array}{ccc} \frac{1}{d_x} & 0 & c_x \\ 0 & \frac{1}{d_y} & c_y \\ 0 & 0 & 1 \\ \end{array} \right] \cdot \left[ \begin{array}{cccc} f & 0 & 0 & 0\\ 0 & f & 0 & 0\\ 0 & 0 & 1 & 0\\ \end{array} \right] \cdot \left[ \begin{array}{cc} R & T \\ 0 & 1 \end{array} \right] \cdot \left[ \begin{array}{c} x_w \\ y_w \\ z_w \\ 1 \end{array} \right] \notag \\ &= \frac{1}{z_c} \left[ \begin{array}{cccc} f_x=\frac{f}{d_x} & 0 & c_x & 0 \\ 0 & f_y=\frac{f}{d_y} & c_y & 0\\ 0 & 0 & 1 & 0\\ \end{array} \right] \cdot \left[ \begin{array}{cc} R & T \\ 0 & 1 \end{array} \right] \cdot \left[ \begin{array}{c} x_w \\ y_w \\ z_w \\ 1 \end{array} \right] \notag \end{align} $$

5. 畸变参数

通常畸变有径向畸变、切向畸变、离心畸变、薄棱镜畸变等。
明晰：
下述畸变模型作用的坐标系并非上图中的典型的像平面坐标系，即 $(x_i, y_i)$，而是作用在类似单位化后的像平面坐标系，即 $f_x$ 一侧而非 $\frac{1}{d_x}$ 一侧，细节可参考OpenCV文档。

5.1 径向畸变

使像点产生径向位置的偏差。径向畸变又分为正向畸变和负向畸变，正向畸变称为枕形畸变，负向畸变称为桶形畸变
由镜头的形状缺陷所造成的畸变，关于相机主光轴对称

$\Delta_r=k_1r^3+k_2r^5+k_3r^7+\cdots$，其中 $r=\sqrt{u_d^2+v_d^2}$，且 $k_1,k_2,k_3,\cdots$ 为径向畸变系数

若令 $u_d=r\sin\alpha,v_d=r\cos\alpha$，则有

$\Delta_{ur}=u_d(k_1r^2+k_2r^4+k_3r^6+\cdots)$ $\Delta_{vr}=v_d(k_1r^2+k_2r^4+k_3r^6+\cdots)$

5.2 切向畸变

$\Delta_{ut}=2p_1u_dv_d+p_2(r^2+2u_d^2)$ $\Delta_{vt}=p_1(r^2+2v_d^2)+2p_2u_dv_d$

5.3 离心畸变

光学系统的光学中心和几何中心不一致（镜头各器件的光学中心）所造成的畸变
既包含径向畸变，又包含镜头主光轴不对称多造成的切向畸变

$\Delta_{ud}=2p_1u_dv_d+p_2(u_d^2+3v_d^2)+\cdots$ $\Delta_{vd}=p_1(3u_d^2+v_d^2)+2p_2u_dv_d+\cdots$

5.4 薄棱镜畸变

镜头设计缺陷与加工安装误差所造成
同时引起径向畸变和切向畸变，高价位镜头可以忽略薄棱镜畸变

$\Delta_{up}=s_1(u_d^2+v_d^2)+\cdots$，其中 $s_1$ 为薄棱镜畸变系数

$\Delta_{vp}=s_2(u_d^2+v_d^2)+\cdots$，其中 $s_2$ 为薄棱镜畸变系数

6. 畸变矫正

6.1 径向畸变

6.2 切向畸变

7. 工业相机参数

相机标定

相机标定一般包括内参标定及外参标定。常见的标定方法是张正友标定法。

目录