DSO windowed optimization 代碼 (1)

這里不想解釋怎么 marginalize，什么是 First-Estimates Jacobian (FEJ)。這里只看看代碼，看看Hessian矩陣是怎么構造出來的。

1 優化流程

整個優化過程，也是 Levenberg–Marquardt 的優化過程，這個優化過程在函數 FullSystem::makeKeyFrame() 中被調用，也是在確定當前幀成為關鍵幀，並且用當前幀激活了窗口中其他幀的 immaturePoints 之后。過程在 FullSystem::optimize() 函數中。

優化的目標值，是所有需要優化點的逆深度、相機的4個內參數、窗口中8個幀的狀態量。

FullSystem::optimize() 函數流程大致如下。首先 FullSystem::linearizeAll(false) 把相關的導數計算一下。然后在所有的 residual 中找到 isLinearized 為 false 的 residual，調用 PointFrameResidual::applyRes(true)，設置它們的 PointFrameResidual::ResState （按照 FullSystem::optimizeImmaturePoint 的結果），如果是正常的點(ResState::IN)調用EFResidual::takeDataF()把 EFResidual::JpJdF 設置一下。這就算完成了優化的准備工作。

隨后進入循環體，進行最高有限次的優化循環。每一次優化都有可能使得整體能量升高，所以每次優化前調用 FullSystem::backupState() 保存當前所有待優化參數的 state 和 step，FullSystem::solveSystem() 進行優化（得到的優化變化量 step），FullSystem::doStepFromBackup() 將優化結果生效，FullSystem::linearizeAll(false) 計算新的能量值，如果能量升高了，就使用 FullSystem::loadSateBackup() 將結果回滾。

在跳出循環體之后調用一次 FullSystem::linearizeAll(true)，效果是將優化之后成為 outlier 的 residual 剔除，剩下正常的 residual 調用一次 EFResidual::takeDataF() 計算新的 EFResidual::JpJdF （這里涉及到 FEJ）。注意一下 FullSystem::linearizeAll() 參數為 false 和 true 的區別。

FullSystem::solveSystem() 設置優化變化量 step 的過程在 EnergyFunctional::resubstituteF_MT() 中，能幫助理解 idepth 是如何更新的（Schur Complement 將 idepth 剔除出最終需要矩陣求逆的系統，然而這些 idepth 的優化變化量 step 是如何計算的？）。

2 導數准備

需要遍歷每一個 PointFrameResidual 將與這個 Residual 相關的導數計算出來，再進行優化。而這些計算出來的相關導數被存儲在 RawResidualJacobian 中。

https://github.com/JakobEngel/dso/blob/master/src/OptimizationBackend/RawResidualJacobian.h#L32

在優化過程中 Frame, PointHessian, PointFrameResidual 都有與之對應的實體，分別是 EFFrame, EFPoint, EFResidual。通過保留指針，保存層次之間的索引。

在計算 RawResidualJacobian 時，是計算 PointFrameResidual 的 J，爾后會將這個 J 轉移到 EFResidual 的 J，並且計算 EFResidual::JpJdF，這個過程在 EFResidual::takeDataF() 中。所以這里把 JpJdF 的計算過程寫出來，弄清 JpJdF 的意義。

https://github.com/JakobEngel/dso/blob/5fb2c065b1638e10bccf049a6575ede4334ba673/src/OptimizationBackend/EnergyFunctionalStructs.cpp#L37

struct RawResidualJacobian
{
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
	// ================== new structure: save independently =============.
	VecNRf resF; // typdef Eigen::Matrix<float,MAX_RES_PER_POINT,1> VecNRf; MAX_RES_PER_POINT == 8

	// the two rows of d[x,y]/d[xi].
	Vec6f Jpdxi[2];			// 2x6

	// the two rows of d[x,y]/d[C].
	VecCf Jpdc[2];			// 2x4

	// the two rows of d[x,y]/d[idepth].
	Vec2f Jpdd;				// 2x1

	// the two columns of d[r]/d[x,y].
	VecNRf JIdx[2];			// 9x2

	// = the two columns of d[r] / d[ab]
	VecNRf JabF[2];			// 9x2


	// = JIdx^T * JIdx (inner product). Only as a shorthand.
	Mat22f JIdx2;				// 2x2
	// = Jab^T * JIdx (inner product). Only as a shorthand.
	Mat22f JabJIdx;			// 2x2
	// = Jab^T * Jab (inner product). Only as a shorthand.
	Mat22f Jab2;			// 2x2

};

以上變量的類型中出現NR，說明該變量是存儲了每一個 pattern 點的信息。

