这里有一个细节,我想了很久才想明白,DSO 中的 residual 联系了两个关键帧之间的相对位姿,但是最终需要优化帧的绝对位姿,中间的导数怎么转换?这里使用的是李群、李代数中的Adjoint。
参考 http://ethaneade.com/lie.pdf

需要变通一下,字母太多,表达不方便。此处 \(\xi\) 表示 se(3) 和 affLight 参数。

Adjoint 在其中的使用如下(根据代码推断,具体数学推导看我的博客《Adjoint of SE(3)》):

\[\begin{align} {\partial r_{th}^{(i)} \over \partial \xi_{h}}^T {\partial r_{th}^{(i)} \over \partial \xi_{h}} &= \left( {\partial r_{th}^{(i)} \over \partial \xi_{th}} {\partial \xi_{th} \over \partial \xi_{h}} \right)^T {\partial r_{th}^{(i)} \over \partial \xi_{th}} {\partial \xi_{th} \over \partial \xi_{h}} \notag \\ &= {\partial \xi_{th} \over \partial \xi_{h}}^T {\partial r_{th}^{(i)} \over \partial \xi_{th}}^T {\partial r_{th}^{(i)} \over \partial \xi_{th}} {\partial \xi_{th} \over \partial \xi_{h}} \notag \\
{\partial \xi_{th} \over \partial \xi_{h}} &= -\text{Ad}_{T_{th}}\notag \\
{\partial \xi_{th} \over \partial \xi_{t}} &= I \notag\end{align}\]

复习一下 Schur Complement:

\[\begin{align} \begin{bmatrix} H_{\rho\rho} & H_{\rho X} \\ H_{X\rho} & H_{XX} \end{bmatrix} \begin{bmatrix} \delta \rho \\ \delta X \end{bmatrix} &= - \begin{bmatrix} J_{\rho}^T r \\ J_X^T r \end{bmatrix} \notag \\
\begin{bmatrix} H_{\rho\rho} & H_{\rho X} \\ 0 & H_{XX} - H_{X\rho} H_{\rho\rho}^{-1} H_{\rho X} \end{bmatrix} \begin{bmatrix} \delta \rho \\ \delta X \end{bmatrix} &= - \begin{bmatrix} J_{\rho}^T r \\ J_X^T r - H_{X\rho} H_{\rho\rho}^{-1} J_{\rho}^T r \end{bmatrix} \notag \end{align}\]

EnergyFuntional::accumulateAF_MT 和 EnergyFunctional::accumulateLF_MT 的目标是计算\(H_{XX}, J_X^T r\),EnergyFunctional::accumulateSCF_MT 的目标是计算\(H_{X\rho} H_{\rho\rho}^{-1} H_{\rho X}, H_{X\rho} H_{\rho\rho}^{-1} J_{\rho}^T r\)。

\(X\)是68维的,4个相机参数加上8*8个帧状态量。就不写出来了。

这里需要注意一下,\(r\)是Nx1,\(\rho\)是Mx1,\(M \le N\),即 residual 的数目与需要优化的逆深度的数目不一定相等。\(J_{\rho}\)是NxM,\(J_X\)是Nx68。

非 Schur Complement 部分

\[H_{XX} = \begin{bmatrix} {\partial r \over \partial C}^T {\partial r \over \partial C} & {\partial r \over \partial C}^T {\partial r \over \partial \xi} \\ {\partial r \over \partial \xi}^T {\partial r \over \partial C} & {\partial r \over \partial \xi}^T {\partial r \over \partial \xi} \end{bmatrix}\]

\[\begin{align} {\partial r \over \partial C}^T {\partial r \over \partial C} &= \begin{bmatrix} {\partial r^{(1)} \over \partial C}^T & {\partial r^{(2)} \over \partial C}^T & \dots & {\partial r^{(N)} \over \partial C}^T \end{bmatrix} \begin{bmatrix} {\partial r^{(1)} \over \partial C} \\ {\partial r^{(2)} \over \partial C} \\ \dots \\ {\partial r^{(N)} \over \partial C} \end{bmatrix} \notag \\ &= \sum_{i=1}^{N} {\partial r^{(i)} \over \partial C}^T{\partial r^{(i)} \over \partial C} \notag \end{align}\]

