DSO Windowed Optimization Code

Here do not want to explain how marginalize, what is first-estimate Jacobian. Just look at the code here to see how the Hessian matrix is structured.

The derivative used in the Hessian matrix used to store the window optimization process in the code is stored in the structural body Rawresidualjacobian.

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

structrawresidualjacobian{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 both rows of d[x,y]/d[xi].vec6f jpdxi[2];//2x6    //The both rows of d[x,y]/d[c].VECCF jpdc[2];//2x4    //The both rows of d[x,y]/d[idepth].VEC2F JPDD;//2x1    //The columns of d[r]/d[x,y].VECNRF jidx[2];//9x2    //= the 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};

The type of the above variable appears NR , indicating that the variable is the information that stores each pattern point.

Now the derivative of these variables is listed as follows:

2. VecNRf resF;Correspondence \ (r_{21}\), 1x8, here \ (r_{21}\) is a vector of eight pattern residual for a point.
4. Vec6f Jpdxi[2];Correspondence \ (\partial x_2 \over \partial \xi_{21}\), 2x6, notice that \ (x_2\) here is pixel coordinates. (I generally write the pixel coordinates \ (x\), the corresponding code variables Ku , normalized coordinates written in \ (x^{\prime}\), corresponding to the variables in the code u . ）
6. VecCf Jpdc[2];Correspondence \ (\partial x_2 \over \partial c\), here \ (c\) refers to camera internal (\begin{bmatrix} f_x, F_y, c_x, c_y\end{ bmatrix}^t\).
8. Vec2f Jpdd;Correspondence \ (\partial x_2 \over \partial \rho_1\), 2x4, note that the inverse depth of the host frame is derivative.
10. VecNRf JIdx[2];Corresponds to \ (\partial r_{21} \over \partial x_2\), 8x2, which is related to the gradient of the image on the target frame.
12. VecNRf JabF[2];corresponding \ ({\partial r_{21} \over \partial a_{21}}, {\partial r_{21} \over \partial b_{21}}\), 8x1,8x1.
14. Mat22f JIdx2;corresponding \ ({\partial r_{21} \over \partial x_2}^t{\partial r_{21} \over \partial x_2}\), 2x8 8x2,2x2.
16. Mat22f JabJIdx;corresponding \ (\begin{bmatrix}{\partial r_{21} \over \partial a_{21}} & {\partial r_{21} \over \partial b_{21}} \end{bmatrix }^t{\partial r_{21} \over \partial x_2}\), 2x8 8x2,2x2.
18. Mat22f Jab2;corresponding \ (\begin{bmatrix}{\partial r_{21} \over \partial a_{21}} & {\partial r_{21} \over \partial b_{21}} \end{bmatrix }^t\begin{bmatrix}{\partial r_{21} \over \partial a_{21}} & {\partial r_{21} \over \partial b_{21}} \end{bmatrix}\) ,2x8 2x8,2x2.

These variables are calculated in Pointframeresidual::linearize.

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

Variables in the projection process are used at the time of calculation, and these variables are now mapped to formulas. The standard formula for the projection process is as follows:

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

The corresponding relationship of variables is as follows:

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();

Calculation \ (\partial x_2 \over \partial \rho_1\), which has been discussed in the blog "Direct photometric error derivative derivation" has been described how to solve. The resulting results are:

\[\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;

I calculate the derivative of the camera's internal control is inconsistent with the code, first write my derivation process.

\[{\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} & Amp {\partial v_2 \over \partial f_y} & {\partial v_2 \over \partial c_x} & {\partial v_2 \over \partial c_y} \END{BMA Trix}\]

\[\begin{align}x_2 &= Kx_2^{\prime} \notag \\begin{bmatrix} u_2 \ v_2 \ 1 \end{bmatrix} &= \begin{bmatrix} f_x & Amp 0 & c_x \ 0 & f_y & c_y \ 0 & 0 & 1\end{bmatrix} \begin{bmatrix} u_2^{\prime} \ \ V_2^{\prime} \ 1\e Nd{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} \no Tag &{\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 &{\part ial v_2 \over \partial c_y} &= f_y {\partial v_2^{\prime} \over \partial c_y} + 1 \notag \end{align}\]

First \ ({\partial x_2^{\prime} \over \partial c}\) , then use the chain rule to find \ ({\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 + \dots \notag \&= \rho_2 \rho_1^{-1} \begin{bmatrix} r_{11} &am P 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{bmatri X} u_1 \ v_1 \ 1 \end{bmatrix} + \dots \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} + \dots \notag \end{align}\]

\[\begin{align} u_2^{\prime} &= \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}) + \do TS \notag \v_2^{\prime} &= \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}) + \dots \ n Otag \end{align}\]

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

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

Chain Type:

\[\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_{11}) F_x^{-1} (u_1-c_x) \notag \{\part ial u_2 \over \partial f_y} &= f_x {\partial u_2^{\prime} \over \partial f_y} \notag \ &= f_x f_y^{-1} \rho_2 \rh O_1^{-1} (-r_{12}) F_y^{-1} (v_1-c_y) \notag \{\partial u_2 \over \partial c_x} &= f_x {\partial u_2^{\prime} \over \pa Rtial c_x} + 1 \notag \ &= \rho_2 \rho_1^{-1} (-r_{11}) + 1\notag \{\partial u_2 \over \partial c_y} &= f_x {\part Ial U_2^{\prime} \over \partial c_y} \notag \ &= f_x f_y^{-1} \rho_2 \rho_1^{-1} (-r_{12}) \notag\end{align}\] /p>

\[\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_{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_{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_{21}) \notag \{\partial v_2 \over \partial c_y} &= f_y {\partial v_2^{\prime} \over \p Artial c_y} + 1 \notag \ &= \rho_2 \rho_1^{-1} (-r_{22}) + 1 \notag \end{align}\]

The derivative that the Code considers (the effect of removing the scale parameter):

\[\begin{align} {\partial u_2 \over \partial f_x} &= 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 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} &= \rho_2 \rho_1^{-1} (r _{31} u_2^{\prime}-r_{11}) + 1 \notag \ {\partial u_2 \over \partial c_y} &= 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 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} + \rho_2 \rho_1^{-1} (r_{32} v_2^{\prime}-r_{2 2}) F_y^{-1} (v_1-c_y) \notag \{\partial v_2 \over \partial c_x} &= 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} &= \rho_2 \rho_1^{-1} (r_{32} v_2^{\prime}-r_{22}) + 1 \notag \end{ Align}\]

I think the derivative corresponds to the following code:

d_c_x[2] = drescale* (-pre_rtll_0 (0,0));d _c_x[3] = HCALIB->FXL () * drescale* (-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 (1,0)) * HCALIB->FXLI ();d _c_y[3] = drescale* (-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;

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

d_xi_x[0] = new_idepth*HCalib->fxl();d_xi_x[10;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[00;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();

Calculation \ (\partial x_2 \over \partial \xi_{21}\), which has been discussed in the blog "Direct photometric error derivative derivation" has been described how to solve. The resulting results are:

\[{\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^{\PR Ime}_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];Corresponding\ (\partial r_{21} \over \partial x_2\)

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

Calculation \ (\partial r_{21} \over \partial x_2\), which has been discussed in the blog "Direct photometric error derivative derivation" has been described how to solve. The resulting results are:

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

Note that this variable is 8 dimensions in the code.

DSO Windowed Optimization Code