現在將這些變量對應的導數一一列出：

1. VecNRf resF;對應\(r_{21}\)，1x8，這里的\(r_{21}\)是對於一個點，八個 pattern residual 組成的向量。
2. Vec6f Jpdxi[2];對應\(\partial x_2 \over \partial \xi_{21}\)，2x6，注意這里的\(x_2\)是像素坐標。（我一般把像素坐標寫成\(x\)，對應代碼中的變量Ku，歸一化坐標寫成\(x^{\prime}\)，對應代碼中的變量u。）
3. VecCf Jpdc[2];對應\(\partial x_2 \over \partial C\)，2x4，這里的\(C\)指相機內參\(\begin{bmatrix} f_x, f_y, c_x, c_y\end{bmatrix}^T\)。
4. Vec2f Jpdd;對應\(\partial x_2 \over \partial \rho_1\)，2x1，注意是對 host 幀的逆深度求導。
5. VecNRf JIdx[2];對應\(\partial r_{21} \over \partial x_2\)，8x2，這個和 target 幀上的影像梯度相關。
6. VecNRf JabF[2];對應\({\partial r_{21} \over \partial a_{21}}, {\partial r_{21} \over \partial b_{21}}\)，8x1，8x1。
7. Mat22f JIdx2;對應\({\partial r_{21} \over \partial x_2}^T{\partial r_{21} \over \partial x_2}\)，2x8 8x2，2x2。
8. Mat22f JabJIdx;對應\({\partial r_{21} \over \partial l_{21}}^T{\partial r_{21} \over \partial x_2}\)，2x8 8x2，2x2，這里的\(l_{21}\)指\(\begin{bmatrix} a_{21} \\ b_{21}\end{bmatrix}\)。
9. Mat22f Jab2;對應\({\partial r_{21} \over \partial l_{21}}^T{\partial r_{21} \over \partial l_{21}}\)，2x8 8x2，2x2。
10. JpJdF對應\(\begin{bmatrix} {\partial r_{21} \over \partial \xi_{21}}^T{\partial r_{21} \over \partial \rho_1} \\ {\partial r_{21} \over \partial l_{21}}^T{\partial r_{21} \over \partial \rho_1}\end{bmatrix}\)，8x1。

在 PointFrameResidual::linearize 中對這些變量進行了計算。

https://github.com/JakobEngel/dso/blob/master/src/FullSystem/Residuals.cpp#L78

在計算時使用了投影過程中的變量，現在將這些變量與公式對應。投影過程標准公式如下：

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

變量的對應關系如下：

KliP = \(K^{-1}x_1\) = \(x_1^{\prime}\)

ptp = \(R_{21}K^{-1}x_1 + \rho_1 t_{21}\) = \(\rho_2^{-1}\rho_1K^{-1}x_2\)

drescale = \(\rho_2 \rho_1^{-1}\)

[u, v, 1]^T = \(K^{-1}x_2\) = \(x_2^{\prime}\)

[Ku, Kv, 1]^T = \(x_2\)

1. Vec2f Jpdd;\(\partial x_2 \over \partial \rho_1\)

d_d_x = drescale * (PRE_tTll_0[0]-PRE_tTll_0[2]*u)*SCALE_IDEPTH*HCalib->fxl();
d_d_y = drescale * (PRE_tTll_0[1]-PRE_tTll_0[2]*v)*SCALE_IDEPTH*HCalib->fyl();

計算\(\partial x_2 \over \partial \rho_1\)，這個在博客《直接法光度誤差導數推導》中已經講了如何求解。得到的結果是：

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

2.VecCf Jpdc[2];\(\partial x_2 \over \partial C\)

d_C_x[2] = drescale*(PRE_RTll_0(2,0)*u-PRE_RTll_0(0,0));
d_C_x[3] = HCalib->fxl() * drescale*(PRE_RTll_0(2,1)*u-PRE_RTll_0(0,1)) * HCalib->fyli();
d_C_x[0] = KliP[0]*d_C_x[2];
d_C_x[1] = KliP[1]*d_C_x[3];

d_C_y[2] = HCalib->fyl() * drescale*(PRE_RTll_0(2,0)*v-PRE_RTll_0(1,0)) * HCalib->fxli();
d_C_y[3] = drescale*(PRE_RTll_0(2,1)*v-PRE_RTll_0(1,1));
d_C_y[0] = KliP[0]*d_C_y[2];
d_C_y[1] = KliP[1]*d_C_y[3];

d_C_x[0] = (d_C_x[0]+u)*SCALE_F;
d_C_x[1] *= SCALE_F;
d_C_x[2] = (d_C_x[2]+1)*SCALE_C;
d_C_x[3] *= SCALE_C;

