世界坐标系，相机坐标系，像素坐标系转换 详细说明（附代码）

几个坐标系介绍，相机内外参的回顾参考此文。
本文主要说明如何在几个坐标系之间转换。

本文涉及：

1. 使用相机内参 在 像素坐标系 和 相机坐标系 之间转换。
2. 使用相机外参（位姿）在相机坐标系 和 世界坐标系 之间转换。
3. (qw,qx,qy,qz,tx,ty,tz)形式的外参如何使用。
4. 以具体情景为例，每一步详细说明，并结合代码进一步理解每个步骤。

以下面的情景为例。
假设 I1 (img1) 上有一点p1，现在要通过相机1，相机2的内外参把p1映射到 I2 (img2)上的对应点p2.
还需要知道p1的深度，假设有img1的深度图，可以读取p1处的深度。

整体思路：

p1在图片 I1 上，是像素坐标系，根据camera1的内参把它转到camera1的相机坐标系，得到(xc1, yc1, zc1),
根据camera1的外参把 (xc1, yc1, zc1) 转到 世界坐标系，得到上图中的P点坐标(xw1, yw1, zw1),
根据camera2的外参把P点 (xw1, yw1, zw1) 转到camera2的相机坐标系，得到 (xc2, yc2, zc2).
最后根据camera2的内参 把 (xc2, yc2, zc2) 转到像素坐标系，得到图像 I2 上的 p2 点坐标(x2, y2).

整个坐标系的转换关系：像素1 -> 相机1 -> 世界 -> 相机2 -> 像素2

其中，像素坐标系为2D，其他都是3D。
相机外参 也 称为 位姿 (pose).

具体步骤：

(1). p1 像素坐标 --> 相机1 坐标

这两个坐标系的关系由相机内参决定，
相机内参（fx, fy, cx, cy)
假设像素坐标为(x1, y1), 相机1坐标为(xc1, yc1, zc1), 其中zc1为 I1 的深度图 (xc1, yc1)处的值，那么

$x_1=f_x\frac{x_{c1}}{z_{c1}}+c_x$,  $y_1=f_y\frac{y_{c1}}{z_{c1}}+c_y$  (1)

现在要求 xc1 和 yc1, 由（1）得到
$x_{c1}=(x_1-c_x)\ast z_{c1}/f_x$   $y_{c1}=(y_1-c_y)\ast z_{c1}/f_y$

代码：

depth1_ori = cv2.imread("depth1.png", -1)  #uint16型
depth1 = cv2.split(depth1_ori)[0]
#p1点对应的相机坐标
zc1 = depth1[y1, x1] / 1000.0  #这里深度单位是mm
xc1 = (x1 - cx) * zc1 / fx
yc1 = (y1 - cy) * zc1 / fy

(2). p1 的相机1 坐标 --> 世界坐标

转换关系： 相机坐标 = T * 世界坐标， 世界坐标 = T^-1 * 相机坐标
其中 T 为world -> camera的转换矩阵。

如何求得转换矩阵 T ？先从概念介绍开始，

旋转矩阵R ：3 * 3矩阵
平移向量 t : 3 * 1矩阵
把R 和 t 拼成转换矩阵 T ：4 * 4矩阵，

$T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]$

顺便提一下李群李代数，T是SE(3), R是SO(3).

话题回到坐标，(xc1, yc1, zc1)为相机1坐标，(xw, yw, zw) 为世界坐标，那么世界坐标转相机坐标为：

$\left[\begin{array}{c}{x}_{c1}\\ y_{c1}\\ z_{c1}\\ 1\end{array}\right]=T\cdot \left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$

你肯定很好奇，为什么要加一维呢？
如果 $T$ 不加最后一行的$\left[\begin{array}{cc}{0}^T& 1\end{array}\right]$，坐标也不加最后一维的1，直接 $T=\left[\begin{array}{cc}R& t\end{array}\right]$ 也能计算，为什么一定要加一维？

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=T\cdot \left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]$, 这里$T=\left[\begin{array}{cc}R& t\end{array}\right]$

是这样的，现在是从 世界坐标 转 相机1坐标 ，如果要把 相机1坐标 转 世界坐标 呢？
（我们现在要做的就是把 p1的 相机1坐标 转到 世界坐标。）

那就需要这么计算了,

$\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]=T^{-1}\cdot \left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]$，这里$T=\left[\begin{array}{cc}R& t\end{array}\right]$，无法求逆矩阵

求 T 的逆矩阵，T 必须是square（行数 = 列数）的，不能是3 * 4, 必须是4 * 4的。

所以加上一行，凑成 4 * 4 矩阵

$T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]$

