A Pose-Only Solution to Visual Reconstruction and Navigation 推导过程

数学记号定义

假设相机已标定（intrinsics已知），图像坐标已归一化。论文中涉及的符号：

• $\mathbf{X}_W = (x_W, y_W, z_W)^T$ ：世界坐标系中的3D点（上标T表示转置）。
• $\mathbf{X}^{C_i} = (x^{C_i}, y^{C_i}, z^{C_i})^T$ ：第i个相机坐标系中的3D点（上标T表示转置）。
• $\mathbf{X}_i = (x_i, y_i, 1)^T$ ：第i个视图中的归一化图像坐标。
• $\mathbf{R}_{iW}$ ：第i个相机的旋转矩阵（正交矩阵，满足 $\mathbf{R}_i^T \mathbf{R}_i = \mathbf{I}$ ，其中 $\mathbf{I}$ 是单位矩阵）。
• $\mathbf{t}_{Wi} \in \mathbb{R}^3$ ：第i个相机的平移向量。
• $z^{C_i} > 0$ ：第i个相机坐标系中的深度。

满足：

$$\mathbf{X}_i = \frac{1}{z^{C_i}} \mathbf{X}^{C_i} = \frac{1}{z^{C_i}} \mathbf{R}_{iW} (\mathbf{X}_W - \mathbf{t}_{Wi} )$$

其中，原文中的 $\mathbf{R} _ i$ $\mathbf{t} _ i$ 等于本文中的 $\mathbf{R} _ {iW}$ , $\mathbf{t} _ {Wi}$，以便更清晰的突显其将世界坐标系中的点转换到相机坐标系下的作用。这样的表达可以更好的帮助理解坐标变换的关系，即

$$\mathbf{X}^{C_i} = \mathbf{R}_{iW} (\mathbf{X}_W - \mathbf{t}_{iW} ) \\$$

$$\mathbf{R}_{Wi} \mathbf{X}^{C_i} + \mathbf{t}_{iW}= \mathbf{R}_{iW}^T \mathbf{X}^{C_i} + \mathbf{t}_{iW}= \mathbf{X}_W$$

这一表达更符合笔者习惯的机器人学中的记号，即 $X_i = R_{ij} X_j + t_{ij}, R_{ij} = R_{ik} R_{kj}$ 的形式。

数学背景知识

• 叉乘矩阵：对于向量 $\mathbf{v} = (v_x, v_y, v_z)^T$ ，其叉乘矩阵 $[\mathbf{v}^\times]$ 定义为：

$$[\mathbf{v}^\times] = \begin{pmatrix} 0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0 \end{pmatrix}$$

性质： $[\mathbf{v}^\times] \mathbf{w} = \mathbf{v} \times \mathbf{w}$ （叉乘），且 $[\mathbf{v}^\times] \mathbf{v} = \mathbf{0}$ （零向量）， $\mathbf{v} \times \mathbf{w} = -\mathbf{w} \times \mathbf{v}$。

• 坐标变换

令世界坐标系到位姿 (i) 的齐次变换为

$$T_{iW}=\begin{bmatrix}R_{iW} & t_{iW} \\ 0 & 1\end{bmatrix},$$

同理位姿 (j) 为

$$T_{jW}=\begin{bmatrix}R_{jW} & t_{jW} \\ 0 & 1\end{bmatrix},$$

其中 $`R_{iW},R_{jW} \in\mathbb{R}^{3\times3} `$ 是旋转矩阵，$`t_i,t_j\in\mathbb{R}^3`$ 是平移向量（列向量）。

逆变换为

$$(T_{jW})^{-1}=\begin{bmatrix}R_{jW}^\top & -R_{jW}^\top t_{jW} \\ 0 & 1\end{bmatrix}.$$

相对变换 $`T_{ij}`$ 的计算过程

按矩阵乘法

$$\begin{aligned} T_{ij} &= T_{iW} (T_{jW})^{-1} \\ &= \begin{bmatrix} R_{iW} & t_{iW} \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R_{jW}^\top & -R_{jW}^\top t_{jW} \\ 0 & 1 \end{bmatrix} \\ &= \begin{bmatrix} R_{iW} R_{jW}^\top & t_{iW} - R_{iW} R_{jW}^\top t_{jW} \\ 0 & 1 \end{bmatrix}. \end{aligned}$$

因此,展开的旋转和位移分量分别为