d_C_y[0] *= SCALE_F;
d_C_y[1] = (d_C_y[1]+v)*SCALE_F;
d_C_y[2] *= SCALE_C;
d_C_y[3] = (d_C_y[3]+1)*SCALE_C;

鏈式的求導過程。

\[{\partial x_2 \over \partial C} = \begin{bmatrix} {\partial u_2 \over \partial f_x} & {\partial u_2 \over \partial f_y} & {\partial u_2 \over \partial c_x} & {\partial u_2 \over \partial c_y} \\ {\partial v_2 \over \partial f_x} & {\partial v_2 \over \partial f_y} & {\partial v_2 \over \partial c_x} & {\partial v_2 \over \partial c_y} \end{bmatrix} \]

\[\begin{align}x_2 &= Kx_2^{\prime} \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_2^{\prime} \\ v_2^{\prime} \\ 1\end{bmatrix} \notag \end{align}\]

\[\begin{align} u_2 &= f_x u_2^{\prime} + c_x \notag \\ v_2 &= f_y v_2^{\prime} + c_y \notag \end{align}\]

\[\begin{align} {\partial u_2 \over \partial f_x} &= u_2^{\prime} + f_x {\partial u_2^{\prime} \over \partial f_x} \notag & {\partial u_2 \over \partial f_y} &= f_x {\partial u_2^{\prime} \over \partial f_y} \notag \\ {\partial u_2 \over \partial c_x} &= f_x {\partial u_2^{\prime} \over \partial c_x} + 1 \notag & {\partial u_2 \over \partial c_y} &= f_x {\partial u_2^{\prime} \over \partial c_y} \notag \end{align}\]

\[\begin{align} {\partial v_2 \over \partial f_x} &= f_y {\partial v_2^{\prime} \over \partial f_x} \notag & {\partial v_2 \over \partial f_y} &= v_2^{\prime} + f_y {\partial v_2^{\prime} \over \partial f_y} \notag \\ {\partial v_2 \over \partial c_x} &= f_y {\partial v_2^{\prime} \over \partial c_x} \notag & {\partial v_2 \over \partial c_y} &= f_y {\partial v_2^{\prime} \over \partial c_y} + 1 \notag \end{align}\]

先求\({\partial x_2^{\prime} \over \partial C}\)，再使用鏈式法則求\({\partial x_2 \over \partial C}\)。

\[\begin{align} x_2^{\prime} &= \rho_2 \rho_1^{-1}(R_{21}K^{-1}x_1+\rho_1t_{21}) \notag \\ &= \rho_2 \rho_1^{-1} R_{21}K^{-1}x_1 + \rho_2 t_{21} \notag \\ &= \rho_2 \rho_1^{-1} \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33}\end{bmatrix} \begin{bmatrix} f_x^{-1} & 0 & -f_x^{-1}c_x \\ 0 & f_y^{-1} & -f_y^{-1}c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} u_1 \\ v_1 \\ 1 \end{bmatrix} + \rho_2 t_{21} \notag \\ &= \rho_2 \rho_1^{-1} \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33}\end{bmatrix} \begin{bmatrix} f_x^{-1}(u_1 - c_x) \\ f_y^{-1}(v_1 - c_y) \\ 1 \end{bmatrix} + \rho_2 \begin{bmatrix} t_{21}^x \\ t_{21}^y \\ t_{21}^z \end{bmatrix} \notag \end{align}\]

\[\begin{align} u_2^{\prime} &= \frac{\rho_2 \rho_1^{-1}(r_{11}f_x^{-1}(u_1 - c_x) + r_{12}f_y^{-1}(v_1-c_y) + r_{13}) + \rho_2 t_{21}^x}{\rho_2 \rho_1^{-1}(r_{31}f_x^{-1}(u_1 - c_x) + r_{32}f_y^{-1}(v_1-c_y) + r_{33}) + \rho_2 t_{21}^z} = \frac{A}{C} \notag \\ v_2^{\prime} &= \frac{\rho_2 \rho_1^{-1}(r_{21}f_x^{-1}(u_1 - c_x) + r_{22}f_y^{-1}(v_1-c_y) + r_{23}) + \rho_2 t_{21}^y}{\rho_2 \rho_1^{-1}(r_{31}f_x^{-1}(u_1 - c_x) + r_{32}f_y^{-1}(v_1-c_y) + r_{33}) + \rho_2 t_{21}^z} = \frac{B}{C}\notag \end{align}\]

