Visual Jacobians

假定一个 3D 点同时被两个 frame 观测到，两个 frame 的位姿分别为 $T_{wc_i},T_{wc_j}$，在 frame i 上观测得到的归一化坐标为 $[u_i,v_i]$，逆深度为 $\lambda$，则在 frame i 相机坐标系下的齐次坐标 $P_{c_i}$ 为

$$P_{c_i}= \begin{bmatrix} \frac{1}{\lambda}u_i\\ \frac{1}{\lambda}v_i\\ \frac{1}{\lambda}\\ 1 \end{bmatrix}$$

假定在 frame $j$ 相机坐标系下的齐次坐标 $P_{c_j}$ 为

$$\begin{bmatrix} x_{c_j}\\ y_{c_j}\\ z_{c_j}\\ 1 \end{bmatrix}$$

有了上述定义后我们就可以构建视觉重投影残差了，如下

$$P_{c_j}= \begin{bmatrix} x_{c_j}\\ y_{c_j}\\ z_{c_j}\\ 1 \end{bmatrix}= T_{wc_j}^{\top}T_{wc_i} \begin{bmatrix} \frac{1}{\lambda}u_i\\ \frac{1}{\lambda}v_i\\ \frac{1}{\lambda}\\ 1 \end{bmatrix}$$

将两个位姿展开

$$P_{c_j}= \begin{bmatrix} R_{wc_j}^{\top} & -R_{wc_j}^{\top} t_{wc_j} \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R_{wc_i} & t_{wc_i} \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \frac{1}{\lambda}u_i\\ \frac{1}{\lambda}v_i\\ \frac{1}{\lambda}\\ 1 \end{bmatrix}$$

继续化简得

$$P_{c_j}= \begin{bmatrix} R_{wc_j}^{\top}R_{wc_i} & R_{wc_j}^{\top}t_{wc_i}-R_{wc_j}^{\top} t_{wc_j} \\ 0 & 1 \end{bmatrix} P_{c_i}$$

注意在展开后，我们用 $P'$ 来表示去掉最后一个维度 1 的坐标，也就是非齐次坐标。

$$P_{c_j}' = R_{wc_j}^{\top}R_{wc_i}P_{c_i}' + R_{wc_j}^{\top}(t_{wc_i}- t_{wc_j})$$

则残差为

$$f(\cdot) = R_{wc_j}^{\top}R_{wc_i}P_{c_i}' + R_{wc_j}^{\top}(t_{wc_i}- t_{wc_j})-P_{c_j}'$$

我们要优化的变量分别是两个位姿和逆深度，也就是 $R_{wc_i},t_{wc_i},R_{wc_j},t_{wc_j},\lambda$，我们分别进行推导，推导之前先看几个常见的导数

• $R_1^{\top} R_2 p$ 对 $R_2$ 的导数

$$\begin{aligned} \frac{\partial R_1^{\top} R_2 p}{R_2} &=\lim_{\phi \to 0} \frac{R_1^{\top}R_2\exp(\phi^{\wedge})p - R_1^{\top} R_2 p}{\phi}\\ &\approx \lim_{\phi \to 0} \frac{R_1^{\top}R_2 (I+\phi^{\wedge}) p - R_1^{\top} R_2 p}{\phi}\\ &=\lim_{\phi \to 0} \frac{R_1^{\top}R_2 \phi^{\wedge}p}{\phi}\\ &=\lim_{\phi \to 0} \frac{-R_1^{\top}R_2 p^{\wedge} \phi }{\phi}\\ &= -R_1^{\top}R_2 p^{\wedge} \end{aligned}$$

• $R^{\top}p$ 对 $R$ 的导数 （$R^{-1}p$ 对 $R$ 的导数）

