认知机器人：相机校准

本文章收录在黑鲸智能系统知识库-黑鲸智能系统知识库成立于2021年，致力于建立一个完整的智能系统知识库体系。我们的工作：收集和整理世界范围内的学习资源，系统地建立一个内容全面、结构合理的知识库。

作者博客：途中的树

在机器人的感知系统中，有时我们需要摄像机来构建三维世界的投影图像，这篇文章来聊一聊

摄像机模型

描述了一个三维世界点在摄像机图像中的投影。
假设
- 有一个无限小的小孔的盒子
摄像机中心是射线的交汇点（针孔）。
后墙是图像平面image plane
摄像机中心和图像平面之间的距离是摄像机常数Camera constant

在欧几里德几何学中描述这种变换是比较困难的，不过在射影几何中就简单许多，射影几何的变换可以在齐次坐标系下表示

射影几何 Projective geometry是几何变换的一个可替代方法
齐次坐标系homogenous coordinates 在机器人学中广泛应用
优势：在射影几何中仿射变换和投影变换可以用一个矩阵的乘法来传达

相机校准

相机可以将三维世界的点投射到二维图像平面上
校准的影响因素
- 图片中心
- 焦距
- 镜头畸变参数
为什么需要校准
- 便宜的镜头，相机的制作过程中都会产生误差
- 精确的校准是非常必要的
  - 图像3D信息解释
  - 重建地图模型
  - 机器人与环境的互动（手眼协调）

Pinhole相机的三个假设

来自物体的所有射线相交于一个点
所有图像点都被投射在一个平面上
从物体点到图像的射线点是一条直线

当然这些假设往往不成立，导致图像不完美

镜头和针孔的区别

镜头只是针孔相机模型的一个近似值
物体上和图像中的相应点，以及镜头的中心通常不在一条线上
光束通过透镜中心的距离越远，误差就越大

坐标系框架

环境坐标系 World coordinate frame SoS_oSo
- 表示为 [X,Y,Z]T[X,Y,Z]^T[X,Y,Z]T
相机坐标系 Camera coordinate frame SkS_kSk
- 表示为 [kX,kY,kZ]T[^kX,^kY,^kZ]^T[kX,kY,kZ]T
图像坐标系 image coordinate frame ScS_cSc
- 表示为 [cx,cy]T[^cx,^cy]^T[cx,cy]T
传感器坐标系 Sensor coordinate SsS_sSs
- 表示为 [sx,sy]T[^sx,^sy]^T[sx,sy]T

坐标系转换 Transformation

我们想要从传感器坐标系转换到环境坐标系

其中环境坐标系SoS_oSo 到相机坐标系SkS_kSk属于外在转换
- 外在参数描述摄像机在环境中的姿态
其他的转换属于内在转换
- 内在参数描述了相机前的场景与最终图像（传感器）中的像素的映射关系

假设一个点P\mathcal{P}P

外在参数

摄像机相对于世界的姿态
可逆变换 Invertible transformation
How many parameters are needed?
- 6个：3个位置参数+3个方向参数
在环境坐标系中点P\mathcal{P}P的坐标为 XP=[XP,YP,ZP]T\mathrm{X}_P=[X_P,Y_P,Z_P]^TXP=[XP,YP,ZP]T
在相机坐标系中圆点的坐标（在环境坐标系中的坐标）为 XO=[XO,YO,ZO]T\mathrm{X}_O=[X_O,Y_O,Z_O]^TXO=[XO,YO,ZO]T
在下文中，我们将看到为什么与欧几里得坐标相比，H.C.是描述变换的更好选择

环境坐标系转换到相机坐标系 So→SkS_o \rightarrow S_kSo→Sk

直觉上讲，知道了环境坐标系中的坐标，也知道的相机坐标系的坐标，想将点坐标从环境坐标系转换到相机坐标系，可以先做一个平移和旋转 kXP=R(XP−XO)^kX_P=R(X_P-X_O)kXP=R(XP−XO),当然这是在欧式坐标系中的表示

