DSO 代碼中初始化的部分。CoarseInitializer 將第一幀作為 ref frame，第二幀作為 new frame。ref frame 的 idepth (inverse depth) 一開始的時候都設置為1，隨后在確定 new frame 相對 ref frame 之間相對位姿、光度變化過程中設定為正確值。

new frame 相對 ref frame 之間存在 8 個參數需要確定，前 6 個參數是 se(3)，后 2 個參數是光度仿射變換的參數。

本文參考高翔博士的《DSO詳解》：https://zhuanlan.zhihu.com/p/29177540。

此處以及以后 DSO 相關的文章對導數的定義如下：

\[\begin{align} \frac{\partial \mathbf{f}}{\partial \mathbf{x}} = \begin{bmatrix} \frac{\partial f_1}{\partial x_1} & \frac{\partial f_1}{\partial x_2} & \dots & \frac{\partial f_1}{\partial x_m} \\ \frac{\partial f_2}{\partial x_1} & \frac{\partial f_2}{\partial x_2} & \dots & \frac{\partial f_2}{\partial x_m} \\ \vdots & \vdots & \ddots & \vdots \\ \frac{\partial f_p}{\partial x_1} & \frac{\partial f_p}{\partial x_2} & \dots & \frac{\partial f_p}{\partial x_m} \end{bmatrix} \end{align}\]

1 光度仿射變換

光度仿射變換是將兩幀之間輻射值進行對應。對應的參數有兩個 \(\begin{bmatrix} a, b \end{bmatrix}\)。

將影像 1 的輻射值變換到影像 2 中：

\[\begin{align} I_2 = a_{21} I_1 + b_{21} \end{align}\]

\(I_1\) 表示影像 1 中的輻射值，\(I_2\) 表示影像 2 中的輻射值。

系統中其他地方求光度誤差的方法，是按照 AffLight::fromToVecExposure 和計算光度誤差的語句計算的。所以實際上相對光度仿射變換參數應該是如下方法計算（\(\Delta t_1, \Delta t_2\) 是曝光時間）：

\[\begin{align} a_{21} &= {e^{a_2} \Delta t_2 \over e^{a_1} \Delta t_1} \\ b_{21} &= b_2 - a_{21} b_1 \end{align}\]

但是需要注意到現在是初始化，設定了影像 1 的光度變換參數為 \(\begin{bmatrix} 0, 0 \end{bmatrix}\)。所以相對光度仿射變換參數可以退化成下面的形式：

\[\begin{align} a_{21} &= {e^{a_2} \Delta t_2 \over \Delta t_1} \\ b_{21} &= b_2 \end{align}\]

2 光度誤差

光度誤差可以用以下公式計算：

\[\begin{align} r = w_h (I_2[x_2] - (a_{21}I_1[x_1] + b_{21})) \end{align}\]

其中 \(w_h\) 是 Huber 權重，\(I_1, I_2\) 分別是影像1、2，\(x_1, x_2\) 分別是空間中一點 \(X\) 在影像上的像素坐標。

其中 \(x_2\) 可以寫作

\[\begin{align} x_2 = f(x_1, \xi_{21}, \rho_1) \end{align}\]

即 \(x_2\) 是由 \(x_1\) 投影而來，投影的過程中需要兩幀之間的相對位姿 \(\xi_{21}\) 和 \(x_1\) 在 1 中的逆深度 \(\rho_1\)。

3 導數

3.1 光度仿射變換導數 \({\partial r_{21} \over \partial a_{21}}, {\partial r_{21} \over \partial b_{21}}\)

從最簡單的光度誤差參數開始求導數：

\[\begin{align} {\partial r_{21} \over \partial a_{21}} &= - w_h I_1[x_1] \\ {\partial r_{21} \over \partial b_{21}} &= -w_h \end{align}\]

我認為 DSO 代碼在這里寫錯了，按照 DSO 代碼，應該如此：

\[\begin{align} {\partial r_{21} \over \partial a_{21}} = - w_h a_{21}I_1[x_1] \end{align}\]

我懷疑是不是我看錯了，其實 DSO 優化 \(a_2\)，這樣就有

\[\begin{align} {\partial r_{21} \over \partial a_{2}} = - w_h a_{2}I_1[x_1] \end{align}\]

但是實際并不是這樣的，代碼是正確的。DSO 優化的參數就是 \(a_{21}\)，從這里將曝光時間加入到初始值中，就可以看出優化的參數就是相對光度變換參數。

