FAST-LIVO2（手持设备）教程3：内参标定

相机内参及其物理含义

至此，我们已经了解了相机大概是如何运作的。但为了更好的描述，我们对各个参数进行一下的定义：镜头光心到成像平面的距离称为焦距 $f$ ，分辨率定义为图片的宽和高 $(H,W)$ ，像素尺寸的大小定义为 $(\alpha_x,\alpha_y)$ 。此外，为了便于理解，我们画出了这个成像模型的俯视图（沿着y轴正方向）和侧视图（沿着x轴负方向）。

在以光心为原点的相机坐标系下，空间中的一个点 $p=(x,y,z)$ 经过光心后投影到成像平面上设为 $p'=(-x',-y',-z')$ 。此时形成的像是一个倒像，为了方便使用，我们可以对称到光心的z轴正方向上，即 $p'=(x',y',z')$ 。那么根据相似三角形，且由于 $z'=f$ ，可以有以下结论： $\frac{x'}{x} = \frac{y'}{y} = \frac{z'}{z} = \frac{f}{z}$

整理可以得到： $x'= \frac{f}{z} x$ 和 $y'= \frac{f}{z} y$ 。此时我们已经得到成像平面上的坐标和三维点之间的关系，即 $p'=(\frac{f}{z} x,\frac{f}{z} y,f)$ 。但此时所有的单位仍然是米制下的长度单位，而我们想知道的和像素坐标系下，即 $(u, v)$ 坐标系下的坐标。而我们上面已经知道了，像素的尺寸实际上由CMOS决定，每个像素对应的实际长度是固定的。因此我们直接将上述坐标除以像素长度即可： $u = \frac{x'}{\alpha_x} = \frac1z \frac{f}{\alpha_x} x$ $v = \frac{y'}{\alpha_y} = \frac1z \frac{f}{\alpha_y} y$ 我们取 $\frac{f}{\alpha_x} = f_x$ 和 $\frac{f}{\alpha_y} = f_y$ 实际上就是将焦距转换为像素单位，只是像素的尺寸通常在x轴和y轴上有区别，例如 OV2740的像素尺寸就为(1.4um / 1.4um)，因此焦距的在x轴和y轴下的，以像素为单位就也有区别。至此，我们得到了： $u = \frac1z f_x x$ $v = \frac1z f_y y$ 但是，还有一个问题：此时我们是以光心为单位，即此时正中心坐标为(0, 0)，而成像平面的左上角坐标为 $(-W/2,-H/2)$ 。为了让图像的坐标系是从左上角为原点，我们加入了一个原点的偏移：

$u = \frac1z f_x x + c_x$ $v = \frac1z f_y y + c_y$

我们可以将这个式子写成矩阵的形式：

$\begin{pmatrix} u \\ v \\ 1 \end{pmatrix}=\frac{1}{z} \begin{pmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} x \\ y \\ z \end{pmatrix}\stackrel{\text{def}}{=}\frac{1}{z}KP.$

我们将中间的矩阵定义为了内参矩阵 $K$ ，所谓内参就是这个矩阵只和相机本身的构造相关，即我们上面提到的CMOS和焦距相关。需要注意的是这里的 $(c_x,c_y)$ 并不严格等于 $(W/2.H/2)$ ，因为物理世界中，光心不可能精确的处于图像的正中心。

至此我们明确了内参矩阵的物理含义： $(f_x,f_y)$ 是相机焦距分别在x轴和y轴的像素单位下的长度， $(c_x,c_y)$ 代表了图像原点的偏移，单位也是多少个像素。

ROS驱动安装

安装ROS的相机内参标定的包

sudo apt install ros-$ROS_DISTRO-camera-calibration

修改相机的启动文件

将相机ROS驱动配置文件：src/mvs_ros_driver/config/left_camera_trigger.yaml，中的scale改为1.0。要不然图像的分辨率会缩小一半。

image_scale: 1 # 1 0.5

启动相机节点

roslaunch mvs_ros_driver mvs_camera_trigger.launch

确定标定板尺寸和角点个数

我这个标定的角点个数为 6 * 8，每个格子的长度为0.15m，我们的发布的话题为image:=/left_camera/image

启动标定节点

根据角点个数、格子边长以及相机的话题设置参数

rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.15 image:=/left_camera/image

标定

启动标定节点后，会弹出下面这个可视化界面。此时如果一切正常，那么标定板的角点上会出现一个一个特征点。可以看到右边又X、Y、size和skew四个进度条。

我们通过左右平移相机、上下平移相机、旋转相机以及前后移动相机让标定程序采样。等到进度条达标后，CALIBRATE按钮会变绿色，此时按下按钮就开始计算标定。等到计算完毕后，终端中就会出现标定的结果。

如图所示：

第一行的D对应的就是畸变系数，分别是[d_0, d_1, d_2, d_3, 0.0]。
第二行的K矩阵就是内参矩阵，分别对应[fx, 0.0, cx, 0.0, fy, cy, 0, 0, 0]

我们把结果记录到fastlivo2的配置文件中： src/FAST-LIVO2/config/camera_pinhole.yaml

cam_model: Pinhole
cam_width: 1280
cam_height: 1024
scale: 1.0
cam_fx: 1276.76059
cam_fy: 1277.02958
cam_cx: 623.98846
cam_cy: 527.10601
cam_d0: -0.06116
cam_d1: 0.092949
cam_d2: 0.001036
cam_d3: -0.00045