矩阵常见分解算法及其在SLAM中的应用

https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html

常见特殊矩阵定义

• 酉矩阵，正交矩阵
  • A的元素是属于复数域矩阵，如果 $$A{A}^{\ast }=I$$，那么A是属于酉矩阵（Unitary Matrix）
  • A的元素是属于实数域矩阵，如果 $$A{A}^T=I$$，那么A是属于正交矩阵（Orthogonal Matrix），当然也是Unitary Matrix
• 埃尔米特矩阵，对阵矩阵
  • A的元素是属于复数域矩阵，如果 $$A=A^{\ast }$$，那么A是属于埃尔米特矩阵（Hermitian Matrix）
  • A的元素是属于实数域矩阵，如果 $$A=A^T$$，那么A是属于对称矩阵（Symmetric Matrix），当然也是Hermitian Matrix
• 正定矩阵
  

Cholesky分解（正定Hermittian矩阵，分解结果唯一）

将一个正定Hermitian矩阵分解为两个下三角矩阵的乘积
$$A=L{L}^T$$
另一种分解方式$$A=L{L}^T=LD{L}^T$$，要求A是半正定或者半负定矩阵，条件比上面的分解方式宽松

Cholesky分解应用

• 对于LinearGaussian 系统的状态估计问题求解 $$H^T{W}^{-1}Hx=H^T{W}^{-1}Z$$
  求解x的时候会把 $$H^T{W}^{-1}H=L{L}^{\ast }$$ 分解完，令 $$L^{\ast }x=d$$
  （1）求解 $$Ld=H^T{W}^{-1}Z$$中的d
  （2）求解$$L^{\ast }x=d$$中的x
  以上可以导出LG系统的batch递推形式，包括5个前向公式和1个后项公式。其等价于RTS算法。

SVD分解（将singularvalues排序后分解唯一）

将任意矩阵 $$A_{M\times N}=U_{M\ast M}{\Sigma }_{M\times N}{V}_{N\times N}$$,其中 U和V都是Unitary Matrix， $$\Sigma$$是对角矩阵，
如果M > N，如下图。U_null_space.transpose() * A 应该是零矩阵。
反之如N> M，会有A * V_null_space为零矩阵。
MSCKF利用零空间这个性质，可以消除feature 3d位置估计误差在整个残差中的影响。

  Eigen::MatrixXd A = Eigen::MatrixXd::Random(6, 3);
  // Eigen::MatrixXd A = Eigen::MatrixXd::Random(3, 6);
  A << 1, 0, -1, -2, 1, 4, 3, 4, 5, 5, -7, 9, -1, 4, -6, 4, 7, -9;

  cout << "Here is the matrix A:" << endl << A << endl << endl;
  // do SVD decomposition, and print out the singular values
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(
      A, Eigen::ComputeFullU | Eigen::ComputeFullV);
  cout << "Its singular values are:" << endl
       << svd.singularValues() << endl
       << endl;
  cout << "Its left singular vectors are the columns of the thin U matrix:"
       << endl
       << svd.matrixU() << endl
       << endl;
  cout << "Its right singular vectors are the columns of the thin V matrix:"
       << endl
       << svd.matrixV() << endl
       << endl;

  const int dim = svd.singularValues().size();
  if (A.rows() >= A.cols()) {
    const Eigen::MatrixXd U_null_space =
        svd.matrixU().rightCols(A.rows() - dim);
    const Eigen::MatrixXd U_null_space_times_A = U_null_space.transpose() * A;
    cout << "U null space * A" << endl
         << U_null_space_times_A << endl
         << endl;  // should be zero
  } else {
    const Eigen::MatrixXd V_null_space =
        svd.matrixV().rightCols(A.cols() - dim);
    const Eigen::MatrixXd A_times_V_null_space = A * V_null_space;
    cout << "A * V null space" << endl
         << A_times_V_null_space << endl
         << endl;  // should be zero
  }

  cout << "U*U^T = \n"
       << svd.matrixU() * svd.matrixU().transpose() << endl
       << endl;  // should be identity
  cout << "V*V^T = \n"
       << svd.matrixV() * svd.matrixV().transpose() << endl
       << endl;  // should be identity

  return 0;

SVD 分解的应用（任意矩阵）

• 用于PCA，SVD的奇异值的平方等于特征值，即 $${\sigma }_i=\sqrt{{\lambda }_i}$$。比如一堆点云，可以利用PCA性质提取line point和plane point。
• 求矩阵零空间。MSCKF中的应用。矩阵列向量的零空间维度为：dim(null space) = cols(A) - rank(A)
  • 行向量子空间的维度 = rank(A) = m - （A的零空间的维度）= m - dim[Null(A)]
  • 列向量子空间的维度 = rank(A) = n - （A的零空间的维度）=n - dim[Null(A)]

QR分解（任意矩阵，如果A可逆并且限定分解R对角线为正，则分解唯一）