3.2 相對位姿導數 \({\partial r_{21} \over \partial \xi_{21}}\) 和逆深度導數 \({\partial r_{21} \over \partial \rho_{1}}\)

隨后求光度誤差對相對位姿和逆深度的導數：

\[{\partial r_{21} \over \partial \xi_{21}}, {\partial r_{21} \over \partial \rho_{1}}\]

通過鏈式法則：

\[\begin{align} {\partial r_{21} \over \partial \xi_{21}} &= {\partial r_{21} \over \partial x_{2}} {\partial x_{2} \over \partial \xi_{21}} \\ {\partial r_{21} \over \partial \rho_{1}} &= {\partial r_{21} \over \partial x_{2}} {\partial x_{2} \over \partial \rho_{1}} \end{align}\]

\({\partial r_{21} \over \partial x_{2}}\) 可以方便求得：

\[\begin{align} {\partial r_{21} \over \partial x_{2}} = w_h {\partial I_2[x_2] \over \partial x_{2}} = w_h \begin{bmatrix} g_x \\ g_y\end{bmatrix} \end{align}\]

\(g_x, g_y\) 是影像 \(I_2\) 在 \(x_2\) 處的梯度。

現在，我們需要求的是 \({\partial x_{2} \over \partial \xi_{21}}\) 和 \({\partial x_{2} \over \partial \rho_1}\)。

分別列兩張影像的投影方程如下：

\[\begin{align} \begin{cases} \rho_1^{-1} x_1 &= K X_1 \\ \rho_2^{-1} x_2 &= K (R_{21} X_1 + t_{21}) \end{cases} \end{align}\]

整理一下

\[\begin{align} X_1 &= \rho_1^{-1} K^{-1} x_1 \\ x_2 &= K \rho_2 (R_{21} \rho_1^{-1} K^{-1} x_1 + t_{21}) \notag \\ &= K x^{\prime}_2 \end{align}\]

上式中的 \(x^{\prime}_2\) 就是歸一化平面坐標，可以寫作

\[x^{\prime}_2 = \begin{bmatrix}u^{\prime}_2, v^{\prime}_2, 1 \end{bmatrix} ^T\]

通過鏈式法則，整理一下，我們需要計算的\({\partial x_{2} \over \partial \xi_{21}}\) 和 \({\partial x_{2} \over \partial \rho_1}\)：

\[\begin{align}{\partial x_{2} \over \partial \xi_{21}} &= {\partial x_{2}\over \partial x^{\prime}_{2}} {\partial x^{\prime}_{2} \over \partial \xi_{21}} \\ {\partial x_{2} \over \partial \rho_1} &= {\partial x_{2}\over \partial x^{\prime}_{2}} {\partial x^{\prime}_{2} \over \partial \rho_1} \end{align}\]

先計算兩者的共同部分 \({\partial x_{2}\over \partial x^{\prime}_{2}}\)。

\[\begin{align} x_2 &= K x^{\prime}_2 \\ \begin{bmatrix} u_2 \\ v_2 \\ 1 \end{bmatrix}&= \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1\end{bmatrix} \begin{bmatrix} u^{\prime}_2 \\ v^{\prime}_2 \\ 1 \end{bmatrix} \end{align}\]

于是有

\[\begin{align}{\partial x_{2}\over \partial x^{\prime}_{2}} &= \begin{bmatrix} {\partial u_{2}\over \partial u^{\prime}_{2}} & {\partial u_{2}\over \partial v^{\prime}_{2}} & {\partial u_{2}\over \partial 1} \\ {\partial v_{2}\over \partial u^{\prime}_{2}} & {\partial v_{2}\over \partial v^{\prime}_{2}} & {\partial v_{2}\over \partial 1} \\ {\partial 1\over \partial u^{\prime}_{2}} & {\partial 1\over \partial v^{\prime}_{2}} & {\partial 1\over \partial 1}\end{bmatrix}\notag \\ &= \begin{bmatrix} f_x & 0 & 0 \\ 0 & f_y & 0 \\ 0 & 0 & 0 \end{bmatrix} \end{align}\]

隨后分別計算 \({\partial x^{\prime}_{2} \over \partial \xi_{21}}\) 和 \({\partial x^{\prime}_{2} \over \partial \rho_1}\) 。

3.2.1 推導 \({\partial x^{\prime}_{2} \over \partial \rho_1}\)

\[\begin{align} {\partial x^{\prime}_{2} \over \partial \rho_1} = \begin{bmatrix} {\partial u^{\prime}_{2} \over \partial \rho_1} \\ {\partial v^{\prime}_{2} \over \partial \rho_1} \\ 0\end{bmatrix} \end{align}\]