如果放到H.C.齐次坐标系中，则为

[kXp1]=[R00T1][I3−XO0T1][XP1]=[R−RXO0T1][XP1]\begin{bmatrix} ^kX_p \\ 1 \end{bmatrix} = \begin{bmatrix} R & 0 \\ 0^\mathrm{T} & 1 \end{bmatrix}\begin{bmatrix} I_3 & -X_O \\ 0^\mathrm{T} & 1 \end{bmatrix}\begin{bmatrix} X_{P} \\ 1 \end{bmatrix}= \begin{bmatrix} R & -RX_O \\ 0^\mathrm{T} & 1 \end{bmatrix}\begin{bmatrix} X_P \\ 1 \end{bmatrix}[kXp1]=[R0T01][I30T−XO1][XP1]=[R0T−RXO1][XP1]
或者写为kXP=kHXP^k\mathrm{X}_P=^k\mathrm{HX}_PkXP=kHXP有kH=[R−RXO0T1]^k\mathrm{H}= \begin{bmatrix} R & -RX_O \\ 0^\mathrm{T} & 1 \end{bmatrix}kH=[R0T−RXO1]

内在参数

将点从相机框架投射到传感器框架的过程
可逆变换
- 图像平面到传感器框架
- 模型偏差
不能直接倒置：投影
理想状态下的投影可以分为三步
1. 将透视投影投射到图像平面上
2. 转换到传感器坐标系框架（像素）
3. 前面两个映射步骤是理想化的，需要进行校正，或者说补偿compensation

相机坐标系到图像坐标系

在图像坐标系统中：
- 我们定义一个相机常数 ccc 用来表示投影中心（小孔位置）OOO到图像平面中心H\mathcal{H}H的距离
- 该值是作为相机校准的一部分计算出来的
- c<0c<0c<0情况如下图

如果上图的透视投影是没误差的，根据截断定理几何关系我们可以得到点P\mathcal{P}P （3D）在图像平面上的投影点P‾\overline{\mathcal{P}}P（3D）的坐标[cxP‾,cyP‾][^cx_{\overline{\mathcal{P}}},^cy_{\overline{\mathcal{P}}}][cxP,cyP],

cxP‾:=kXP‾=ckXPkZP^cx_{\overline{\mathcal{P}}}:=^kX_{\overline{\mathcal{P}}}=c\frac{^kX_{\mathcal{P}}}{^kZ_{\mathcal{P}}}cxP:=kXP=ckZPkXP

cyP‾:=kYP‾=ckXPkZP^cy_{\overline{\mathcal{P}}}:=^kY_{\overline{\mathcal{P}}}=c\frac{^kX_{\mathcal{P}}}{^kZ_{\mathcal{P}}}cyP:=kYP=ckZPkXP

H.C. 齐次坐标模式

cxP‾=[cuP‾cvP‾cwP‾]=[c0000c000010][cXPcYPcZP1]^c\mathrm{x}_{\overline{\mathcal{P}}}= \begin{bmatrix} ^cu_{\overline{P}}\\^cv_{\overline{P}}\\^cw_{\overline{P}} \end{bmatrix} = \begin{bmatrix} c & 0 & 0 & 0\\0 & c & 0 & 0\\0 & 0 & 1 & 0 \end{bmatrix}\begin{bmatrix} ^cX_P\\^cY_P\\^cZ_P\\1 \end{bmatrix}cxP=⎣⎡cuPcvPcwP⎦⎤=⎣⎡c000c0001000⎦⎤⎣⎢⎢⎡cXPcYPcZP1⎦⎥⎥⎤

可以写成 cxP‾=cPkkXP^c\mathrm{x}_{\overline{\mathcal{P}}}= ^c\mathrm{P}_k \\ ^k\mathrm{X}_PcxP=cPkkXP with cPk=[c0000c000010]^c\mathrm{P}_k =\begin{bmatrix} c & 0 & 0 & 0\\0 & c & 0 & 0\\0 & 0 & 1 & 0 \end{bmatrix}cPk=⎣⎡c000c0001000⎦⎤这定义了从相机框架中的一个点到图像框架的投影