$$\boxed{R_{ij}=R_{iW} R_{jW}^\top} \qquad\text{和}\qquad \boxed{t_{ij}=t_{iW} - R_{iW} R_{jW}^\top t_{jW}}$$

一、 只有位姿的 (PPO) 约束的推导过程

对于一组图像对(i, j) 我们有如下的投影关系：

$$\mathbf{X}^{C_j} = \mathbf{R}_{ji} \mathbf{X}^{C_i} + \mathbf{t}_{ji}$$

我们将其中的深度信息更进一步的表达出来，这是为了在等式两边消去深度从而获得只与位姿相关的约束方程。

$$z^{C_j} \mathbf{X}_j = z^{C_i} \mathbf{R}_{ji} \mathbf{X}_i + \mathbf{t}_{ji}$$

这里的 $`\mathbf{R}_{ji}=\mathbf{R}_{jW} \mathbf{R}_{iW}^T `$ , 而平移的 $`\mathbf{t}_{ji}=\mathbf{R}_{jW} (t_{Wi} - t_{Wj}) `$

注意：平移的结果并不是那么直观的，可以参考第一部分的数学基础进行转换推导。

左乘反对称矩阵 $[ \mathbf{X}_j ] _\times$ 得到

$$z^{C_j} [ \mathbf{X}_j ] _\times \mathbf{X}_j = z^{C_i} [ \mathbf{X}_j ] _\times \mathbf{R}_{ji} \mathbf{X}_i + [ \mathbf{X}_j ] _\times \mathbf{t}_{ji} = 0$$

这样我们可以把第一个深度结果消去，即 $z^{C_j}$ 在这个等式中不再重要，整理上面公式得到

$$z^{C_i} [ \mathbf{X}_j ] _\times \mathbf{R}_{ji} \mathbf{X}_i = - [ \mathbf{X}_j ] _\times \mathbf{t}_{ji}$$

取其模长，我们可以得到

$$z^{C_i} = \frac{\| [ \mathbf{X}_j ] _\times \mathbf{t}_{ji} \|}{\| [ \mathbf{X}_j ] _\times \mathbf{R}_{ji} \mathbf{X}_i \|} \doteq d_i^{(i,j)}$$

这是由于 $z^{C_i}$ 是一个正数，我们才可以进行这样的操作。

同样，为了把第二个深度消掉，我们重复上面的过程左乘反对称矩阵 $[ \mathbf{R}_{ji} \mathbf{X}_i] _\times$ 得到

$$z^{C_j} [ \mathbf{R}_{ji} \mathbf{X}_i] _\times \mathbf{X}_j = [ \mathbf{R}_{ji} \mathbf{X}_i] _\times \mathbf{t}_{ji}$$

再次取模长得到

$$z^{C_j} = \frac{\| [ \mathbf{R}_{ji} \mathbf{X}_i] _\times \mathbf{t}_{ji} \|}{\| [ \mathbf{R}_{ji} \mathbf{X}_i] _\times \mathbf{X}_j \|} \doteq d_j^{(i,j)}$$

注意根据叉乘的形式，这里有$`[ \mathbf{R}_{ji} \mathbf{X}_i] _\times \mathbf{X}_j = -[\mathbf{X}_j ] _\times \mathbf{R}_{ji} \mathbf{X}_i`$，由于我们取了模长它们就相等了，则可以定义一个变量 $`\theta_{i,j}=\|[ \mathbf{R}_{ji} \mathbf{X}_i] _\times \mathbf{X}_j\|`$ 。

将上面计算得到的 $z^{C_i}$ 和 $z^{C_j}$ 表达式替换原本约束，即可得到PPO约束，在这个约束中只包含位姿，场景的深度信息被隐含了。

$$d^{(i,j)}_j \mathbf{X}_j = d^{(i,j)}_i \mathbf{R}_{ji} \mathbf{X}_i + \mathbf{t}_{ji}$$

二、多视图下的DPO约束

考虑在(i,j)之外的第l帧图像，它和第i帧同样满足PPO约束如下

$$d^{(i,l)}_l \mathbf{X}_l = d^{(i,l)}_i \mathbf{R}_{li} \mathbf{X}_i + \mathbf{t}_{li}$$

并且有如下关系, 即无论在(i, l)或(i, j) 图像对中，在图像i中的深度信息都是一致的。

$$d^{(i,j)}_i = d^{(i,l)}_i = z_{C_i}$$

我们将上面两个式子联立可以得到三个视图下位姿的关系约束