\[\begin{align} x^{\prime}_2 = \rho_2 (R_{21} \rho_1^{-1} K^{-1} x_1 + t_{21}) \end{align}\]

將 \(R_{21} K^{-1} x_1\) 看做整體，矩陣 \(A\)，矩陣 \(A\) 有三行。

\[\begin{align} A = \begin{bmatrix} a_1^T \\ a_2^T \\ a_3^T \end{bmatrix} \end{align}\]

\(x^{\prime}_2\) 就可以寫成如下形式

\[\begin{align} \begin{bmatrix} u^{\prime}_2 \\ v^{\prime}_2 \\ 1 \end{bmatrix} = \rho_2 \begin{bmatrix} \rho_1^{-1} a_1^Tx_1 + t_{21}^x \\ \rho_1^{-1} a_2^Tx_1 + t_{21}^y \\ \rho_1^{-1} a_3^Tx_1 + t_{21}^z \end{bmatrix} \end{align}\]

可得 2 中的逆深度

\[\begin{align} \rho_2 = (\rho_1^{-1} a_3^Tx_1 + t_{21}^z)^{-1} \end{align}\]

代入

\[\begin{align} u^{\prime}_2 &= \rho_2(\rho_1^{-1}a_1^Tx_1 + t_{21}^x) \notag \\ &= {{\rho_1^{-1}a_1^Tx_1 + t_{21}^x} \over {\rho_1^{-1} a_3^Tx_1 + t_{21}^z}} \notag \\ &= {{a_1^Tx_1 + \rho_1 t_{21}^x} \over {a_3^Tx_1 + \rho_1 t_{21}^z}} \end{align}\]

\[\begin{align} {\partial u^{\prime}_{2} \over \partial \rho_1} &= t_{21}^x {1 \over {a_3^Tx_1 + \rho_1 t_{21}^z}} \notag \\ &\ \ + (a_1^Tx_1 + \rho_1 t_{21}^x) {1 \over {(a_3^Tx_1 + \rho_1 t_{21}^z)^2}} (-1) t_{21}^z \notag \\ &= \rho_1^{-1} {1 \over {\rho_1^{-1}a_3^Tx_1 + t_{21}^z}}(t_{21}^x - {{a_1^Tx_1 + \rho_1 t_{21}^x} \over {a_3^Tx_1 + \rho_1 t_{21}^z}}t_{21}^z)\notag \\ &= \rho_1^{-1}\rho_2(t_{21}^x - u^{\prime}_2t_{21}^z) \end{align}\]

同理

\[\begin{align} {\partial v^{\prime}_{2} \over \partial \rho_1} = \rho_1^{-1}\rho_2(t_{21}^y - v^{\prime}_2t_{21}^z) \end{align}\]

結論

\[\begin{align} {\partial x^{\prime}_{2} \over \partial \rho_1} = \begin{bmatrix} \rho_1^{-1}\rho_2(t_{21}^x - u^{\prime}_2t_{21}^z) \\ \rho_1^{-1}\rho_2(t_{21}^y - v^{\prime}_2t_{21}^z) \\ 0\end{bmatrix} \end{align}\]

3.2.2 推導 \({\partial x^{\prime}_{2} \over \partial \xi_{21}}\)

\[\begin{align} x^{\prime}_2 = \rho_2(R_{21}X_1 + t_{21}) = \rho_2 X_2 \end{align}\]

參考高翔博士的《SLAM十四講》 4.3.5 的 \(\partial (Tp) \over \partial \xi\)。

可知

\[\begin{align}{\partial X_2 \over \partial \xi_{21}} &= \begin{bmatrix} {\partial X_2 \over \partial \xi_{21}} \\ {\partial Y_2 \over \partial \xi_{21}} \\ {\partial Z_2 \over \partial \xi_{21}}\end{bmatrix} \notag \\ &= \begin{bmatrix} 1 & 0 & 0 & 0 & Z_2 & - Y_2 \\ 0 & 1 & 0 & -Z_2 & 0 & X_2 \\ 0 & 0 & 1 & Y_2 & -X_2 & 0\end{bmatrix} \end{align}\]

\[\begin{align} {\partial x^{\prime}_{2} \over \partial \xi_{21}} = \begin{bmatrix} {\partial \rho_2 \over \partial \xi_{21}} X_2 \\ {\partial \rho_2 \over \partial \xi_{21}} Y_2 \\ {\partial \rho_2 \over \partial \xi_{21}} Z_2 \end{bmatrix} + \rho_2 {\partial X_2 \over \partial \xi_{21}} \end{align}\]

