坐标系转换问题

前言

已经是第n次写坐标系转换问题了，但是之前都是零散的写，还是需要系统总结一遍。

转换方法

一共有四个坐标系：世界坐标系、相机坐标系、图像坐标系(忽略)、像素坐标系

世界坐标系&相机坐标系

输入： 相机外参矩阵 T_WC(4*4)及其逆矩阵T_CW
公式： 相机坐标系下的对应点(x_c,y_c,z_c)到世界坐标系下的任一点(x_w,y_w,z_w)转换方式如下
$$\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=T_{WC}\ast \left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{1,1}& r_{1,2}& r_{1,3}& t_1\\ r_{2,1}& r_{2,2}& r_{2,3}& t_2\\ r_{3,1}& r_{3,2}& r_{3,3}& t_3\\ 0& 0& 0& 1\end{array}\right]\ast \left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]$$
反过来就是乘上逆矩阵T_CW。

相机坐标系&像素坐标系

图像坐标系和像素坐标系的区别： 两者都是以图像左上角为原点，向右为 x 轴正方向，向下为 y 轴正方向，如下图所示：

区别在于，图像坐标系的单位通常是毫米或者英寸等物理单位，与实际图像的大小和分辨率有关。而像素坐标系将图像坐标系上的点转换为整数坐标的坐标系，它将图像坐标系上的每个单位划分为像素，每个像素都对应于图像上的一个点。
$$p(u,v)=p(x/dx+u_0,y/dy+v_0)$$
dx,dy分别表示X,Y方向上的一个像素在相机感光板上的物理长度（即一个像素在感光板上是多少毫米），u0,v0分别表示相机感光板中心在像素坐标系下的坐标。…这里不解释，反正不重要…

通常情况下会忽略相机坐标系到图像坐标系的变换，直接使用如下公式由相机坐标系到像素坐标系：
输入： 相机内参矩阵K(3*3)及其逆矩阵K^-1，距离z_c(注意这里的距离是相机坐标系的下的距离)
公式： 相机坐标系下的任一点(x_c,y_c,z_c)到像素坐标系下的对应点(u₀,v₀)转换方式如下:
$$\left[\begin{array}{c}{u}_0\\ v_0\\ 1\end{array}\right]=K\ast \left[\begin{array}{c}{x}_c/z_c\\ y_c/z_c\\ 1\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\ast \left[\begin{array}{c}{x}_c/z_c\\ y_c/z_c\\ 1\end{array}\right]$$
这里其实包含两步，首先需要除以z_c，然后再乘上相机内参，获得像素坐标。

Note: 学着学着就搞混了

1. 内参乘上的是相机坐标系，获得的是像素坐标系。
2. 除以的是z_c是相机坐标系下的距离，即：相机到物体的距离。
3. 注意矩阵维度

一些应用场景

RGBD转点云

使用如下公式描述将RGBD转为点云：

X_c = (u - c_x) * d / f_x
Y_c = (v - c_y) * d / f_y
Z_c = d

其实就是相机坐标系转像素坐标系的逆变换，转换获得的是相机坐标系下的点云

SLAM位姿转换

这里就只涉及外参和点在世界坐标系下的变换，相机坐标系（机器人）的坐标原点在世界坐标系下的坐标就是轨迹。轨迹可以有以下几种表示方法：

1. 外参矩阵[R|T]_4*4
2. 时间+七元数(t_x, t_y, t_z, q_x,q_y,q_z,q_w)

# 七元数转外参矩阵的代码如下

c2w = pose_matrix_from_quaternion(pose_vecs[k])

def pose_matrix_from_quaternion(pvec):
    """ convert 4x4 pose matrix to (t, q) """
    from scipy.spatial.transform import Rotation

    pose = np.eye(4)
    pose[:3, :3] = Rotation.from_quat(pvec[3:]).as_matrix()
    pose[:3, 3] = pvec[:3]
    return pose

情形1： 一号机器人在某个点看到的坐标p(x1,y1,z1)在二号机器人坐标系下的坐标
$$\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]=T_{R_2\_W}\ast T_{W\_R_1}\ast \left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]$$
情形2：（情形1的拓展） 可视化点云、mesh（或者绝大部分预处理都有这么一步），这时就要把所有像素点转换到一个坐标系下，通常以为一个轨迹下的坐标系为标准，将其他轨迹下的所有点通过上述变换，变换到第一帧的坐标系下，第一帧坐标系变为单位阵代码如下：

inv_pose = None
for ix in indicies:
    c2w = pose_matrix_from_quaternion(pose_vecs[ix])        # 将7元数组转为外参矩阵 4*4
    if inv_pose is None:
        inv_pose = np.linalg.inv(c2w)       # 求逆矩阵
        c2w = np.eye(4)
    else:
        c2w = inv_pose@c2w
    # c2w[:3, 1] *= -1
    # c2w[:3, 2] *= -1

    poses += [c2w]

3D目标检测

在此之前做过3D目标检测，当时一直不理解老师说的外参是随相机位姿改变而发生改变的，（以下是个人理解）在基于kitti的3d目标检测中，外参是不发生改变的，永远以相机为中心建立世界坐标系，寻找相机与物体间的关系。相机坐标系与世界坐标系可以重合，又或者以相机附近的点建立坐标系，该点位置与相机保持相对静止（例如：kitti中的0号相机和彩色相机的关系）。所以如何定义相机外参还是要看你所做的任务是什么吧。