$$d^{(i,l)}_l \mathbf{X}_l = d^{(i,j)}_i \mathbf{R}_{li} \mathbf{X}_i + \mathbf{t}_{li}$$

对于任意多视图下的约束，我们需要对此进行扩展，首先考虑任意的第i帧，得到关于 $(\zeta,\eta)$ 两帧之间的约束集合

$$\mathcal{D}(\zeta, \eta)=\left\{ d_{i}^{(\zeta, i)}\boldsymbol{X}_{i}=d_{\zeta}^{(\zeta, \eta)} R_{i, \zeta}\boldsymbol{X}_{\zeta}+\boldsymbol{t}_{i, \zeta} \mid 1 \leq i \leq n, i \neq \zeta \right\}$$

因此，多视图几何中任意两帧的约束需要通过其他帧来进行传播，在这个传播过程中，笔者认为是将两视图之间的尺度信息传递到第三帧的过程，也就是原本地图点的作用。在这里是以一种隐式的方法进行的。在这里我们使用了中间帧深度相等的信息，因此被称为深度-仅位姿约束，也就是DPO约束。

三、线性全局平移（Linear Global Translation, LiGT）约束

目前存在许多成熟稳定的算法能够准确的估计相对旋转，因此论文中认为全局最优的平移估计是更具有挑战性的工作。上面所推导的只有位姿的约束在旋转矩阵已知时能够很好的转换为线性全局平移（Linear Global Translation, LiGT）约束，从而实现全局平移的高效求解。

因此，本小节将假设旋转矩阵 $`\{\mathbf{R}_i\}_{i=1}^n`$ 已知，从DPO约束中推导LiGT约束

首先，我们将原始的DPO约束写在下面

$$d_{i}^{(\zeta, i)}\boldsymbol{X}_{i}=d_{\zeta}^{(\zeta, \eta)} R_{i, \zeta}\boldsymbol{X}_{\zeta}+\boldsymbol{t}_{i, \zeta}$$

在等式的两端乘上 $`[\mathbf{X}_i]_\times`$ 得到

$$0 = [\mathbf{X}_i]_\times \left( d_{\zeta}^{(\zeta, \eta)} R_{i, \zeta}\boldsymbol{X}_{\zeta}+\boldsymbol{t}_{i, \zeta} \right)$$

观察上面的式子发现，相对旋转矩阵和像素坐标都是确定的常数，而 $`d_{\zeta}^{(\zeta, \eta)}`$ 需要被进一步的化简以直观表达其和相对平移之间的关联。

我们回顾 $`d_{\zeta}^{(\zeta, \eta)}`$ 的推导过程，原本的定义为

$$d_{\zeta}^{(\zeta, \eta)} = \frac{\|[\mathbf{R}_{\eta, \zeta} \mathbf{X}_{\zeta}]_\times \mathbf{t}_{\eta, \zeta}\|}{\theta_{\zeta, \eta}}$$

其中，$`\theta_{\zeta, \eta} = \| [ \mathbf{X}_\eta]_\times \mathbf{R}_{\eta, \zeta} \mathbf{X}_\zeta \|`$

满足如下的PPO约束

$$d^{(\zeta,\eta)}_\eta \mathbf{X}_\eta = d^{(\zeta, \eta)}_\zeta \mathbf{R}_{\eta, \zeta} \mathbf{X}_\zeta + \mathbf{t}_{\eta, \zeta}$$

由于原本的定义包含绝对值等操作，不便于计算，因此我们从PPO约束出发，推导 $`d_{\zeta}^{(\zeta, \eta)}`$ 的另一种表达形式，即首先通过左乘反对称矩阵 $`[ \mathbf{X}_\eta]_\times`$ 消去 $`d_{\eta}^{(\zeta, \eta)}`$ 得到

$$0 = d^{(\zeta, \eta)}_\zeta [ \mathbf{X}_\eta]_\times \mathbf{R}_{\eta, \zeta} \mathbf{X}_\zeta + [ \mathbf{X}_\eta]_\times \mathbf{t}_{\eta, \zeta}$$

上式可以整理得到

$$d^{(\zeta, \eta)}_\zeta [ \mathbf{X}_\eta]_\times \mathbf{R}_{\eta, \zeta} \mathbf{X}_\zeta = - [ \mathbf{X}_\eta]_\times \mathbf{t}_{\eta, \zeta}$$