\[\begin{align} {\partial \rho_2 \over \partial \xi_{21}} = {\partial {1 \over Z_2}\over \partial \xi_{21}} = {-1 \over Z_2^2} \begin{bmatrix} 0 & 0 & 1 & Y_2 & -X_2 & 0\end{bmatrix} \end{align}\]

\[\begin{align} {\partial x^{\prime}_2 \over \partial \xi_{21}} &= \begin{bmatrix} 0 & 0 & -{X_2 \over Z_2^2} & - {X_2 Y_2\over Z_2^2} & {X_2^2 \over Z_2^2} & 0 \\ 0 & 0 & -{Y_2 \over Z_2^2} & -{Y_2^2 \over Z_2^2} & {X_2 Y_2 \over Z_2^2} & 0 \\ 0 & 0 & -{Z_2 \over Z_2^2} & -{Y_2 Z_2\over Z_2^2} & {X_2 Z_2 \over Z_2^2} & 0\end{bmatrix} \notag \\ & \ \ + \begin{bmatrix} {1 \over Z_2} & 0 & 0 & 0 & 1 & -{Y_2 \over Z_2} \\ 0 & {1 \over Z_2} & 0 & -1 & 0 & {X_2 \over Z_2} \\ 0 & 0 & {1 \over Z_2} & {Y_2 \over Z_2} & -{X_2 \over Z_2} & 0\end{bmatrix} \notag \\ &= \begin{bmatrix} {1 \over Z_2} & 0 & -{X_2 \over Z_2^2} & -{X_2 Y_2 \over Z_2^2} & 1 + {X_2^2 \over Z_2^2} & -{Y_2 \over Z_2} \\ 0 & {1 \over Z_2} & -{Y_2 \over Z_2^2} & -(1 + {Y_2^2 \over Z_2^2}) & {X_2 Y_2 \over Z_2^2} & {X_2 \over Z_2} \\ 0 & 0 & 0 & 0 & 0 & 0\end{bmatrix} \notag \\ &= \begin{bmatrix} \rho_2 & 0 & -\rho_2 u^{\prime}_2 & -u^{\prime}_2 v^{\prime}_2 & 1 + u^{\prime 2}_2 & -v^{\prime}_2 \\ 0 & \rho_2 & -\rho_2 v^{\prime}_2 & -(1 + v^{\prime 2}_2) & u^{\prime}_2 v^{\prime}_2 & u^{\prime}_2 \\ 0 & 0 & 0 & 0 & 0 & 0\end{bmatrix} \end{align}\]

最后一步是將空間坐標系坐標 \(X\) 用歸一化坐標系坐標 \(u^{\prime}_2\) 和逆深度 \(\rho_2\) 替換。

3.2.3 替換求得 \({\partial x_{2} \over \partial \xi_{21}}\) 和 \({\partial x_{2} \over \partial \rho_1}\)

\[\begin{align} {\partial x_{2} \over \partial \rho_1} &= {\partial x_{2}\over \partial x^{\prime}_{2}} {\partial x^{\prime}_{2} \over \partial \rho_1} \notag \\ &= \begin{bmatrix} f_x & 0 & 0 \\ 0 & f_y & 0 \\ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} \rho_1^{-1}\rho_2(t_{21}^x - u^{\prime}_2t_{21}^z) \\ \rho_1^{-1}\rho_2(t_{21}^y - v^{\prime}_2t_{21}^z) \\ 0\end{bmatrix} \notag \\ &= \begin{bmatrix} f_x \rho_1^{-1}\rho_2(t_{21}^x - u^{\prime}_2t_{21}^z) \\ f_y \rho_1^{-1}\rho_2(t_{21}^y - v^{\prime}_2t_{21}^z) \\ 0\end{bmatrix} \end{align}\]

