矩阵常见分解算法及其在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 valuesEigen::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 identitycout << "V*V^T = \n"<< svd.matrixV() * svd.matrixV().transpose() << endl<< endl;  // should be identityreturn 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 = bEigen::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();