那么 相机坐标 --> 世界坐标 就变为：

$\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=T^{-1}\cdot \left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]$

有的程序中会使用Twc, Tcw这样的称呼，这里w指world, 是世界坐标，c指camera, 是相机坐标。
T表示转换矩阵，至于Twc 是world转camera 还是camera转world, 需要根据实际情况而定（每个开发者习惯不一样）。

实际中，到了这里估计还是不知如何计算 T，问题在哪呢？

我们拿到的 相机外参 一般会是一个四元数+平移向量的形式，其中并没有R矩阵。
相机外参：(qw, qx, qy, qz, tx, ty, tz), （这个顺序要根据实际情况而定，有的相机顺序并不是这样）。

这里用四元数 q = (qw, qx, qy, qz) 代替了R矩阵，
原因在于R是3 * 3矩阵，有9个量，而一次旋转只有3个自由度，这种表达方式是冗余的，四元数的表达更紧凑。

上面是涉及到的相关概念，现在开始计算T。

计算转换矩阵 T

现在要先把 q 转为 R，再由R, t 得到T。
q = (qw, qx, qy, qz), （一定是qw, qx, qy ,qz的顺序，不是的先调整到这个顺序）
t = (tx, ty, tz), 这里要注意t 的单位，如果是mm, 需要 / 1000.0.

如果用Eigen库，可以这么得到T，
Isometry3d是4 * 4 欧式变换矩阵，就是T的格式（参考）

Eigen::Quaterniond q(qw, qx, qy, qz);
Eigen::Isometry3d T(q);
//先设置的旋转矩阵，下面平移要在旋转前的坐标系上平移，所以是pretranslate
T.pretranslate(Eigen::Vector3d(tx, ty, tz));

如果用Sophus::SE3d

SE3d T = SE3d(Quaterniond(qw, qx, qy, qz),
                     Vector3d(tx, ty, tz))
                );

直接计算的话，由四元数 q 到旋转矩阵 R 的公式为（转此处的图）：
这里q0, q1, q2, q3分别对应 qw, qx, qy, qz.

结合 (tx, ty, tz), 下面再加一行$\left[\begin{array}{cc}{0}^T& 1\end{array}\right]$，得到T1 （由相机1的外参得到）。

T1 = np.array(
    [
        [
            1 - 2 * q2 ** 2 - 2 * q3 ** 2,
            2 * q1 * q2 - 2 * q0 * q3,
            2 * q1 * q3 + 2 * q0 * q2,
            tx,  #注意单位，如果是mm,要/1000.0
        ],
        [
            2 * q1 * q2 + 2 * q0 * q3,
            1 - 2 * q1 ** 2 - 2 * q3 ** 2,
            2 * q2 * q3 - 2 * q0 * q1,
            ty,  #注意单位，如果是mm,要/1000.0
        ],
        [
            2 * q1 * q3 - 2 * q0 * q2,
            2 * q2 * q3 + 2 * q0 * q1,
            1 - 2 * q1 ** 2 - 2 * q2 ** 2,
            tz,  #注意单位，如果是mm,要/1000.0
        ],
        [0,0,0,1],
    ])

已经得到了T1，下面可把相机坐标转为世界坐标

$\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=T_1^{-1}\cdot \left[\begin{array}{c}{x}_{c1}\\ y_{c1}\\ z_{c1}\\ 1\end{array}\right]$

代码：

p1_c = np.array([xc1, yc1, zc1, 1])
p_w = np.matmul(np.linalg.inv(T1), np.expand_dims(p1_c,1))

(3). 世界坐标 --> 相机2坐标

上面已经说明了如何由 世界坐标 转 相机坐标。
注意上面求的T1 是由相机1的外参得到，
这里要用到相机2的外参，camera2: (qw2, qx2, qy2, qz2, tx2, ty2, tz2),
求得T2 后，由下式得到 P 的相机2坐标

$\left[\begin{array}{c}{x}_{c2}\\ y_{c2}\\ z_{c2}\\ 1\end{array}\right]=T_2\cdot \left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$

p2_c = np.matmul(T2, p_w)

(4) 相机2坐标 --> 像素坐标2

相机内参（fx, fy, cx, cy)

$x_2=f_x\frac{x_{c2}}{z_{c2}}+c_x$,  $y_2=f_y\frac{y_{c2}}{z_{c2}}+c_y$ 

xc2 = p2_c[0]
yc2 = p2_c[1]
zc2 = p2_c[2]
x2 = xc2 * fx / zc2 + cx
y2 = yc2 * fy / zc2 + cy

这样就得到了图像 I2 上的映射点 p2的坐标。