SLAM｜2. 差异与统一：坐标系变换与外参标定

本章主要内容
1.坐标系变换
2.相机外参标定
上一章我们了解了相机内参的概念，内参主要解决三维世界与二维图像之间的映射关系。有了内参我们可以一定程度上还原相机看到了什么（但缺乏尺度）。但相机看到的数据只是处于相机坐标系，为局部观测，我们需要将局部观测转换到全局观测上，这就涉及坐标系间转换。将传感器坐标系观测转换到载体坐标系需要通过外参。本章将介绍坐标系转换及相机外参这两部分内容。

2.1 坐标系转换

在这里插入图片描述

如图6，对同一物体，在不同坐标系下便有不同坐标。那么如果知道A与B的相对位置关系，且知道树在A坐标系下的坐标，我们如何求出树在B坐标系下的坐标呢，这就涉及到坐标系转换的内容。
首先我们有一个人为设定的世界坐标系，任何物体在这个坐标系下有其固定的坐标，这个是不变的。而这个物体被相机观测到之后，在相机坐标系下会有一个观测坐标，如图6中的A,B,C,但这个观测是相对观测，只对当前的相机坐标系有意义，对其他观测者是没有意义的，因为它无法把这个信息转换为对自己有用的信息，只有当坐标能转化到世界坐标系这个公共坐标系下，其他观测者才能利用这次观测，从而达到对环境的“有效建模”。
那么如何转换呢，需要使用转换矩阵来完成。接下来我们会从最简单的二维坐标系变换开始，一步一步推导出三维坐标系之间坐标转换的计算方式。

2.1.1 二维纯旋转变换

在这里插入图片描述

假设XOY为固定的世界坐标系，X’OY’为活动坐标系，在XOY坐标系下有一点P(x,y),假设X’OY’相对于XOY旋转了θ度，则根据三角函数，可以得到在X’OY’下点P的坐标P’为

$\mathbf{x}^{*}=\mathbf{O D}+\mathrm{DF}=\mathbf{x} \times \cos (\theta)+\mathbf{y} \times \sin (\theta) \\ \mathbf{y}^{*}=\mathbf{P C}-\mathrm{FC}=y \times \cos (\theta)-\mathbf{x} \times \sin (\theta) \\$

在这里插入图片描述

根据上述计算关系，我们也可以把P看做首先在X’OY’中测得的坐标，而XOY相对于X’OY’反向旋转了θ度（-θ），可以得到在活动坐标系下点P(x,y)，在固定世界坐标系下坐标为
$\mathbf{x}^{*}=\mathbf{O D}+\mathrm{DF}=\mathbf{x} \times \cos (-\theta)+\mathbf{y} \times \sin (-\theta)=\mathbf{x} \times \cos (\theta)-\mathbf{y} \times \sin (\theta) \\ \mathbf{y}^{*}=\mathbf{P C}-\mathbf{F C}=y \times \cos (-\theta)-\mathbf{x} \times \sin (-\theta)=y \times \cos (\theta)+\mathbf{x} \times \sin (\theta) \\$

写为矩阵形式为

$\left(\begin{array}{l} x^{*} \\ y^{*} \end{array}\right)=\left(\begin{array}{cc} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right)\left(\begin{array}{l} x \\ y \end{array}\right)=R\left(\begin{array}{l} x \\ y \end{array}\right) \\$
其中R为X’OY’在XOY坐标系下的旋转矩阵

2.1.2二维纯平移变换

两个坐标系只有平移变换，则坐标系转换很简单，如下
在这里插入图片描述

$\mathbf{x}^{*}=\mathbf{x}+t_{x} \\ \mathbf{y}^{*}=\mathbf{y}+t_{y} \\$

2.1.3二维坐标系变换

在这里插入图片描述

把上述旋转变换与平移变换结合在一起，就是二维坐标系变换
$\left(\begin{array}{l} x^{*} \\ y^{*} \end{array}\right)=\left(\begin{array}{cc} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right)\left(\begin{array}{l} x \\ y \end{array}\right)+\left(\begin{array}{l} t_{x} \\ t_{y} \end{array}\right) \\$