\[\begin{align} {\partial r \over \partial C}^T {\partial r \over \partial \xi} &= \begin{bmatrix} {\partial r^{(1)} \over \partial C}^T & {\partial r^{(2)} \over \partial C}^T & \dots & {\partial r^{(N)} \over \partial C}^T \end{bmatrix} \begin{bmatrix} {\partial r^{(1)} \over \partial \xi_1} & {\partial r^{(1)} \over \partial \xi_2} & \dots & {\partial r^{(1)} \over \partial \xi_8} \\ {\partial r^{(2)} \over \partial \xi_1} & {\partial r^{(2)} \over \partial \xi_2} & \dots & {\partial r^{(2)} \over \partial \xi_8} \\ \vdots & \vdots & \ddots & \vdots \\ {\partial r^{(N)} \over \partial \xi_1} & {\partial r^{(N)} \over \partial \xi_2} & \dots & {\partial r^{(N)} \over \partial \xi_8} \end{bmatrix} \notag \\ &= \begin{bmatrix} \sum_{i=1}^{N} {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \xi_1} & \sum_{i=1}^{N} {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \xi_2} & \dots & \sum_{i=1}^{N} {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \xi_8} \end{bmatrix} \notag \end{align}\]

\[\begin{align} {\partial r \over \partial \xi}^T {\partial r \over \partial \xi} &= \begin{bmatrix} {\partial r^{(1)} \over \partial \xi_1}^T & {\partial r^{(2)} \over \partial \xi_1}^T & \dots & {\partial r^{(N)} \over \partial \xi_1}^T \\ {\partial r^{(1)} \over \partial \xi_2}^T & {\partial r^{(2)} \over \partial \xi_2}^T & \dots & {\partial r^{(N)} \over \partial \xi_2}^T \\ \vdots & \vdots & \ddots & \vdots \\ {\partial r^{(1)} \over \partial \xi_8}^T & {\partial r^{(2)} \over \partial \xi_8}^T & \dots & {\partial r^{(N)} \over \partial \xi_8}^T \end{bmatrix} \begin{bmatrix} {\partial r^{(1)} \over \partial \xi_1} & {\partial r^{(1)} \over \partial \xi_2} & \dots & {\partial r^{(1)} \over \partial \xi_8} \\ {\partial r^{(2)} \over \partial \xi_1} & {\partial r^{(2)} \over \partial \xi_2} & \dots & {\partial r^{(2)} \over \partial \xi_8} \\ \vdots & \vdots & \ddots & \vdots \\ {\partial r^{(N)} \over \partial \xi_1} & {\partial r^{(N)} \over \partial \xi_2} & \dots & {\partial r^{(N)} \over \partial \xi_8} \end{bmatrix} \notag \\
&= \begin{bmatrix} \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T{\partial r^{(i)} \over \partial \xi_1} & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T{\partial r^{(i)} \over \partial \xi_2} & \dots & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T{\partial r^{(i)} \over \partial \xi_8} \\ \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_2}^T{\partial r^{(i)} \over \partial \xi_1} & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_2}^T{\partial r^{(i)} \over \partial \xi_2} & \dots & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_2}^T{\partial r^{(i)} \over \partial \xi_8} \\ \vdots & \vdots & \ddots & \vdots \\ \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T{\partial r^{(i)} \over \partial \xi_1} & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T{\partial r^{(i)} \over \partial \xi_2} & \dots & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T{\partial r^{(i)} \over \partial \xi_8} \end{bmatrix} \notag \end{align}\]