一个m*n矩阵A（m>=n），可以分解为一个Unitary Matrix Q和一个上三角矩阵R
$$A_{m\times n}=Q_{m\times m}{R}_{m\times n}$$
其中R的后m-n行全部为零，可以写作
$$A=QR=Q\left[\begin{array}{c}{R}_{1_{n\times n}}\\ 0\end{array}\right]=\left[\begin{array}{ll}{Q}_{1_{m\times n}}& Q_{2_{m\times (m-n)}}\end{array}\right]\left[\begin{array}{c}{R}_{1_{n\times n}}\\ 0_{(m-n)\times n}\end{array}\right]=Q_{1_{m\times n}}{R}_{1_{n\times n}}$$
类似的可以有QL，RQ，LQ分解，其中L是下三角矩阵。

  Eigen::MatrixXd A = Eigen::MatrixXd::Random(6, 3);
  A << 1, 0, -1, -2, 1, 4, 3, 4, 5, 5, -7, 9, -1, 4, -6, 4, 7, -9;
  Eigen::VectorXd b = Eigen::VectorXd::Random(6);

  Eigen::ColPivHouseholderQR<Eigen::MatrixXd> qr(A);
  MatrixXd householderQ = qr.householderQ();
  MatrixXd matrixQ = qr.matrixQ();  // 和householderQ一样
  MatrixXd matrixQR_triangular_upper =
      qr.matrixQR().triangularView<Eigen::Upper>();
  MatrixXd matrixR = qr.matrixR();

  cout << "The rank of A is " << qr.rank() << endl << endl;
  cout << "householderQ matrix is:\n" << householderQ << endl << endl;
  cout << "matrixQ matrix is:\n" << matrixQ << endl << endl;
  cout << "matrixQR matrix is:\n" << qr.matrixQR() << endl << endl;
  cout << "matrixQR.triangularView<Eigen::Upper>() matrix is:\n"
       << matrixQR_triangular_upper << endl
       << endl;
  cout << "matrixR matrix is:\n" << matrixR << endl << endl;

  // Do QR decomposition to sove Ax = b
  Eigen::VectorXd x = qr.solve(b);
  cout << "The solution is:\n" << x << endl << endl;

QR分解应用

• 求矩阵的零空间，类似SVD
• 用来求解线性最小二乘问题 $$A_{m\times n}{x}_{n\times 1}=b_{m\times 1}$$, m >> n
  通常的解法 $$x=(A^{T}A{)}_{m\times m}^{-1}b$$ 直接求逆的话，维度很大，耗时
  将A进行QR分解，化简得到 $$R_{1}x=Q_1^{T}b$$ 并且R1是上三角矩阵，无需求逆，直接方程最后一行开始求解，便可以快速得到x
• Kalman filter中观测量的维度m过大导致m>>状态维度n， $$H_{m\times n}$$矩阵计算 $$(HP{H}^T+R{)}^{-1}$$非常耗时

 const MatrixXX innovation_covariance = H * P * HT + V;
 const MatrixNX K = P * HT * innovation_covariance.inverse();

观测误差方程 $$r(x)=H\tilde{X}+n_0$$
对H进行QR分解，两边乘以 $$Q^T$$并带入上式得到 $$\left[\begin{array}{l}{\mathbf{Q}}_1^T{\mathbf{r}}_o\\ {\mathbf{Q}}_2^T{\mathbf{r}}_o\end{array}\right]=\left[\begin{array}{c}{\mathbf{R}}_1\\ 0\end{array}\right]\tilde{\mathbf{X}}+\left[\begin{array}{l}{\mathbf{Q}}_1^T{\mathbf{n}}_o\\ {\mathbf{Q}}_2^T{\mathbf{n}}_o\end{array}\right]$$
于是观测量维度为m的问题转化为了观测量维度为n的
$$Q_1^T{r}_0=R1\tilde{X}+Q_1^T{n}_0$$
将原问题中的H，残差r，观测噪声n都进行了变换，代码实现如下

  MatrixXd H_thin;
  VectorXd r_thin;

  if (H.rows() > H.cols()) {
    // Convert H to a sparse matrix.
    SparseMatrix<double> H_sparse = H.sparseView();

    // Perform QR decompostion on H_sparse.
    SPQR<SparseMatrix<double>> spqr_helper;
    spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);
    spqr_helper.compute(H_sparse);

    MatrixXd H_temp;
    VectorXd r_temp;
    (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp);
    (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp);

    H_thin = H_temp.topRows(21 + state_server.cam_states.size() * 6);
    r_thin = r_temp.head(21 + state_server.cam_states.size() * 6);

    // HouseholderQR<MatrixXd> qr_helper(H);
    // MatrixXd Q = qr_helper.householderQ();
    // MatrixXd Q1 = Q.leftCols(21+state_server.cam_states.size()*6);

    // H_thin = Q1.transpose() * H;
    // r_thin = Q1.transpose() * r;
  } else {
    H_thin = H;
    r_thin = r;
  }

  // Compute the Kalman gain.
  const MatrixXd &P = state_server.state_cov;
  MatrixXd S = H_thin * P * H_thin.transpose() +
               Feature::observation_noise *
                   MatrixXd::Identity(H_thin.rows(), H_thin.rows());
  // MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H_thin*P);
  MatrixXd K_transpose = S.ldlt().solve(H_thin * P);
  MatrixXd K = K_transpose.transpose();