直接法光度误差导数推导

Posted Jinge

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了直接法光度误差导数推导相关的知识,希望对你有一定的参考价值。

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

光度仿射变换

光度仿射变换是将两帧之间辐射值进行对应。对应的参数有两个[a, b]。

将图片 1 的辐射值变换到影像 2 中:

\[I_2 = e^{a_{21}} I_1 + b_{21}\]

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

光度误差

光度误差可以用以下公式计算:

\[r = w_h (I_2[x_2] - (e^{a_{21}}I_1[x_1] + b_{21}))\]

其中 \(w_h\) 是 Huber 权重,\(I_1, I_2\) 分别是影像1、2,\(x_1, x_2\) 分别是空间中一点 \(X\) 在影像上的像素坐标。

其中 \(x_2\) 可以写作

\(x_2 = f(x_1, \xi_{21}, \rho_1)\)

\(x_2\) 是由 \(x_1\) 投影而来,投影的过程中需要两帧之间的相对位姿 \(\xi_{21}\)\(x_1\) 在 1 中的逆深度 \(\rho_1\)

求导

从最简单的光度误差参数开始求导数:

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

随后求光度误差对相对位姿和逆深度的导数:

\[{\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}} \notag \\ {\partial r_{21} \over \partial \rho_{1}} &= {\partial r_{21} \over \partial x_{2}} {\partial x_{2} \over \partial \rho_{1}}\notag \end{align}\]

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

\[{\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}\]

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

现在,我们需要求的是 \({\partial x_{2} \over \partial \xi_{21}}\)\({\partial x_{2} \over \partial \rho_1}\)

分别列两张影像的投影方程如下:

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

整理一下

\[X_1 = \rho_1^{-1} K^{-1} x_1\]

\[\begin{align} x_2 &= K \rho_2 (R_{21} \rho_1^{-1} K^{-1} x_1 + t_{21}) \notag \\ &= K x^{\prime}_2\notag \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}} \notag \\ {\partial x_{2} \over \partial \rho_1} &= {\partial x_{2}\over \partial x^{\prime}_{2}} {\partial x^{\prime}_{2} \over \partial \rho_1} \notag \end{align}\]

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

\[\begin{align} x_2 &= K x^{\prime}_2 \notag \\ \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}\notag \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 & u_2 \\ 0 & f_y & v_2 \\ 0 & 0 & 0 \end{bmatrix}\notag \end{align}\]

随后分别计算 \({\partial x^{\prime}_{2} \over \partial \xi_{21}}\)\({\partial x^{\prime}_{2} \over \partial \rho_1}\)

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

\[{\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}\]

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

\(R_{21} K^{-1} x_1\) 看做以整体,矩阵 \(A\),矩阵 \(A\) 有三行。

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

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

\[\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}\]

可得 2 中的逆深度

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

\[\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}}\notag \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) \notag \end{align}\]

同理

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

结论

\[{\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}\]

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

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

参考高翔博士的《SLAM十四讲》 4.3.5 的 \(\partial (Tp) \over \partial \xi\)

可知

\[\begin{align}{\partial X_2 \over \partial \xi_{21}} &= \begin{bmatrix} {\partial X_2 \over \xi_{21}} \\ {\partial Y_2 \over \xi_{21}} \\ {\partial Z_2 \over \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} \notag \end{align}\]

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

\[ {\partial \rho_2 \over \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}\]

\[\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} \notag \end{align}\]

最后一步是将空间坐标系坐标 \(X\) 用归一化坐标系坐标 \(u^{\prime}_2\) 和逆深度 \(\rho_2\) 替换。

替换求得 \({\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 & u_2 \\ 0 & f_y & v_2 \\ 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} \notag \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 & u_2 \\ 0 & f_y & v_2 \\ 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} \notag \end{align}\]

最终得到 \({\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)) \notag \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 \notag \end{align}\]

代码中变量的含义

函数 Vec3f CoarseInitializer::calcResAndGS(...)

dp0, dp1, dp2, dp3, dp4, dp5 是光度误差对 se(3) 六个量的导数,即 \({\partial r_{21} \over \partial \xi_{21}}\)

dp6, dp7 是光度误差对辐射仿射变换的参数的导数,即 \({\partial r_{21} \over \partial a_{21}}\)\({\partial r_{21} \over \partial b_{21}}\)

dd 是光度误差对逆深度的导数,即 \({\partial r_{21} \over \partial \rho_1}\)

以上是关于直接法光度误差导数推导的主要内容,如果未能解决你的问题,请参考以下文章

[机器学习]—梯度下降法

甲醛的检测方法都有哪些

反向传播算法之要点(Backpropagation)

梯度提升分类树原理推导(超级详细!)

python: 深度学习-误差反向传播法

拉格朗日乘子法和KKT条件