\[\begin{align} J_{X}^Tr &= \begin{bmatrix} {\partial r \over \partial C}^T \\ {\partial r \over \partial \xi}^T \end{bmatrix} r \notag \\ &= \begin{bmatrix} {\partial r^{(1)} \over \partial C}^T & {\partial r^{(2)} \over \partial C}^T & \dots & {\partial r^{(N)} \over \partial C}^T \\ {\partial r^{(1)} \over \partial \xi_1}^T & {\partial r^{(2)} \over \partial \xi_1}^T & \dots & {\partial r^{(N)} \over \partial \xi_1}^T \\ {\partial r^{(1)} \over \partial \xi_2}^T & {\partial r^{(2)} \over \partial \xi_2}^T & \dots & {\partial r^{(N)} \over \partial \xi_2}^T \\ \vdots & \vdots & \ddots & \vdots \\ {\partial r^{(1)} \over \partial \xi_8}^T & {\partial r^{(2)} \over \partial \xi_8}^T & \dots & {\partial r^{(N)} \over \partial \xi_8}^T \end{bmatrix} \begin{bmatrix} r^{(1)} \\ r^{(2)} \\ \vdots \\ r^{(N)}\end{bmatrix} \notag \\
&= \begin{bmatrix} \sum_{i = 1}^{N} {\partial r^{(i)} \over \partial C}^T r^{(i)} \\ \sum_{i = 1}^{N} {\partial r^{(i)} \over \partial \xi_1}^T r^{(i)} \\ \sum_{i = 1}^{N} {\partial r^{(i)} \over \partial \xi_2}^T r^{(i)} \\ \vdots \\ \sum_{i = 1}^{N} {\partial r^{(i)} \over \partial \xi_8}^T r^{(i)} \end{bmatrix} \notag \end{align}\]

所以算这些矩阵就是遍历每一个 residual,累加求和。

Schur Complement 部分

\[\begin{align} \begin{bmatrix} H_{\rho\rho} & H_{\rho X} \\ H_{X\rho} & H_{XX} \end{bmatrix} \begin{bmatrix} \delta \rho \\ \delta X \end{bmatrix} &= - \begin{bmatrix} J_{\rho}^T r \\ J_X^T r \end{bmatrix} \notag \\
\begin{bmatrix} H_{\rho\rho} & H_{\rho X} \\ 0 & H_{XX} - H_{X\rho} H_{\rho\rho}^{-1} H_{\rho X} \end{bmatrix} \begin{bmatrix} \delta \rho \\ \delta X \end{bmatrix} &= - \begin{bmatrix} J_{\rho}^T r \\ J_X^T r - H_{X\rho} H_{\rho\rho}^{-1} J_{\rho}^T r \end{bmatrix} \notag \end{align}\]

Hsc:
\[H_{X\rho} H_{\rho\rho}^{-1} H_{\rho X}\]
bsc:
\[H_{X\rho} H_{\rho\rho}^{-1} J_{\rho}^T r\]

\[\begin{align} J_\rho^TJ_\rho &= {\partial r \over \partial \rho}^T {\partial r \over \partial \rho} \notag \\
&= \begin{bmatrix} {\partial r^{(1)} \over \partial \rho^{(1)}}^T & {\partial r^{(2)} \over \partial \rho^{(1)}}^T & \dots & {\partial r^{(N)} \over \partial \rho^{(1)}}^T \\ {\partial r^{(1)} \over \partial \rho^{(2)}}^T & {\partial r^{(2)} \over \partial \rho^{(2)}}^T & \dots & {\partial r^{(N)} \over \partial \rho^{(2)}}^T \\ \vdots & \vdots & \ddots & \vdots \\ {\partial r^{(1)} \over \partial \rho^{(M)}}^T & {\partial r^{(2)} \over \partial \rho^{(M)}}^T & \dots & {\partial r^{(N)} \over \partial \rho^{(M)}}^T \end{bmatrix} \begin{bmatrix} {\partial r^{(1)} \over \partial \rho^{(1)}} & {\partial r^{(1)} \over \partial \rho^{(2)}} & \dots & {\partial r^{(1)} \over \partial \rho^{(M)}} \\ {\partial r^{(2)} \over \partial \rho^{(1)}} & {\partial r^{(2)} \over \partial \rho^{(2)}} & \dots & {\partial r^{(2)} \over \partial \rho^{(M)}} \\ \vdots & \vdots & \ddots & \vdots \\ {\partial r^{(N)} \over \partial \rho^{(1)}} & {\partial r^{(N)} \over \partial \rho^{(2)}} & \dots & {\partial r^{(N)} \over \partial \rho^{(M)}} \end{bmatrix} \notag \\
&= \begin{bmatrix} \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(1)}}^T{\partial r^{(i)} \over \partial \rho^{(1)}} & \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(1)}}^T{\partial r^{(i)} \over \partial \rho^{(2)}} & \dots & \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(1)}}^T{\partial r^{(i)} \over \partial \rho^{(M)}} \\ \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(2)}}^T{\partial r^{(i)} \over \partial \rho^{(1)}} & \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(2)}}^T{\partial r^{(i)} \over \partial \rho^{(2)}} & \dots & \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(2)}}^T{\partial r^{(i)} \over \partial \rho^{(M)}} \\ \vdots & \vdots & \ddots & \vdots \\ \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(M)}}^T{\partial r^{(i)} \over \partial \rho^{(1)}} & \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(M)}}^T{\partial r^{(i)} \over \partial \rho^{(2)}} & \dots & \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(M)}}^T{\partial r^{(i)} \over \partial \rho^{(M)}} \end{bmatrix} \notag \\
&= \begin{bmatrix} \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(1)}}^T{\partial r^{(i)} \over \partial \rho^{(1)}} & 0 & \dots & 0 \\ 0 & \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(2)}}^T{\partial r^{(i)} \over \partial \rho^{(2)}} & \dots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \dots & \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(M)}}^T{\partial r^{(i)} \over \partial \rho^{(M)}} \end{bmatrix} \notag \end{align}\]

