# 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}
$$