\[\begin{align} {\partial x_{2} \over \partial \xi_{21}} &= {\partial x_{2}\over \partial x^{\prime}_{2}} {\partial x^{\prime}_{2} \over \partial \xi_{21}} \notag \\ &= \begin{bmatrix} f_x & 0 & 0 \\ 0 & f_y & 0 \\ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} \rho_2 & 0 & -\rho_2 u^{\prime}_2 & -u^{\prime}_2 v^{\prime}_2 & 1 + u^{\prime 2}_2 & -v^{\prime}_2 \\ 0 & \rho_2 & -\rho_2 v^{\prime}_2 & -(1 + v^{\prime 2}_2) & u^{\prime}_2 v^{\prime}_2 & u^{\prime}_2 \\ 0 & 0 & 0 & 0 & 0 & 0\end{bmatrix} \notag \\ &= \begin{bmatrix} f_x \rho_2 & 0 & -f_x \rho_2 u^{\prime}_2 & -f_xu^{\prime}_2 v^{\prime}_2 & f_x(1 + u^{\prime 2}_2) & -f_x v^{\prime}_2 \\ 0 & f_y\rho_2 & -f_y \rho_2 v^{\prime}_2 & -f_y(1 + v^{\prime 2}_2) & f_y u^{\prime}_2 v^{\prime}_2 & f_y u^{\prime}_2 \\ 0 & 0 & 0 & 0 & 0 & 0\end{bmatrix} \end{align}\]

3.2.4 最終得到 \({\partial r_{21} \over \partial \xi_{21}}\) 和 \({\partial r_{21} \over \partial \rho_{1}}\)

\[\begin{align} {\partial r_{21} \over \partial \rho_1} &= {\partial r_{21} \over \partial x_{2}} {\partial x_{2} \over \partial \rho_1} \notag \\ &= w_h {\partial I_2[x_2] \over \partial x_2} {\partial x_2 \over \partial \rho_1} \notag \\ &= w_h \begin{bmatrix} g_x & g_y & 0\end{bmatrix} \begin{bmatrix} f_x \rho_1^{-1}\rho_2(t_{21}^x - u^{\prime}_2t_{21}^z) \\ f_y \rho_1^{-1}\rho_2(t_{21}^y - v^{\prime}_2t_{21}^z) \\ 0\end{bmatrix} \notag \\ &= w_h(g_x f_x \rho_1^{-1}\rho_2(t_{21}^x - u^{\prime}_2t_{21}^z) + g_y f_y \rho_1^{-1}\rho_2(t_{21}^y - v^{\prime}_2t_{21}^z)) \end{align}\]

\[\begin{align} {\partial r_{21} \over \partial \xi_{21}} &= {\partial r_{21} \over \partial x_{2}} {\partial x_{2} \over \partial \xi_{21}} \notag \\ &= w_h {\partial I_2[x_2] \over \partial x_2} {\partial x_2 \over \partial \xi_{21}} \notag \\ &= w_h \begin{bmatrix} g_x & g_y & 0\end{bmatrix} \notag \\ & \ \ \begin{bmatrix} f_x \rho_2 & 0 & -f_x \rho_2 u^{\prime}_2 & -f_x u^{\prime}_2 v^{\prime}_2 & f_x(1 + u^{\prime 2}_2) & -f_x v^{\prime}_2 \\ 0 & f_y \rho_2 & -f_y \rho_2 v^{\prime}_2 & -f_y(1 + v^{\prime 2}_2) & f_y u^{\prime}_2 v^{\prime}_2 & f_y u^{\prime}_2 \\ 0 & 0 & 0 & 0 & 0 & 0\end{bmatrix}\notag \\ &= w_h \begin{bmatrix} g_x f_x \rho_2 \\ g_y f_y \rho_2 \\ -g_x f_x \rho_2 u^{\prime}_2 - g_y f_y \rho_2 v^{\prime}_2 \\ -g_x f_x u^{\prime}_2 v^{\prime}_2 -g_y f_y(1 + v^{\prime 2}_2) \\ g_x f_x(1 + u^{\prime 2}_2) + g_y f_y u^{\prime}_2 v^{\prime}_2 \\ -g_x f_x v^{\prime}_2 + g_y f_y u^{\prime}_2\end{bmatrix}^T \end{align}\]

4 代碼中變量的含義

函數 Vec3f CoarseInitializer::calcResAndGS(...)。

dp0, dp1, dp2, dp3, dp4, dp5 是光度誤差對 se(3) 六個量的導數，即 \({\partial r_{21} \over \partial \xi_{2}}\)。

dp6, dp7 是光度誤差對輻射仿射變換的參數的導數，即 \({\partial r_{21} \over \partial a_{2}}\) 和 \({\partial r_{21} \over \partial b_{2}}\)。

dd 是光度誤差對逆深度的導數，即 \({\partial r_{21} \over \partial \rho_1}\)。

轉載于:https://www.cnblogs.com/JingeTU/p/8203606.html

總結

以上是生活随笔為你收集整理的直接法光度误差导数推导的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。