\[ \begin{align} {\partial r \over \partial C}^T {\partial r \over \partial \rho} &= \begin{bmatrix} {\partial r^{(1)} \over \partial C}^T & {\partial r^{(2)} \over \partial C}^T & \dots & {\partial r^{(N)} \over \partial C}^T\end{bmatrix} \begin{bmatrix} {\partial r^{(1)} \over \partial \rho^{(1)}} & {\partial r^{(1)} \over \partial \rho^{(2)}} & \dots & {\partial r^{(1)} \over \partial \rho^{(M)}} \\ {\partial r^{(2)} \over \partial \rho^{(1)}} & {\partial r^{(2)} \over \partial \rho^{(2)}} & \dots & {\partial r^{(2)} \over \partial \rho^{(M)}} \\ \vdots & \vdots & \ddots & \vdots \\ {\partial r^{(N)} \over \partial \rho^{(1)}} & {\partial r^{(N)} \over \partial \rho^{(2)}} & \dots & {\partial r^{(N)} \over \partial \rho^{(M)}} \end{bmatrix} \notag \\ &= \begin{bmatrix} \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(1)}} & \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(2)}} & \dots & \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \end{bmatrix} \notag \end{align} \]

\[\begin{align} {\partial r \over \partial \xi}^T {\partial r \over \partial \rho} &= \begin{bmatrix} {\partial r^{(1)} \over \partial \xi_1}^T & {\partial r^{(2)} \over \partial \xi_1}^T & \dots & {\partial r^{(N)} \over \partial \xi_1}^T \\ {\partial r^{(1)} \over \partial \xi_2}^T & {\partial r^{(2)} \over \partial \xi_2}^T & \dots & {\partial r^{(N)} \over \partial \xi_2}^T \\ \vdots & \vdots & \ddots & \vdots \\ {\partial r^{(1)} \over \partial \xi_8}^T & {\partial r^{(2)} \over \partial \xi_8}^T & \dots & {\partial r^{(N)} \over \partial \xi_8}^T \end{bmatrix} \begin{bmatrix} {\partial r^{(1)} \over \partial \rho^{(1)}} & {\partial r^{(1)} \over \partial \rho^{(2)}} & \dots & {\partial r^{(1)} \over \partial \rho^{(M)}} \\ {\partial r^{(2)} \over \partial \rho^{(1)}} & {\partial r^{(2)} \over \partial \rho^{(2)}} & \dots & {\partial r^{(2)} \over \partial \rho^{(M)}} \\ \vdots & \vdots & \ddots & \vdots \\ {\partial r^{(N)} \over \partial \rho^{(1)}} & {\partial r^{(N)} \over \partial \rho^{(2)}} & \dots & {\partial r^{(N)} \over \partial \rho^{(M)}} \end{bmatrix} \notag \\
&= \begin{bmatrix} \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(1)}} & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(2)}} & \dots & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \\ \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_2}^T {\partial r^{(i)} \over \partial \rho^{(1)}} & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_2}^T {\partial r^{(i)} \over \partial \rho^{(2)}} & \dots & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_2}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \\ \vdots & \vdots & \ddots & \vdots \\ \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(1)}} & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(2)}} & \dots & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \end{bmatrix} \notag \end{align}\]