环境坐标系到图像坐标系

如此结合一下前面的两部转换,从世界框架到相机框架的转换，然后投射到图像框架中（H.C.）

cxP‾=cPX^c\mathrm{x}_{\overline{\mathcal{P}}}= ^c\mathrm{P}\mathrm{X}cxP=cPX with cP=cPkkH=[c0000c000010][R−RXO0T1]^c\mathrm{P}=^c\mathrm{P}_k {^k\mathrm{H}}=\begin{bmatrix} c & 0 & 0 & 0\\0 & c & 0 & 0\\0 & 0 & 1 & 0 \end{bmatrix}\begin{bmatrix} R & -RX_O \\ 0^\mathrm{T} & 1 \end{bmatrix}cP=cPkkH=⎣⎡c000c0001000⎦⎤[R0T−RXO1]
理想相机的校准矩阵 cK=[c000c0001]^c\mathrm{K}=\begin{bmatrix} c & 0 & 0\\0 & c & 0\\0 & 0 & 1\\\end{bmatrix}cK=⎣⎡c000c0001⎦⎤
我们可以把整个映射写成右图
各种矩阵变换，就不细说了下面写一下最后结果

校准矩阵
- 现在我们有cP=cKR[I3∣−XO]^c\mathrm{P}=^c\mathrm{K}R[I_3|-X_O]cP=cKR[I3∣−XO]
- 所以现在把一个点映射到图像层为 cx=cKR[I3∣−XO]X^c\mathrm{x}=^c\mathrm{K}R[I_3|-X_O]\mathrm{X}cx=cKR[I3∣−XO]X
- 最后得到H.C.坐标为 cx^c\mathrm{x}cx
  - [cu′cv′cw′]=[c000c0001][r11r12r13r21r22r23r31r32r33][X−XOY−YOZ−ZO]\begin{bmatrix} ^cu'\\^cv'\\^cw' \end{bmatrix} = \begin{bmatrix} c & 0 & 0 \\0 & c & 0 \\0 & 0 & 1 \end{bmatrix}\begin{bmatrix} r_11 & r_12 & r_13 \\r_21 & r_22 & r_23 \\r_31 & r_32 & r_33 \end{bmatrix}\begin{bmatrix} X-X_O\\Y-Y_O\\Z-Z_O \end{bmatrix}⎣⎡cu′cv′cw′⎦⎤=⎣⎡c000c0001⎦⎤⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤⎣⎡X−XOY−YOZ−ZO⎦⎤

环境坐标到传感器坐标

现在要把图像层映射到传感器层
假设图像层到传感器层是线性误差
主点在图像平面中的位置（偏移）
基于芯片设计的x和y的比例差异
图像层的中心点的位置
- 传感器坐标系的中心点(0,0)(0,0)(0,0)并不是图像的中心点
- 通过移位来补偿偏移
- cHc=[10xH01yH001]^c\mathrm{H}_c=\begin{bmatrix} 1 & 0 & x_{\mathrm{H}}\\0 & 1 & y_{\mathrm{H}}\\0 & 0 & 1\\\end{bmatrix}cHc=⎣⎡100010xHyH1⎦⎤

比例差别
- cHc=[10xH01+myH001]^c\mathrm{H}_c=\begin{bmatrix} 1 & 0 & x_{\mathrm{H}}\\0 & 1+m & y_{\mathrm{H}}\\0 & 0 & 1\\\end{bmatrix}cHc=⎣⎡10001+m0xHyH1⎦⎤
最后得到传感器坐标
- sx=sHccKR[I3∣−XO]X^s\mathrm{x}=^s\mathrm{H}_c\\^c\mathrm{K}R[I_3|-X_O]\mathrm{X}sx=sHccKR[I3∣−XO]X
- K=sHccK=[10xH01+myH001][c000c0001]=[10xH0c(1+m)yH001]\mathrm{K}=^s\mathrm{H}_c\\^c\mathrm{K}=\begin{bmatrix} 1 & 0 & x_{\mathrm{H}}\\0 & 1+m & y_{\mathrm{H}}\\0 & 0 & 1\\\end{bmatrix}\begin{bmatrix} c & 0 & 0 \\0 & c & 0 \\0 & 0 & 1 \end{bmatrix}=\begin{bmatrix} 1 & 0 & x_{\mathrm{H}}\\0 & c(1+m) & y_{\mathrm{H}}\\0 & 0 & 1\\\end{bmatrix}K=sHccK=⎣⎡10001+m0xHyH1⎦⎤⎣⎡c000c0001⎦⎤=⎣⎡1000c(1+m)0xHyH1⎦⎤