2.1.4 最终坐标系变换-扩展到三维

二维的旋转，可以看做三维下只绕Z轴的旋转，其中Z坐标不变，旋转矩阵扩展为
$\left(\begin{array}{l} x^{*} \\ y^{*} \\ z^{*} \end{array}\right)=\left(\begin{array}{ccc} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{l} x \\ y \\ z \end{array}\right) \\$
相应的，对于不同旋转，目前只需要记住中间有一个3X3的旋转矩阵就行（该矩阵有一个特殊性质，就是属于SO3群），而平移则是从二维变为三维就行了，最后三维变换：

$\left(\begin{array}{l} x^{*} \\ y^{*} \\ z^{*} \end{array}\right)=\left(\begin{array}{ccc} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{array}\right)\left(\begin{array}{l} x \\ y \\ z \end{array}\right)+\left(\begin{array}{l} t_{x} \\ t_{y} \\ t_{z} \end{array}\right)=R\left(\begin{array}{l} x \\ y \\ z \end{array}\right)+t \\$

但对于坐标系变换，常用齐次矩阵来表示，及把旋转变换与平移变换，放到一个矩阵里

$\left(\begin{array}{l} x^{*} \\ y^{*} \\ z^{*} \\ 1 \end{array}\right)=\left(\begin{array}{cccc} \cos \theta & -\sin \theta & 0 & t_{x} \\ \sin \theta & \cos \theta & 0 & t_{y} \\ 0 & 0 & 1 & t_{z} \\ 0 & 0 & 0 & 1 \end{array}\right)\left(\begin{array}{l} x \\ y \\ z \\ 1 \end{array}\right) \\$

在原三维向量下，添加1后的坐标，称为齐次坐标，这个是几何变换中常用的数学技巧

$\widetilde{a^{*}}=\left(\begin{array}{cc} R & t \\ 0 & 1 \end{array}\right) \widetilde{a} \\$
于是可以简化到下面这种形式：

$\widetilde{a_{1}}=T_{2}^{1} \widetilde{a_{2}} \\$
其中T12称为变换矩阵，表示将坐标系2下的坐标，转换到1所需要变换。
在这里插入图片描述

使用坐标系变换，可以把相机坐标系（cam）下的坐标，转换到世界坐标系下。反之，也可以把世界坐标系下坐标，转换到相机坐标系下。

2.2 相机外参

2.2.1 定义

对于移动载体来说，相机只是载体的一个传感器，我们更感兴趣的是，通过相机观测到的物体，相对于载体而言，其坐标为什么。如果说相机坐标系为camera_link，那么载体坐标系为base_link，这二者之间存在一个变换矩阵，通常用camera_link到base_link的欧式转换表示，而这个变换矩阵，就是常说的相机外参。使用外参矩阵，可以把相机坐标系下看到的点转换到机器人坐标下。
通俗来讲，相机外参描述了相机安装在载体的什么地方，安装角度是怎样的。
如下图，相机0与相机1均不与飞行器本体坐标系重叠，假设相机0安装在本体坐标系的t处（平移），旋转矩阵为R，则其外参为（R,t）。通过相机外参，可以把相机观测转换到飞行器本体坐标系的观测中。
在这里插入图片描述

2.2.2 相机外参的标定

如果知道相机相对于载体坐标系安装的位置与角度，是可以直接得到相机外参的，但是，由于机械安装的误差，或者运动过程的变形，这个外参是会改变的。为了获取此时的外参，常借助于一些外参标定工具，常见的外参标定工具有如下几种：
OpenCV
OpenCV除了提供相机内参标定外，也提供了相机外参标定的功能。通过拍摄已知的三维空间点的图像，并利用这些点在相机坐标系下的三维坐标，可以计算相机的位姿。
Matlab
Matlab也提供了相机外参标定的工具箱，其中包括了单目和立体相机的标定工具。通过拍摄已知的三维空间点的图像，可以计算相机的位姿。
Kalibr
Kalibr是ETH Zurich的一个开源相机标定工具，支持单目、双目和多目相机标定。Kalibr使用基于优化的方法来计算相机的位姿，同时也可以进行相机和IMU的联合标定。