\[\begin{align} H_{X\rho} H_{\rho\rho}^{-1} H_{\rho X} &= J_X^T J_\rho (J_\rho^TJ_\rho)^{-1} J_\rho^T J_X \notag \\
&= \begin{bmatrix} {\partial r \over \partial C}^T \\ {\partial r \over \partial \xi}^T \end{bmatrix} {\partial r \over \partial \rho} \left( {\partial r \over \partial \rho}^T {\partial r \over \partial \rho} \right)^{-1} {\partial r \over \partial \rho}^T \begin{bmatrix} {\partial r \over \partial C} & {\partial r \over \partial \xi} \end{bmatrix} \notag \\
&= \begin{bmatrix} {\partial r \over \partial C}^T {\partial r \over \partial \rho} \\ {\partial r \over \partial \xi}^T {\partial r \over \partial \rho} \end{bmatrix} \begin{bmatrix} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(1)}}^T{\partial r^{(i)} \over \partial \rho^{(1)}} \right)^{-1} & 0 & \dots & 0 \\ 0 & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(2)}}^T{\partial r^{(i)} \over \partial \rho^{(2)}} \right)^{-1} & \dots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(M)}}^T{\partial r^{(i)} \over \partial \rho^{(M)}} \right)^{-1} \end{bmatrix} \notag \\ & \begin{bmatrix} {\partial r \over \partial \rho}^T {\partial r \over \partial C} & {\partial r \over \partial \rho}^T {\partial r \over \partial \xi} \end{bmatrix} \notag \\
&= \begin{bmatrix} \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(1)}} & \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(2)}} & \dots & \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \\ \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(1)}} & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(2)}} & \dots & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \\ \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_2}^T {\partial r^{(i)} \over \partial \rho^{(1)}} & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_2}^T {\partial r^{(i)} \over \partial \rho^{(2)}} & \dots & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_2}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \\ \vdots & \vdots & \ddots & \vdots \\ \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(1)}} & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(2)}} & \dots & \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \end{bmatrix} \notag \\
&\begin{bmatrix} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(1)}}^T{\partial r^{(i)} \over \partial \rho^{(1)}} \right)^{-1} & 0 & \dots & 0 \\ 0 & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(2)}}^T{\partial r^{(i)} \over \partial \rho^{(2)}} \right)^{-1} & \dots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(M)}}^T{\partial r^{(i)} \over \partial \rho^{(M)}} \right)^{-1} \end{bmatrix} \notag \\
&\begin{bmatrix} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(1)}} \right)^T & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(1)}} \right)^T & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(1)}} \right)^T \\ \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(2)}} \right)^T & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(2)}} \right)^T & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(2)}} \right)^T \\ \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(3)}} \right)^T & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(3)}} \right)^T & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(3)}} \right)^T \\
\vdots & \vdots & \ddots & \vdots \\ \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \right)^T & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \right)^T & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \right)^T \end{bmatrix} \notag \\
&= \begin{bmatrix} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(1)}} \right)\left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(1)}}^T{\partial r^{(i)} \over \partial \rho^{(1)}} \right)^{-1} & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(2)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(2)}}^T{\partial r^{(i)} \over \partial \rho^{(2)}} \right)^{-1} & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(M)}}^T{\partial r^{(i)} \over \partial \rho^{(M)}} \right)^{-1} \\ \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(1)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(1)}}^T{\partial r^{(i)} \over \partial \rho^{(1)}} \right)^{-1} & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(2)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(2)}}^T{\partial r^{(i)} \over \partial \rho^{(2)}} \right)^{-1} & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(M)}}^T{\partial r^{(i)} \over \partial \rho^{(M)}} \right)^{-1} \\ \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_2}^T {\partial r^{(i)} \over \partial \rho^{(1)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(1)}}^T{\partial r^{(i)} \over \partial \rho^{(1)}} \right)^{-1} & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_2}^T {\partial r^{(i)} \over \partial \rho^{(2)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(2)}}^T{\partial r^{(i)} \over \partial \rho^{(2)}} \right)^{-1} & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_2}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(M)}}^T{\partial r^{(i)} \over \partial \rho^{(M)}} \right)^{-1} \\ \vdots & \vdots & \ddots & \vdots \\ \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(1)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(1)}}^T{\partial r^{(i)} \over \partial \rho^{(1)}} \right)^{-1} & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(2)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(2)}}^T{\partial r^{(i)} \over \partial \rho^{(2)}} \right)^{-1} & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(M)}}^T{\partial r^{(i)} \over \partial \rho^{(M)}} \right)^{-1} \end{bmatrix} \notag \\
&\begin{bmatrix} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(1)}} \right)^T & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(1)}} \right)^T & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(1)}} \right)^T \\ \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(2)}} \right)^T & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(2)}} \right)^T & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(2)}} \right)^T \\ \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(3)}} \right)^T & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(3)}} \right)^T & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(3)}} \right)^T \\
\vdots & \vdots & \ddots & \vdots \\ \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \right)^T & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \right)^T & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \right)^T \end{bmatrix}\notag \end{align}\]