\(u_2^{\prime}, v_2^{\prime}\) 的分母的計算結果都為 1，但是這個計算過程和內參 \(f_x, f_y, c_x, c_y\) 都有關系。求導過程不能省略分母，感謝某同學指出我這里認知上的錯誤。（為方便表達，我用字母替代分子、分母，\(C=1\)。）

求導示例：

\[\begin{align} {\partial u_2^{\prime} \over \partial f_x} &= \frac{\partial A}{\partial f_x} \frac{1}{C} + A\frac{1}{C^2}(-1)\frac{\partial C}{\partial f_x} \notag \\ &= \rho_2 \rho_1^{-1}r_{11}(u_1-c_x)f_x^{-2}(-1)\frac{1}{C} - \frac{A}{C}\frac{1}{C}\rho_2 \rho_1^{-1}r_{31}(u_1-c_x)f_x^{-2}(-1) \notag \\ &= \frac{1}{C}(\rho_2 \rho_1^{-1}r_{11}(u_1-c_x)f_x^{-2}(-1) + \rho_2 \rho_1^{-1}r_{31}(u_1-c_x)f_x^{-2}u_2^{\prime}) \notag \\ &= \rho_2 \rho_1^{-1} (r_{31}u_2^{\prime}-r_{11})f_x^{-2}(u_1 - c_x) \notag \end{align}\]

同理得到以下結果：

\[\begin{align} {\partial u_2^{\prime} \over \partial f_x} &= \rho_2 \rho_1^{-1} (r_{31}u_2^{\prime}-r_{11})f_x^{-2}(u_1 - c_x) \notag & {\partial u_2^{\prime} \over \partial f_y} &= \rho_2 \rho_1^{-1}(r_{32}u_2^{\prime}-r_{12})f_y^{-2}(v_1 - c_y) \notag \\ {\partial u_2^{\prime} \over \partial c_x} &= \rho_2 \rho_1^{-1}(r_{31}u_2^{\prime}-r_{11})f_x^{-1} \notag & {\partial u_2^{\prime} \over \partial c_y} &= \rho_2 \rho_1^{-1}(r_{32}u_2^{\prime}-r_{12}) f_y^{-1} \notag \end{align}\]

\[\begin{align} {\partial v_2^{\prime} \over \partial f_x} &= \rho_2 \rho_1^{-1} (r_{31}v_2^{\prime}-r_{21})f_x^{-2}(u_1 - c_x) \notag & {\partial v_2^{\prime} \over \partial f_y} &= \rho_2 \rho_1^{-1}(r_{32}v_2^{\prime}-r_{22})f_y^{-2}(v_1 - c_y) \notag \\ {\partial v_2^{\prime} \over \partial c_x} &= \rho_2 \rho_1^{-1}(r_{31}v_2^{\prime}-r_{21})f_x^{-1} \notag & {\partial v_2^{\prime} \over \partial c_y} &= \rho_2 \rho_1^{-1}(r_{32}v_2^{\prime}-r_{22}) f_y^{-1} \notag \end{align}\]

鏈式：

\[\begin{align} {\partial u_2 \over \partial f_x} &= u_2^{\prime} + f_x {\partial u_2^{\prime} \over \partial f_x} \notag \\ &= u_2^{\prime} + \rho_2 \rho_1^{-1} (r_{31}u_2^{\prime}-r_{11})f_x^{-1}(u_1 - c_x) \notag \\ {\partial u_2 \over \partial f_y} &= f_x {\partial u_2^{\prime} \over \partial f_y} \notag \\ &= f_x f_y^{-1} \rho_2 \rho_1^{-1}(r_{32} u_2^{\prime}-r_{12})f_y^{-1}(v_1 - c_y) \notag \\ {\partial u_2 \over \partial c_x} &= f_x {\partial u_2^{\prime} \over \partial c_x} + 1 \notag \\ &= \rho_2 \rho_1^{-1}(r_{31}u_2^{\prime}-r_{11}) + 1\notag \\ {\partial u_2 \over \partial c_y} &= f_x {\partial u_2^{\prime} \over \partial c_y} \notag \\ &= f_x f_y^{-1} \rho_2 \rho_1^{-1}(r_{32}u_2^{\prime}-r_{12}) \notag \end{align}\]