这个式子表明， $`[ \mathbf{X}_\eta]_\times \mathbf{R}_{\eta, \zeta} \mathbf{X}_\zeta`$ 和 $`[ \mathbf{X}_\eta]_\times \mathbf{t}_{\eta, \zeta}`$ 是方向相反的两个共线向量。对原本的定义做一个数学上的变换上下乘$`\theta_{\zeta, \eta}`$ 得到

$$d_{\zeta}^{(\zeta, \eta)} = \frac{\|[\mathbf{R}_{\eta, \zeta} \mathbf{X}_{\zeta}]_\times \mathbf{t}_{\eta, \zeta}\| \| [ \mathbf{X}_\eta]_\times \mathbf{R}_{\eta, \zeta} \mathbf{X}_\zeta \| }{\theta_{\zeta, \eta}^2}$$

由于上面的反向共线关系，我们可以将这个取模长的非线性步骤拿掉，化简为

$$d_{\zeta}^{(\zeta, \eta)} = \frac{- \left( [ \mathbf{X}_\eta]_\times \mathbf{R}_{\eta, \zeta} \mathbf{X}_\zeta \right)^\top [\mathbf{R}_{\eta, \zeta} \mathbf{X}_{\zeta}]_\times \mathbf{t}_{\eta, \zeta} }{\theta_{\zeta, \eta}^2}$$

在这个式子中，除了$`\mathbf{t}_{\eta, \zeta} `$ 都是常数，并且不包含取模长等非线性操作，可以进一步简化表达为

$$d_{\zeta}^{(\zeta, \eta)} = \frac{\mathbf{a}^\top_{\zeta, \eta}}{\theta_{\zeta, \eta}^2} \mathbf{t}_{\eta, \zeta}$$

至此，我们将$`d_{\zeta}^{(\zeta, \eta)} `$转换为平移的线性表达，这里我们还是重复一下上面推导的DPO约束，

$$0 = [\mathbf{X}_i]_\times \left( d_{\zeta}^{(\zeta, \eta)} R_{i, \zeta}\boldsymbol{X}_{\zeta}+\boldsymbol{t}_{i, \zeta} \right)$$

我们将其中的$`d_{\zeta}^{(\zeta, \eta)} `$替换为线性的表达可以得到

$$0 = [\mathbf{X}_i]_\times \left( R_{i, \zeta}\boldsymbol{X}_{\zeta} \mathbf{a}^\top_{\zeta, \eta} \mathbf{t}_{\eta, \zeta} +\theta_{\zeta,\eta}^{2} \boldsymbol{t}_{i, \zeta} \right)$$

我们希望进一步化简得到更直接的线性表达

$$B \mathbf{t}_{W, \eta} + C \mathbf{t}_{W, i} + D \mathbf{t}_{W, \zeta} = 0, \quad i \leq i \leq n, i \neq \zeta$$

其中，根据上面的数学基础可知 $`\mathbf{R}_{ji}=\mathbf{R}_{jW} \mathbf{R}_{iW}^T `$ , 而平移的 $`\mathbf{t}_{ji}=\mathbf{R}_{jW} (t_{Wi} - t_{Wj}) `$

带入上面的式子即可得到如下化简之后的结果，这里我们直接给出最终的结果

$$\begin{array}{l} B=\left[\boldsymbol{X}_{i}\right]_{\times} R_{i, \zeta}\boldsymbol{X}_{\zeta}\boldsymbol{a}_{\zeta,\eta}^{T} R_{\eta, W}\\ C=\theta_{\zeta,\eta}^{2}\left[\boldsymbol{X}_{i}\right]_{\times} R_{i, W}\\ D=-(B+C) . \end{array}$$

上面考虑的是$`(\zeta, \eta)`$ 图像对的约束，那么进一步考虑多视图几何中全部图像的约束，则可以定义 $`\mathbf{t} = (\mathbf{t}_{1, W}^\top, \cdots, \mathbf{t}_{n, W}^\top)^\top`$ ， 由于都是线性的关系，我们可以把所有的约束组合成为一个简洁的向量表达形式

$$\mathbf{L} \cdot \mathbf{t} = 0$$

其中$`\mathbf{L}`$是$`B,C,D`$组合得到的矩阵。

通过求解上面的线性方程组即可在给定旋转时得到全局平移向量，但是这种求解方法存在两个不足

1. 依赖给定的旋转矩阵精度，如果给的旋转矩阵精度较低则求解得到结果较差
2. 线性方程组的求解结果可能有两个符号相反的解，需要通过一些判定取其中一个解