同理
\[\begin{align} H_{X\rho} H_{\rho\rho}^{-1} J_\rho^T r \notag & = \notag \begin{bmatrix} \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(1)}} \right)\left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(1)}}^T{\partial r^{(i)} \over \partial \rho^{(1)}} \right)^{-1} & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(2)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(2)}}^T{\partial r^{(i)} \over \partial \rho^{(2)}} \right)^{-1} & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial C}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(M)}}^T{\partial r^{(i)} \over \partial \rho^{(M)}} \right)^{-1} \\ \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(1)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(1)}}^T{\partial r^{(i)} \over \partial \rho^{(1)}} \right)^{-1} & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(2)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(2)}}^T{\partial r^{(i)} \over \partial \rho^{(2)}} \right)^{-1} & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_1}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(M)}}^T{\partial r^{(i)} \over \partial \rho^{(M)}} \right)^{-1} \\ \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_2}^T {\partial r^{(i)} \over \partial \rho^{(1)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(1)}}^T{\partial r^{(i)} \over \partial \rho^{(1)}} \right)^{-1} & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_2}^T {\partial r^{(i)} \over \partial \rho^{(2)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(2)}}^T{\partial r^{(i)} \over \partial \rho^{(2)}} \right)^{-1} & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_2}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(M)}}^T{\partial r^{(i)} \over \partial \rho^{(M)}} \right)^{-1} \\ \vdots & \vdots & \ddots & \vdots \\ \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(1)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(1)}}^T{\partial r^{(i)} \over \partial \rho^{(1)}} \right)^{-1} & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(2)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(2)}}^T{\partial r^{(i)} \over \partial \rho^{(2)}} \right)^{-1} & \dots & \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \xi_8}^T {\partial r^{(i)} \over \partial \rho^{(M)}} \right) \left( \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(M)}}^T{\partial r^{(i)} \over \partial \rho^{(M)}} \right)^{-1} \end{bmatrix} \notag \\
& \begin{bmatrix} \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(1)}}^T r^{(i)} \\ \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(2)}}^T r^{(i)} \\
\vdots \\ \sum_{i=1}^N {\partial r^{(i)} \over \partial \rho^{(M)}}^T r^{(i)} \end{bmatrix} \end{align}\]