2.2.3 外参标定的意义

相机外参标定确定了相机在世界坐标系中的位置和朝向，这些信息可以用于将图像中的点转换为世界坐标系中的点，或者将世界坐标系中的点转换为图像中的点。因此，外参标定对于相机视觉算法的准确性和稳定性有着重要的影响。以下是外参标定对算法的几个方面的影响：
(一) 特征匹配：在进行特征匹配时，需要将图像中的特征点匹配到世界坐标系中的点。如果外参标定不准确，将导致匹配错误，从而影响算法的准确性。
(二) 三维重建：在进行三维重建时，需要将多张图像中的点匹配到世界坐标系中的点，并进行三维重建。如果外参标定不准确，将导致重建的几何形状不准确或者出现扭曲。
(三) 目标跟踪：在进行目标跟踪时，需要将跟踪目标在多张图像中的位置匹配到世界坐标系中的点，并进行跟踪。如果外参标定不准确，将导致跟踪不准确或者跟踪失败。
(四) 相机姿态估计：在进行相机姿态估计时，需要将相机在多张图像中的位置和朝向匹配到世界坐标系中的点，并进行姿态估计。

2.2.4 相机标定类型

相机标定是指确定相机内部参数和外部参数的过程，以便在图像中恢复真实世界中的几何信息。常用的相机标定类型包括以下几种：
(一) 内部标定：用于确定相机的内部参数，包括焦距、主点位置、畸变系数等。内部标定通常使用标定板等已知几何形状的物体，并采用校正方法进行计算。
(二) 外部标定：用于确定相机在世界坐标系中的位置和方向。外部标定通常使用已知位置的标定板或者其他几何形状的物体，并采用三维重建方法进行计算。
(三) 传感器到传感器标定：用于确定多个相机之间的相对位置和方向，以便进行双目或多目视觉处理。传感器到传感器标定通常使用标定板或者球形标定物等，采用立体匹配算法进行计算。
(四) 传感器到车体标定：用于确定相机在车体坐标系中的位置和方向，以便进行车载视觉处理。传感器到车体标定通常使用车体固定的标定板或者其他几何形状的物体，采用三维重建方法进行计算。
(五) 姿态标定：用于确定相机在平面或者空间中的朝向，以便进行视觉导航或者机器人控制。姿态标定通常使用旋转平台或者陀螺仪等设备，采用角度解算方法进行计算。
(六) 相机-激光雷达标定：用于确定相机和激光雷达之间的相对位置和方向，以便进行三维点云重建或者障碍物检测。相机-激光雷达标定通常使用已知几何形状的标定板或者球形标定物，采用多视角几何约束方法进行计算。
(七) 相机-IMU标定：用于确定相机和惯性测量单元（IMU）之间的相对位置和方向，以便进行惯性辅助导航或者姿态估计。相机-IMU标定通常使用运动平台或者旋转平台等设备，采用卡尔曼滤波或者优化方法进行计算。

本章小结

本章主要介绍如何将相机坐标系下观测，转换到其他坐标系（如载体坐标系），紧接着介绍了相机外参。相机外参主要作用是将相机的局部观测转到载体坐标系下。载体在世界坐标系中不断运动，如何描述载体在世界坐标系下姿态，见下一章。

本章思考

1.相机外参的作用是什么？
2.一个载体搭载着摄像头，在平面上运动，相机外参【t(x,y,z)与R(roll,pitch,yaw)】哪些量不可以被标定出来？
附录

摄像机外参标定
求解相机的内、外参数矩阵，描述了三维世界到二维像素的映射关系

其中的 $\theta$ 为单个像素两条边的夹角，一般我们认为这个角度为90°，早期对于一些加工不好的传感器，像素可能会是平行四边形。
M矩阵有11个未知量，求解投影矩阵需要最少六对点对应，但实操时一般使用多于六对点获得更鲁棒的效果
径向畸变：图像像素点以畸变中心为中心点,沿着径向产生的位置偏差,从而导致图像中所成的像发生形变
产生原因：光线在远离透镜中心的地方比靠近中心的地方更加弯曲
图像放大率随距光轴距离的增加而减小

在这里插入图片描述