DSO windowed optimization 代码

Posted Jinge

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了DSO windowed optimization 代码相关的知识,希望对你有一定的参考价值。

这里不想解释怎么 marginalize,什么是 First-Estimate Jacobian。这里只想看看代码,看看Hessian矩阵是怎么构造出来的。

代码中用于存储窗口优化过程Hessian矩阵使用的导数存储在结构体RawResidualJacobian中。

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

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\),这里的\(C\)指相机内参\(\begin{bmatrix} f_x, f_y, c_x, c_y\end{bmatrix}^T\)
  4. Vec2f Jpdd;对应\(\partial x_2 \over \partial \rho_1\),2x4,注意是对 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;对应\(\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。
  9. Mat22f Jab2;对应\(\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。

在 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 + \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} & 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} + \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}) + \dots \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 \notag \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 & {\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 & {\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}\]

链式:

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

代码认为的导数(去除 scale 参数的影响):

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

我认为的导数对应的代码如下:

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[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维。

以上是关于DSO windowed optimization 代码的主要内容,如果未能解决你的问题,请参考以下文章

DSO 中的Windowed Optimization

DSO 中的Windowed Optimization

C++学习(四零四)hidden symbol `atexit‘ in XXX is referenced by DSO

Windows Desktop Optimization.bat

conan入门(二十一):解决MinGW编译Openssl的编译错误:crypto/dso/dso_win32.c

DSO 428