DSO windowed optimization 公式的更多相关文章

  1. DSO windowed optimization 代码 (4)

    5 "step"计算 参考<DSO windowed optimization 公式>,计算各个优化变量的增加量. 公式再写一下: \[\begin{align} \b ...

  2. DSO windowed optimization 代码 (3)

    4 Schur Complement 部分信息计算 参考<DSO windowed optimization 公式>,Schur Complement 部分指 Hsc(\(H_{X\rho ...

  3. DSO windowed optimization 代码 (2)

    3 非 Schur Complement 部分信息计算 参考<DSO windowed optimization 公式>,非Schur Complement 部分指 \(H_{XX}\) ...

  4. DSO windowed optimization 代码 (1)

    这里不想解释怎么 marginalize,什么是 First-Estimates Jacobian (FEJ).这里只看看代码,看看Hessian矩阵是怎么构造出来的. 1 优化流程 整个优化过程,也 ...

  5. Adjoint of SE(3)

    以前看的书都提到 SE(3) 和 se(3) 的 Adjoint,但是并没有讲这个东西是干什么用的,只是给了一堆性质.这东西来自群论. 参考 Lie Groups for 2D and 3D Tran ...

  6. Paper Reading: Stereo DSO

    开篇第一篇就写一个paper reading吧,用markdown+vim写东西切换中英文挺麻烦的,有些就偷懒都用英文写了. Stereo DSO: Large-Scale Direct Sparse ...

  7. Omnidirectional DSO: Direct Sparse Odometry with Fisheye Cameras 论文摘要

    1. Abstract 通过一种Unified Omnidirectional Model作为投影方程. 这种方式可以使用图像的所有内容包括有强畸变的区域,而现存的视觉里程计方案只能修正或者切掉来使用 ...

  8. [CS231n-CNN] Linear classification II, Higher-level representations, image features, Optimization, stochastic gradient descent

    课程主页:http://cs231n.stanford.edu/ loss function: -Multiclass SVM loss: 表示实际应该属于的类别的score.因此,可以发现,如果实际 ...

  9. 在线最优化求解(Online Optimization)之五:FTRL

    在线最优化求解(Online Optimization)之五:FTRL 在上一篇博文中中我们从原理上定性比较了L1-FOBOS和L1-RDA在稀疏性上的表现.有实验证明,L1-FOBOS这一类基于梯度 ...

随机推荐

  1. BZOJ2135 刷题计划(贪心+二分)

    相邻数作差后容易转化成将这些数最多再切m刀能获得的最小偏差值.大胆猜想化一波式子可以发现将一个数平均分是最优的.并且划分次数越多能获得的偏差值增量越小.那么就可以贪心了:将所有差扔进堆里,每次取出增量 ...

  2. hg和git命令对照表

    hg和git命令对照表 来源 https://github.com/sympy/sympy/wiki/Git-hg-rosetta-stone Git hg rosetta stone   muxat ...

  3. python构建bp神经网络_曲线拟合(一个隐藏层)__2.代码实现

    IDE:jupyter 抽象程度可能不是那么高,以后再优化. 理论和代码实现的差距还是挺大的 数据集请查看 python构建bp神经网络(一个隐藏层)__1.数据可视化 部分代码预览 git上传.ip ...

  4. django---APIView源码分析

    django---APIView源码分析 前言:APIView基于View 看这部分内容一定要懂django-CBV里的内容 在django-CBV源码分析中,我们是分析的from django.vi ...

  5. Mysql经典sql语句大全

    DDL-数据定义语言(Create,Alter,Drop,DECLARE) DML-数据操纵语言(Select,Delete,Update,Insert) DCL-数据控制语言(GRANT,REVOK ...

  6. shiro中自定义realm实现md5散列算法加密的模拟

    shiro中自定义realm实现md5散列算法加密的模拟.首先:我这里是做了一下shiro 自定义realm散列模拟,并没有真正链接数据库,因为那样东西就更多了,相信学到shiro的人对连接数据库的一 ...

  7. Elasticsearch日志分析系统

    Elasticsearch日志分析系统 作者:尹正杰 版权声明:原创作品,谢绝转载!否则将追究法律责任. 一.什么是Elasticsearch 一个采用Restful API标准的高扩展性的和高可用性 ...

  8. IP基本原理

    IP基本原理 一.IP基本原理 IP是网络层协议,也是当今应用最广泛的网络协议之一 IP协议规定了数据的封装方式,网络节点的标识方法,用于网络上数据的端到端的传递. 1.IP及其相关协议 2.IP的主 ...

  9. 面向对象【day08】:静态方法、类方法、属性方法(九)

    本节内容 概述 静态方法 类方法 属性方法 总结 一.概述 前面我们已经讲解了关于类的很多东西,今天讲讲类的另外的特性:静态方法(staticmethod).类方法(classmethod).属性方法 ...

  10. JVM调优命令-jinfo

    jinfo JVM Configuration info这个命令作用是实时查看和调整虚拟机运行参数. 之前的jps -v口令只能查看到显示指定的参数,如果想要查看未被显示指定的参数的值就要使用jinf ...