\[\begin{align} {\partial v_2 \over \partial f_x} &= f_y {\partial v_2^{\prime} \over \partial f_x} \notag \\ &= f_y f_x^{-1} \rho_2 \rho_1^{-1} (r_{31} v_2^{\prime}-r_{21})f_x^{-1}(u_1 - c_x) \notag \\ {\partial v_2 \over \partial f_y} &= v_2^{\prime} + f_y {\partial v_2^{\prime} \over \partial f_y} \notag \\ &= v_2^{\prime} + \rho_2 \rho_1^{-1}(r_{32} v_2^{\prime}-r_{22})f_y^{-1}(v_1 - c_y) \notag \\ {\partial v_2 \over \partial c_x} &= f_y {\partial v_2^{\prime} \over \partial c_x} \notag \\ &= f_y f_x^{-1} \rho_2 \rho_1^{-1}(r_{31} v_2^{\prime}-r_{21}) \notag \\ {\partial v_2 \over \partial c_y} &= f_y {\partial v_2^{\prime} \over \partial c_y} + 1 \notag \\ &= \rho_2 \rho_1^{-1}(r_{32} v_2^{\prime}-r_{22}) + 1 \notag \end{align}\]

3. Vec6f Jpdxi[2];\(\partial x_2 \over \partial \xi_{21}\)

d_xi_x[0] = new_idepth*HCalib->fxl();
d_xi_x[1] = 0;
d_xi_x[2] = -new_idepth*u*HCalib->fxl();
d_xi_x[3] = -u*v*HCalib->fxl();
d_xi_x[4] = (1+u*u)*HCalib->fxl();
d_xi_x[5] = -v*HCalib->fxl();

d_xi_y[0] = 0;
d_xi_y[1] = new_idepth*HCalib->fyl();
d_xi_y[2] = -new_idepth*v*HCalib->fyl();
d_xi_y[3] = -(1+v*v)*HCalib->fyl();
d_xi_y[4] = u*v*HCalib->fyl();
d_xi_y[5] = u*HCalib->fyl();

計算\(\partial x_2 \over \partial \xi_{21}\)，這個在博客《直接法光度誤差導數推導》中已經講了如何求解。得到的結果是：

\[{\partial x_2 \over \partial \xi_{21}} = \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} \]

4. VecNRf JIdx[2];對應 \(\partial r_{21} \over \partial x_2\)

J->JIdx[0][idx] = hitColor[1];
J->JIdx[1][idx] = hitColor[2];

計算\(\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} \]

注意代碼中這個變量是8維。

5. VecNRf JabF[2];\({\partial r_{21} \over \partial a_{21}}, {\partial r_{21} \over \partial b_{21}}\)

float drdA = (color[idx]-b0);
...
J->JabF[0][idx] = drdA*hw;
J->JabF[1][idx] = hw;

計算\({\partial r_{21} \over \partial l_{21}}\)，這個在博客《直接法光度誤差導數推導》中已經講了如何求解。得到的結果是（結果有點不符合，需要再對照一下）：

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

10. JpJdF對應 \(\begin{bmatrix} {\partial r_{21} \over \partial \xi_{21}}^T{\partial r_{21} \over \partial \rho_1} \\ {\partial r_{21} \over \partial l_{21}}^T{\partial r_{21} \over \partial \rho_1}\end{bmatrix}\)

void EFResidual::takeDataF()
{
	std::swap<RawResidualJacobian*>(J, data->J);

	Vec2f JI_JI_Jd = J->JIdx2 * J->Jpdd;

	for(int i=0;i<6;i++)
		JpJdF[i] = J->Jpdxi[0][i]*JI_JI_Jd[0] + J->Jpdxi[1][i] * JI_JI_Jd[1];

	JpJdF.segment<2>(6) = J->JabJIdx*J->Jpdd;
}

1. JI_JI_Jd對應\({\partial r_{21} \over \partial x_2}^T{\partial r_{21} \over \partial \rho_1}\)，2x8 8x1，2x1。
2. JpJdF.segment<6>(0)對應\({\partial x_2 \over \partial \xi_{21}}^T{\partial r_{21} \over \partial x_2}^T{\partial r_{21} \over \partial \rho_1} = {\partial r_{21} \over \partial \xi_{21}}^T{\partial r_{21} \over \partial \rho_1}\)，6x2 2x8 8x1，6x1。
3. JpJdF.segment<2>(6)對應\({\partial r_{21} \over \partial l_{21}}^T{\partial r_{21} \over \partial x_2}{\partial x_2 \over \partial \rho_1} = {\partial r_{21} \over \partial l_{21}}^T{\partial r_{21} \over \partial \rho_1}\)，2x8 8x2 2x1，2x1。
4. JpJdF對應\(\begin{bmatrix} {\partial r_{21} \over \partial \xi_{21}}^T{\partial r_{21} \over \partial \rho_1} \\ {\partial r_{21} \over \partial l_{21}}^T{\partial r_{21} \over \partial \rho_1}\end{bmatrix}\)，8x1。