Function Marginalize

src/Marginalization.cpp:28–90

{

  void Marginalize(const Vector &Residual, const Matrix &Jacobian,
                   Vector &ResidualMarg, Matrix &JacobianMarg,
                   const int SizeMarginal, const double HessianInflation)
  {
    /** H = J^T * J */
    Matrix Hessian = Jacobian.transpose() * Jacobian;
    Hessian = Matrix((Hessian.array().abs() > 1e-8).select(Hessian.array(), 0));/**< remove small non-zero entries for stability */

    /** b = J^T * r */
    const Vector B = Jacobian.transpose() * Residual;

    /** calculate size of the linear system */
    const int SizeTotal = Hessian.cols();
    const int SizeRemain = SizeTotal - SizeMarginal;

    /** select sub matrices  */
    /**
    Hessian:
    H = |H_MM H_MR|
        |H_RM H_RR|

    Residual vector:
    r = |r_M r_R|'
    */
    const Matrix HessMM = Hessian.block(0, 0, SizeMarginal, SizeMarginal);
    const Matrix HessRR = Hessian.block(SizeMarginal, SizeMarginal, SizeRemain, SizeRemain);
    const Matrix HessMR = Hessian.block(0, SizeMarginal, SizeMarginal, SizeRemain);
    const Matrix HessRM = HessMR.transpose();

    const Vector BM = B.head(SizeMarginal);
    const Vector BR = B.tail(SizeRemain);

    /** compute reduced linear system */
    /**
    New Hessian:
    H* = H_RR - H_RM x H_MM^-1 x H_MR

    New residual vector:
    r* = r_R - H_RM x H_MM^-1 x r_M
    */

    /** at first, invert the marginalized Hessian part */
    Eigen::CompleteOrthogonalDecomposition<Matrix> CODHess(HessMM);
    const Matrix HessMMInv = CODHess.pseudoInverse(); /**< pseudo-inverse for rank deficient matrices*/

    /** than compute the new system */
    Matrix HessRRStar = HessRR - HessRM * HessMMInv * HessMR;
    const Vector BRStar = BR - HessRM * HessMMInv * BM;

    /** add a small uncertainty to prevent accumulation of error (I recommend a value of 1.01)*/
    if (HessianInflation != 1.0)
    {
      HessRRStar /= HessianInflation;
    }

    /** convert to unsquared system system */
    ResidualMarg.resize(SizeRemain);
    JacobianMarg.resize(SizeRemain, SizeRemain);