相机位姿下标定义与地图点在世界/相机坐标系之间的两种转换方式

---

前言

相机位姿的下标定义方式通常被省略了，地图点在相机坐标系与世界坐标系之间的转换可以有两种表达方式。本文主要对这两个部分进梳理清，便于理解最基础的概念。

---

一、从点的下标定义到相机位姿的下标定义

相机c的光心点在世界坐标系下的位置（不考虑旋转，并且指的是相机的光心）坐标被定义为
$$p_{wc}\in {\mathbb{R}}^3$$
它对应的是世界坐标系原点指向相机的光心的向量，并且是在世界坐标系下定义的向量。从这个定义可以看出
$$p_{wc}\ne -p_{cw}$$
这是因为一个是在世界坐标系下的定义，一个是在相机坐标系下的定义，这两个坐标系之间可能会存在一个旋转的关系。

同理，一个地图点f在相机坐标系下的坐标下标为
$$p_{wf}\in {\mathbb{R}}^3$$

很多时候为了方便，会省略点的下标定义，但我们依然需要心里有数。

相机的位姿通常指的是相机光心在世界坐标系下的位置和姿态，即
$$T={\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]}_{4\times 4}\iff T_{wc}={\left[\begin{array}{cc}{R}_{wc}& t_{wc}\\ 0& 1\end{array}\right]}_{4\times 4}$$
当然了，这里的 $R_{wc}$ 的意思是从相机坐标系转换到世界坐标系的旋转过程，并且
$$R_{wc}=(R_{cw}{)}^{-1}=(R_{cw}{)}^T$$
还有一个需要注意的一点，既然 $p_{wc}\ne -p_{cw}$不成立，那怎么去获得 $p_{cw}$ 呢？其实可以整体对转换矩阵 T 求逆再提取即可。这里求逆的时候也不要傻乎乎的直接对整个矩阵求逆，可以通过下面的公式获得
$$T^{-1}=\left(\begin{array}{cc}{R}^T& -R^T\cdot t\\ 0& 1\end{array}\right)$$
证明如下：
对于一个 4x4 的齐次变换矩阵 T 和它的逆矩阵 $T^{-1}$ 满足以下关系
$$T\cdot T^{-1}=I_{4\times 4}$$
假设 $T^{-1}$ 有一个新的形式，则我们只需求解 $R_{new}$ 和 $t_{new}$
$$T^{-1}=\left(\begin{array}{cc}{R}_{\text{new }}& t_{\text{new }}\\ 0& 1\end{array}\right)$$
下面开始丝滑推导
$$T\cdot T^{-1}=\left(\begin{array}{cc}R& t\\ 0& 1\end{array}\right)\cdot \left(\begin{array}{cc}{R}_{\text{new }}& t_{\text{new }}\\ 0& 1\end{array}\right)=\left(\begin{array}{cc}R\cdot R_{\text{new }}& R\cdot t_{\text{new }}+t\\ 0& 1\end{array}\right)$$
我们只需要求解这个方程即可
$$\begin{array}{l}R\cdot R_{\text{new }}={\mathbf{I}}_{3\times 3}\\ R\cdot t_{\text{new }}+t=0\end{array}$$
最后自然得到
$$\begin{gathered} R_{new}=R^{-1}=R^T \\ {t}_{new}=-R^{T}t \end{gathered}$$

二、地图点从世界坐标系到相机坐标系的两种转换过程

假设在某时刻，相机 c 观测到了某地图点 f ，其中相机在世界坐标系下的位置为 $p_{wc}$，姿态为 $R_{wc}$，地图点在世界坐标系下的表示为 $p_{wf}$，则地图点在相机坐标系下的表示可以有两种，分别是先平移再旋转或者先旋转再平移。

但通过下面的推导可以发现，其实是互通的，区别在于一个用了 $p_{wc}$，一个用了 $p_{cw}$。由于这个代表着相机光心与世界坐标系原点的距离，也可以写作我们熟知的 $t_{wc}$ 和 $t_{wc}$
$$p_{cf}=R_{wc}{}^T\left(p_{wf}-p_{wc}\right)=R_{cw}{p}_{wf}+\left(-R_{wc}{}^T{p}_{wc}\right)=R_{cw}{p}_{wf}+p_{cw}$$