$$\begin{aligned} \frac{\partial\left(\boldsymbol{R}^{-1} \boldsymbol{p}\right)}{\partial \boldsymbol{\phi}} & =\lim _{\boldsymbol{\phi} \rightarrow 0} \frac{\left(\boldsymbol{R} \exp \left(\boldsymbol{\phi}^{\wedge}\right)\right)^{-1} \boldsymbol{p}-\boldsymbol{R}^{-\mathbf{1}} \boldsymbol{p}}{\boldsymbol{\phi}} \\ & =\lim _{\boldsymbol{\phi} \rightarrow 0} \frac{\exp \left(\boldsymbol{\phi}^{\wedge}\right)^{-1} \boldsymbol{R}^{-1} \boldsymbol{p}-\boldsymbol{R}^{-\mathbf{1} }\boldsymbol{p}}{\boldsymbol{\phi}} \\ & =\lim _{\boldsymbol{\phi} \rightarrow 0} \frac{\exp \left(-\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{R}^{-1} \boldsymbol{p}-\boldsymbol{R}^{-\mathbf{1} }\boldsymbol{p}}{\boldsymbol{\phi}} \\ & \approx \lim _{\boldsymbol{\phi} \rightarrow 0} \frac{\left(\boldsymbol{I}-\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{R}^{-\mathbf{1}} \boldsymbol{p}-\boldsymbol{R}^{-\mathbf{1} }\boldsymbol{p}}{\boldsymbol{\phi}} \\ & =\lim _{\boldsymbol{\phi} \rightarrow 0} \frac{-\boldsymbol{\phi}^{\wedge} \boldsymbol{R}^{-1} \boldsymbol{p}}{\boldsymbol{\phi}} \\ & =\lim _{\boldsymbol{\phi} \rightarrow 0} \frac{\left(\boldsymbol{R}^{-1} \boldsymbol{p}\right)^{\wedge} \boldsymbol{\phi}}{\phi} \\ & =\left(\boldsymbol{R}^{-1} \boldsymbol{p}\right)^{\wedge} \end{aligned}$$

下面开始正式求导

首先是对 $R_{wc_i}$ 的导数，通过残差公式可见，只有前半部分和 $R_{wc_i}$ 有关，也就是

$$\frac{\partial R_{wc_j}^{\top} R_{wc_i} P_{c_i}'}{\phi}$$

根据上面第一个公式可以得到

$$\frac{\partial R_{wc_j}^{\top} R_{wc_i} P_{c_i}'}{\phi}=-R_{wc_j}^{\top} R_{wc_i} P_{c_i}'^{\wedge}$$

然后是对 $t_{wc_i}$ 的导数，指和后半部分有关，可以直接得到导数为

$$\frac{\partial R_{wc_j}^{\top}(t_{wc_i}- t_{wc_j})}{\partial t_{wc_i}}=R_{wc_j}^{\top}$$

同样可以简单的得到 $t_{wc_j}$ 的导数

$$\frac{\partial R_{wc_j}^{\top}(t_{wc_i}- t_{wc_j})}{\partial t_{wc_j}}=-R_{wc_j}^{\top}$$

对 $R_{wc_j}$ 求导前先进行一些化简，注意到和 $R_{wc_j}$ 有关的项

$$\begin{aligned} R_{wc_j}^{\top}R_{wc_i}P_{c_i}' + R_{wc_j}^{\top}(t_{wc_i}- t_{wc_j})=R_{wc_j}^{\top}(R_{wc_i}P_{c_i}' + t_{wc_i}- t_{wc_j}) \end{aligned}$$

其中

$$R_{wc_j}^{\top}(R_{wc_i}P_{c_i}' + t_{wc_i}- t_{wc_j})=R_{wc_j}^{\top}(P_w'-t_{wc_j})$$

其实到这里我们已经发现其结构和前面的第二个导数结构很像了，$R^{-1}p$ 的结构，导数结果为 $(R^{-1}p)^{\wedge}$，所以接下来我们直接计算该式子的最终结果，然后取反对称即可。根据 SE3 逆运算可知

$$t_{wc_j}=-R_{c_j w}^{\top}t_{c_j w}$$

则

$$\begin{aligned} R_{wc_j}^{\top}(R_{wc_i}P_{c_i}' + t_{wc_i}- t_{wc_j})&=R_{wc_j}^{\top}(P_w'+R_{c_j w}^{\top}t_{c_j w})\\ &=R_{c_jw}P_w'+t_{c_j w}\\ &=P_{c_j}' \end{aligned}$$

所以最终对 $R_{wc_j}$ 的求导结果为 $P_{c_j}'^{\wedge}$。

最后则是对逆深度 $\lambda$的导数，这个很见到，是一个在分子上的常数，直接求导即可，结果为

$$f(\cdot) = R_{wc_j}^{\top}R_{wc_i} \begin{bmatrix} \frac{1}{\lambda}u_i\\ \frac{1}{\lambda}v_i\\ \frac{1}{\lambda} \end{bmatrix} + R_{wc_j}^{\top}(t_{wc_i}- t_{wc_j})-P_{c_j}'$$

结果为

$$-\frac{1}{\lambda^2} R_{wc_j}^{\top}R_{wc_i} \begin{bmatrix} u_i\\ v_i\\ 1 \end{bmatrix}$$