直接法光度误差导数推导

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 相关的文章对导数的定义如下：

[egin{align} frac{partial mathbf{f}}{partial mathbf{x}} = egin{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 光度仿射变换

光度仿射变换是将两帧之间辐射值进行对应。对应的参数有两个 (egin{bmatrix} a, b end{bmatrix})。

将影像 1 的辐射值变换到影像 2 中：

[egin{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) 是曝光时间）：

[egin{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 的光度变换参数为 (egin{bmatrix} 0, 0 end{bmatrix})。所以相对光度仿射变换参数可以退化成下面的形式：

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

2 光度误差

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

[egin{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) 可以写作

[egin{align} x_2 = f(x_1, xi_{21}, ho_1) end{align} ]

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

3 导数

3.1 光度仿射变换导数 ({partial r_{21} over partial a_{21}}, {partial r_{21} over partial b_{21}})

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

[egin{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 代码，应该如此：

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

我怀疑是不是我看错了，其实 DSO 优化 (a_2)，这样就有

[egin{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 ho_{1}})

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

[{partial r_{21} over partial xi_{21}}, {partial r_{21} over partial ho_{1}} ]

通过链式法则：

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

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

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

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

现在，我们需要求的是 ({partial x_{2} over partial xi_{21}}) 和 ({partial x_{2} over partial ho_1})。

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

[egin{align} egin{cases} ho_1^{-1} x_1 &= K X_1 \ ho_2^{-1} x_2 &= K (R_{21} X_1 + t_{21}) end{cases} end{align}]

整理一下

[egin{align} X_1 &= ho_1^{-1} K^{-1} x_1 \ x_2 &= K ho_2 (R_{21} ho_1^{-1} K^{-1} x_1 + t_{21}) otag \ &= K x^{prime}_2 end{align}]

上式中的 (x^{prime}_2) 就是归一化平面坐标，可以写作

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

通过链式法则，整理一下，我们需要计算的({partial x_{2} over partial xi_{21}}) 和 ({partial x_{2} over partial ho_1})：

[egin{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 ho_1} &= {partial x_{2}over partial x^{prime}_{2}} {partial x^{prime}_{2} over partial ho_1} end{align}]

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

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

于是有

[egin{align}{partial x_{2}over partial x^{prime}_{2}} &= egin{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 1over partial u^{prime}_{2}} & {partial 1over partial v^{prime}_{2}} & {partial 1over partial 1}end{bmatrix} otag \ &= egin{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 ho_1}) 。

3.2.1 推导 ({partial x^{prime}_{2} over partial ho_1})

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

[egin{align} x^{prime}_2 = ho_2 (R_{21} ho_1^{-1} K^{-1} x_1 + t_{21}) end{align} ]

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

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

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

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

可得 2 中的逆深度

[egin{align} ho_2 = ( ho_1^{-1} a_3^Tx_1 + t_{21}^z)^{-1} end{align} ]

代入

[egin{align} u^{prime}_2 &= ho_2( ho_1^{-1}a_1^Tx_1 + t_{21}^x) otag \ &= {{ ho_1^{-1}a_1^Tx_1 + t_{21}^x} over { ho_1^{-1} a_3^Tx_1 + t_{21}^z}} otag \ &= {{a_1^Tx_1 + ho_1 t_{21}^x} over {a_3^Tx_1 + ho_1 t_{21}^z}} end{align}]

[egin{align} {partial u^{prime}_{2} over partial ho_1} &= t_{21}^x {1 over {a_3^Tx_1 + ho_1 t_{21}^z}} otag \ & + (a_1^Tx_1 + ho_1 t_{21}^x) {1 over {(a_3^Tx_1 + ho_1 t_{21}^z)^2}} (-1) t_{21}^z otag \ &= ho_1^{-1} {1 over { ho_1^{-1}a_3^Tx_1 + t_{21}^z}}(t_{21}^x - {{a_1^Tx_1 + ho_1 t_{21}^x} over {a_3^Tx_1 + ho_1 t_{21}^z}}t_{21}^z) otag \ &= ho_1^{-1} ho_2(t_{21}^x - u^{prime}_2t_{21}^z) end{align}]

同理

[egin{align} {partial v^{prime}_{2} over partial ho_1} = ho_1^{-1} ho_2(t_{21}^y - v^{prime}_2t_{21}^z) end{align} ]

结论

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

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

[egin{align} x^{prime}_2 = ho_2(R_{21}X_1 + t_{21}) = ho_2 X_2 end{align} ]

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

可知

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

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

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

[egin{align} {partial x^{prime}_2 over partial xi_{21}} &= egin{bmatrix} 0 & 0 & -{X_2 over Z_2^2} & - {X_2 Y_2over 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_2over Z_2^2} & {X_2 Z_2 over Z_2^2} & 0end{bmatrix} otag \ & + egin{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} & 0end{bmatrix} otag \ &= egin{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 & 0end{bmatrix} otag \ &= egin{bmatrix} ho_2 & 0 & - ho_2 u^{prime}_2 & -u^{prime}_2 v^{prime}_2 & 1 + u^{prime 2}_2 & -v^{prime}_2 \ 0 & ho_2 & - ho_2 v^{prime}_2 & -(1 + v^{prime 2}_2) & u^{prime}_2 v^{prime}_2 & u^{prime}_2 \ 0 & 0 & 0 & 0 & 0 & 0end{bmatrix} end{align}]

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

3.2.3 替换求得 ({partial x_{2} over partial xi_{21}}) 和 ({partial x_{2} over partial ho_1})

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

[egin{align} {partial x_{2} over partial xi_{21}} &= {partial x_{2}over partial x^{prime}_{2}} {partial x^{prime}_{2} over partial xi_{21}} otag \ &= egin{bmatrix} f_x & 0 & 0 \ 0 & f_y & 0 \ 0 & 0 & 0 end{bmatrix} egin{bmatrix} ho_2 & 0 & - ho_2 u^{prime}_2 & -u^{prime}_2 v^{prime}_2 & 1 + u^{prime 2}_2 & -v^{prime}_2 \ 0 & ho_2 & - ho_2 v^{prime}_2 & -(1 + v^{prime 2}_2) & u^{prime}_2 v^{prime}_2 & u^{prime}_2 \ 0 & 0 & 0 & 0 & 0 & 0end{bmatrix} otag \ &= egin{bmatrix} f_x ho_2 & 0 & -f_x ho_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 ho_2 & -f_y ho_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 & 0end{bmatrix} end{align}]

3.2.4 最终得到 ({partial r_{21} over partial xi_{21}}) 和 ({partial r_{21} over partial ho_{1}})

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

[egin{align} {partial r_{21} over partial xi_{21}} &= {partial r_{21} over partial x_{2}} {partial x_{2} over partial xi_{21}} otag \ &= w_h {partial I_2[x_2] over partial x_2} {partial x_2 over partial xi_{21}} otag \ &= w_h egin{bmatrix} g_x & g_y & 0end{bmatrix} otag \ & egin{bmatrix} f_x ho_2 & 0 & -f_x ho_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 ho_2 & -f_y ho_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 & 0end{bmatrix} otag \ &= w_h egin{bmatrix} g_x f_x ho_2 \ g_y f_y ho_2 \ -g_x f_x ho_2 u^{prime}_2 - g_y f_y ho_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}_2end{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 ho_1})。