相机参数

这个就是我们要找的相机参数，抛开旋转不论
- 这个校准矩阵Calibration Matrix包含4个参数
  - 相机常数ccc
  - 中心点 xH,yHx_H,y_HxH,yH
  - 比例差异 mmm

非线性误差补偿

到目前为止，我们只考虑了线性参数
现实世界是非线性的
- 不完善的镜头
- 传感器的非平面性
- …

怎么解决呢？
- 再加最后一步校正这个非线性的影响
- 传感器坐标系中与位置有关的移动
- 根据与图像中心的距离，对每个像素进行单独移动

ax=sx+Δx(x,q)^ax=^sx+\Delta x(x,q)ax=sx+Δx(x,q)

ay=sy+Δy(x,q)^ay=^sy+\Delta y(x,q)ay=sy+Δy(x,q)

In the image

来个例子吧
- 畸变的近似值ax=x(1+qr2)，ay=y(1+qr2)^ax=x(1+qr^2)，^ay=y(1+qr^2)ax=x(1+qr2)，ay=y(1+qr2)
- rrr 是像素到图像中心的距离
- qqq 是一般映射的附加参数
放到H.C.
- ax=aHs(x,q)sx^a\mathrm{x}= ^a\mathrm{H}_s(x,q) \\ ^s\mathrm{x}ax=aHs(x,q)sx with aHs(x,q)=[10Δx(x,q)01Δy(x,q)001]^a\mathrm{H}_s(x,q)=\begin{bmatrix} 1 & 0 & \Delta x(x,q)\\0 & 1 & \Delta y(x,q)\\0 & 0 & 1\\\end{bmatrix}aHs(x,q)=⎣⎡100010Δx(x,q)Δy(x,q)1⎦⎤
- ax=aHc(x,q)KR[I3∣−XO]X^a\mathrm{x}=^a\mathrm{H}_c(x,q)\mathrm{K}R[I_3|-X_O]\mathrm{X}ax=aHc(x,q)KR[I3∣−XO]X

最终校正矩阵

一般的校准矩阵是通过将仿生变换的矩阵与一般的映射结合起来得到的
- aK(x,q)=aHs(x,q)K=[10xH+Δx(x,q)0c(1+m)yH+Δy(x,q)001]^a\mathrm{K}(x,q)=^a\mathrm{H}_s(x,q)\mathrm{K}=\begin{bmatrix} 1 & 0 & x_{\mathrm{H}}+\Delta x(x,q)\\0 & c(1+m) & y_{\mathrm{H}}+\Delta y(x,q)\\0 & 0 & 1\\\end{bmatrix}aK(x,q)=aHs(x,q)K=⎣⎡1000c(1+m)0xH+Δx(x,q)yH+Δy(x,q)1⎦⎤
这就求得最终的映射
- ax=aP(x,q)X^a\mathrm{x}=^a\mathrm{P}(x,q)\mathrm{X}ax=aP(x,q)X with aP(x,y)=aK(x,y)R[I3∣−XO]^a\mathrm{P}(x,y)= ^a\mathrm{K}(x,y)R[I_3|-X_O]aP(x,y)=aK(x,y)R[I3∣−XO]
如果Intrinsics是未知的，我们称之为未校准的摄像机
如果Intrinsics是已知的，我们称相机为校准的。
获得Intrinsics的过程被称为相机校准