简介
自动驾驶的项目大多数本质上是一系列数学问题的求解，在求解过程中经常会涉及到各种的矩阵的运算，因此有必要了解一下常用的矩阵分解和其使用场景。由于移动机器人建图和定位上基本不会涉及复数矩阵，所以下面提到的矩阵一般都为实矩阵。矩阵分解常用的使用场景有：求解线性方程组、对一个数据集进行特征提取、以及某些情况下获得特定结构的解。下面测试中用到的代码在：matrix_decomposition
常见的矩阵分解
首先介绍一下常见的矩阵分解方式，这里不会涉及到具体怎么进行分解，具体的操作上数值分析的时候还会一点，现在全忘了：）。大概会整理各种矩阵分解完成后矩阵的形式，对原矩阵结构性质的要求以及计算复杂度。
LU 分解及其变种
LU 分解
对一个可逆方阵，LU 分解能够将矩阵分解为一个下三角矩阵（Lower Triangle）和一个上三角矩阵（Upper Triangle）的乘积，如下所示。LU 分解的理论时间复杂度大约为 $O(n^3)$ 。
$$
\boldsymbol{A} = \boldsymbol{LU}, \qquad \boldsymbol{A}, \boldsymbol{L}, \boldsymbol{U} \in \mathbb{R}^{n\times n}
$$
LDU 分解
在 LU 分解的基础上还能得到 LDU 分解，即从 $L$ 矩阵和 $U$ 矩阵中提取出对角线，形成一个对角矩阵 $D$，如下所示，此时 $L$，$U$ 对角线上元素皆为 1：
$$
\boldsymbol{A} = \boldsymbol{LDU}
$$
LLT 分解（Cholesky 分解）
进一步，如果矩阵 $A$ 是对称正定的，那么 LU 分解中的 $U$ 矩阵为 $L$ 的转置（$L$ 仍为下三角矩阵），因此该分解称为 LLT 分解，时间复杂度大约为 $O(n^3)$，但由于矩阵是对称，需要的运算次数大约为 LU 分解的一半，如下所示：
$$
\boldsymbol{A} = \boldsymbol{LL}^T
$$
LDLT 分解
同样的，我们也可以把三角矩阵的对角线单独提取出来，作为一个独立的对角线矩阵，如下所示：
$$
\boldsymbol{A} = \boldsymbol{LDL}^T
$$
QR 分解
对任意方阵，QR 分解可以将其分解为一个正交矩阵 Q 和一个上三角矩阵 R 的的乘积，QR 分解的理论时间复杂度为 $O(n^3)$，即：
$$
\begin{aligned}
\boldsymbol{A} &= \boldsymbol{QR}\\
\boldsymbol{Q}^T &= \boldsymbol{Q}^{-1}
\end{aligned}
$$
对非方阵也可以进行 QR 分解，此时 Q 仍为正交矩阵，R 由一个上三角矩阵和零矩阵组成，如下所示：
$$
\begin{aligned}
\boldsymbol{A}_{m\times n} &= \boldsymbol{Q}_{m\times m}\boldsymbol{R}_{m\times n}\\
&= \boldsymbol{Q}_{m\times m}\begin{bmatrix}
\boldsymbol{R}^1_{n\times n} \\ \boldsymbol{0}_{(m - n)\times n}
\end{bmatrix}\\
&= \begin{bmatrix}
\boldsymbol{Q}^1_{m\times n} & \boldsymbol{Q}^2_{m\times(m - n)}
\end{bmatrix}
\begin{bmatrix}
\boldsymbol{R}^1_{n\times n} \\ \boldsymbol{0}_{(m - n)\times n}
\end{bmatrix}\\
&= \boldsymbol{Q}^1\boldsymbol{R}^1
\end{aligned}
$$
特征分解
特征值分解
对一个可逆矩阵，特征值分解可以将其分解为一个可逆矩阵 $\boldsymbol{Q}$ 和对角线矩阵 $\boldsymbol{\Lambda}$ 的乘积，时间复杂度为 $O(n^3)$，如下所示：
$$
\begin{aligned}
\boldsymbol{A} &= \boldsymbol{Q}\boldsymbol{\Lambda}\boldsymbol{Q}^{-1}\\
\boldsymbol{Q} &=
\begin{bmatrix}
\boldsymbol{v}_1 & \boldsymbol{v}_2 & ... & \boldsymbol{v}_n
\end{bmatrix}\\
\boldsymbol{\Lambda} &=
\begin{bmatrix}
\lambda_1 & 0 & ... & 0\\
0 & \lambda_2 & ... & 0\\
0 & 0 & ... & \lambda_n
\end{bmatrix}\\
\boldsymbol{A}\boldsymbol{v}_i & = \lambda_i\boldsymbol{v}_i
\end{aligned}
$$
其中，$\boldsymbol{Q}$ 中的每一列 $\boldsymbol{v}_i$ 称为矩阵的特征向量，对应的在对角线中相应的值 $\lambda_i$ 为该向量对应的特征值。
奇异值分解 （SVD 分解）
对任意实矩阵，奇异值分解可以将其分解为两个正交矩阵和一个对角矩阵的乘积，时间复杂度为 $O(\min(mn^2, m^2n))$，如下所示：
$$
\begin{aligned}
\boldsymbol{A}_{m\times n} &= \boldsymbol{U}_{m\times m}\boldsymbol{\Sigma}_{m \times n} \boldsymbol{V}^{T}_{n \times n}\\
\boldsymbol{U}\boldsymbol{U}^{T} &= \boldsymbol{I}_{m\times m}\\
\boldsymbol{V}\boldsymbol{V}^{T} &= \boldsymbol{I}_{n \times n}
\end{aligned}
$$
其中，$\boldsymbol{\Sigma}$ 中对角线上的元素称为该矩阵的奇异值，非零奇异值的个数为矩阵的秩，分解时通常保证奇异值依次递减。$\boldsymbol{U}$ 和 $\boldsymbol{V}$ 的列分别称为该矩阵的左奇异值向量和右奇异值向量。
Eigen 库中的矩阵分解
Eigen 是移动机器人中常用的用于矩阵运算的库，这里简单比较一下 LU 分解和 QR 分解的计算精度和速度。这里使用求解线性方程组来比较精度（求解原理在下一部分简单描述），测试代码为：
#include <chrono>
#include <iostream>
#include <regex>
#include <boost/type_index.hpp>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
template <typename T, typename F>
void evaluate(const T& A, const T& b) {
F dec(A);
auto start = std::chrono::high_resolution_clock::now();
T x = dec.solve(b);
auto end = std::chrono::high_resolution_clock::now();
T diff = A * x - b;
std::string decomposition_type = boost::typeindex::type_id<F>().pretty_name();
std::cmatch match_result;
std::string label = "undefined.\n";
if (std::regex_match(decomposition_type.c_str(), match_result, std::regex("Eigen::(.*)<Eigen.*"))) {
label = match_result[1];
}
std::cout << "--------- " << label << " ---------\n";
std::cout << "Time used: " << std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count() << " ms\n";
std::cout << "Loss: " << diff.norm() << std::endl;
std::cout << "--------- " << label << " ---------\n";
return;
}
int main() {
Eigen::MatrixXf A = Eigen::Matrix<float, 1000, 1000>::Random();
Eigen::MatrixXf b = Eigen::Matrix<float, 1000, 1>::Random();
// LU
evaluate<Eigen::MatrixXf, Eigen::FullPivLU<Eigen::MatrixXf>>(A, b);
evaluate<Eigen::MatrixXf, Eigen::PartialPivLU<Eigen::MatrixXf>>(A, b);
// QR
evaluate<Eigen::MatrixXf, Eigen::HouseholderQR<Eigen::MatrixXf>>(A, b);
evaluate<Eigen::MatrixXf, Eigen::FullPivHouseholderQR<Eigen::MatrixXf>>(A, b);
evaluate<Eigen::MatrixXf, Eigen::ColPivHouseholderQR<Eigen::MatrixXf>>(A, b);
return 0;
}
输出为：
--------- FullPivLU ---------
Time used: 8 ms
Loss: 0.00270244
--------- FullPivLU ---------
--------- PartialPivLU ---------
Time used: 7 ms
Loss: 0.00426831
--------- PartialPivLU ---------
--------- HouseholderQR ---------
Time used: 18 ms
Loss: 0.000284534
--------- HouseholderQR ---------
--------- FullPivHouseholderQR ---------
Time used: 20 ms
Loss: 0.000397712
--------- FullPivHouseholderQR ---------
--------- ColPivHouseholderQR ---------
Time used: 19 ms
Loss: 0.000369753
--------- ColPivHouseholderQR ---------
可以看到 LU 分解普遍比 QR 分解快，精度相应的略低。但由于 LU 分解数值稳定性不够的原因，使用 QR 分解的情况会更多一些，完整的列表见：Linear algebra and decompositions。（注：Eigen 的官方说明中，FullPivLU 的精度应该更高而速度应该会更慢，而 HouseholderQR 也是相反，其中的原因不太清楚，可能是矩阵选择的原因，Eigen 中用来计算的方阵都是对称的。）
求解线性方程组（最小二乘问题）
首先来看一种最常见的的矩阵分解使用场合，即通过最小二乘约束构建线性方程组，需要求该线性方程组的解。关于最小二乘问题，之前有专门写过博客来整理其来由以及常用解法：最小二乘问题和非线性优化，简单来说，对于最小二乘问题，通常最后问题会转换为求解一个齐次线性方程组或者非齐次线性方程组（齐次线性方程组可以视为一组特殊的线性方程组），如下所示：
$$
\begin{aligned}
\boldsymbol{Ax} &= \boldsymbol{b}
\end{aligned}
$$
先来看非齐次线性方程组，根据系数矩阵的特点有几种情况：
系数矩阵是稠密的
求解最小二乘问题原理
如对一系列点进行最小二乘拟合多项式曲线：
$$
\begin{aligned}
&\left\{
\begin{aligned}
a_0 + a_1x_0 + a_2x_0^2 + ... + a_mx_0^m &= b_0 \\
a_0 + a_1x_1 + a_2x_1^2 + ... + a_mx_1^m &= b_1 \\
... \\
a_0 + a_1x_n + a_2x_n^2 + ... + a_mx_n^m &= b_n
\end{aligned}
\right.\Rightarrow\boldsymbol{Ax} =\boldsymbol{b}\\
\boldsymbol{A} &=
\begin{bmatrix}
1 & x_0 & ... & x_0^m\\
1 & x_1 & ... & x_1^m\\
& ...& \\
1 & x_n & ... & x_n^m\\
\end{bmatrix}\\
\boldsymbol{x} &=
\begin{bmatrix}
a_0 \\ a_1 \\ ... \\a_m
\end{bmatrix} \qquad
\boldsymbol{b} = \begin{bmatrix}
b_0 \\ b_1 \\ ... \\ b_n
\end{bmatrix}
\end{aligned}
$$
一般来说，在系数矩阵可逆（矩阵有唯一解）的情况下，应用矩阵分解来求解线性方程组的方程组无非是通过矩阵的三角结构来进行逐个参数求解，以及通过矩阵本身的正交性质来进行快速求逆，从而移到矩阵右侧求解。下面以 LU 分解和 SVD 分解来简单看一下这两种方法：
- LU 分解
上面提到 LU 分解可以将矩阵分解成一个下三角矩阵和上三角矩阵的乘积，因此线性方程组分解如下所示：
$$
\begin{aligned}
\boldsymbol{A} &= \boldsymbol{LU}\\
\Rightarrow \boldsymbol{Ax} &= \boldsymbol{b}\\
\boldsymbol{LUx} &= \boldsymbol{b}\\
\begin{bmatrix}
l_{11} & 0 & ... & 0\\
l_{21} & l_{22} & ... & 0\\
&...&\\
l_{n1} & l_{n2} & ... & l_{nn}
\end{bmatrix}
\begin{bmatrix}
u_{11} & u_{12} & ... & u_{1n}\\
0 & u_{22} & ... & u_{2n}\\
&...&\\
0 & 0 & ... & u_{nn}
\end{bmatrix}
\begin{bmatrix}
x_1 \\ x_2 \\ ... \\ x_n
\end{bmatrix}&=
\begin{bmatrix}
b_1 \\ b_2 \\ ... \\ b_n
\end{bmatrix}
\end{aligned}
$$
令 $\boldsymbol{Ux} = \boldsymbol{x}'$，有：
$$
\begin{aligned}
\boldsymbol{Lx}' &= \boldsymbol{b}\\
\begin{bmatrix}
l_{11} & 0 & ... & 0\\
l_{21} & l_{22} & ... & 0\\
&...&\\
l_{n1} & l_{n2} & ... & l_{nn}
\end{bmatrix}
\begin{bmatrix}
x'_1 \\ x'_2 \\ ... \\ x'_n
\end{bmatrix}&=
\begin{bmatrix}
b_1 \\ b_2 \\ ... \\ b_n
\end{bmatrix}
\end{aligned}
$$
这种三角结构，可以从上至下很简单的计算出每个参数，从而解出 $\boldsymbol{x}'$，而对于 $\boldsymbol{Ux} = \boldsymbol{x}'$，我们同样可以利用 $\boldsymbol{U}$ 的三角结构解出结果。
- SVD 分解
SVD 分解能够将矩阵分解成两个正交矩阵和一个对角矩阵的乘积，利用这个性质可以很轻松的求系数矩阵的逆，从而求解线性方程组，如下所示：
$$
\begin{aligned}
\boldsymbol{Ax} &= \boldsymbol{b}\\
\boldsymbol{U\Sigma V}^T\boldsymbol{x} &= \boldsymbol{b}\\
\boldsymbol{x} &= \boldsymbol{V}\boldsymbol{\Sigma}^{-1}\boldsymbol{U}^T\boldsymbol{b}
\end{aligned}
$$
而对于线性最小二乘问题，通常通过大量数据点构建线性方程组 $\boldsymbol{Ax} = \boldsymbol{b}, \boldsymbol{A} \in \mathbb{R}^{m\times n}, m > n$，此时线性方程组是超定的，即方程组无解，此时我们可以应用以下方式来求出满足该方程组的近似最优解，即求：
$$
\boldsymbol{x}^* = \arg\min_{\boldsymbol{x}}||\boldsymbol{Ax} - \boldsymbol{b}||_2
$$
对于这一类稠密的线性方程组，可以利用以下三种方式求解：
- SVD 分解
设 $\boldsymbol{A}$ 列满秩，则对 $\boldsymbol{A}$ 进行 SVD 分解有：
$$
\begin{aligned}
\boldsymbol{A} &= \boldsymbol{U}
\begin{bmatrix}
\boldsymbol{\Sigma}_{n\times n} \\ \boldsymbol{0}
\end{bmatrix}\boldsymbol{V}^T\\
&=\begin{bmatrix}
\boldsymbol{U}_n & \boldsymbol{U}_{m - n}
\end{bmatrix}
\begin{bmatrix}
\boldsymbol{\Sigma}_{n\times n} \\ \boldsymbol{0}
\end{bmatrix}\boldsymbol{V}^T
\end{aligned}
$$
利用 $\boldsymbol{U}$ 的正交性可以作出变换：
$$
\begin{aligned}
&||\boldsymbol{Ax} - \boldsymbol{b}||^2_2\\
=&||\boldsymbol{U}^T(\boldsymbol{Ax} - \boldsymbol{b})||^2\\
=&||
\begin{bmatrix}
\boldsymbol{\Sigma}_{n\times n} \\ \boldsymbol{0}
\end{bmatrix}\boldsymbol{V}^T\boldsymbol{x} -
\begin{bmatrix}
\boldsymbol{U}_n^T\\\boldsymbol{U}_{m-n}^T
\end{bmatrix}\boldsymbol{b}||^2_2\\
=&\left|\left|
\begin{bmatrix}
\boldsymbol{\Sigma}_{n\times n}\boldsymbol{V}^T\boldsymbol{x} - \boldsymbol{U}_n^T\boldsymbol{b}\\
-\boldsymbol{U}_{m-n}^T\boldsymbol{b}
\end{bmatrix}\right|\right|^2_2\\
=&||\boldsymbol{\Sigma}_{n\times n}\boldsymbol{V}^T\boldsymbol{x} - \boldsymbol{U}_n^T\boldsymbol{b}||^2_2 + ||\boldsymbol{U}_{m-n}^T\boldsymbol{b}||^2_2 \\
\geq& ||\boldsymbol{U}_{m-n}^T\boldsymbol{b}||^2_2
\end{aligned}
$$
上述最后不等式，等号成立当且仅当：
$$
\begin{aligned}
\boldsymbol{\Sigma}_{n\times n}\boldsymbol{V}^T\boldsymbol{x} - \boldsymbol{U}_n^T\boldsymbol{b} &= \boldsymbol{0}\\
\boldsymbol{\Sigma}_{n\times n}\boldsymbol{V}^T\boldsymbol{x} &= \boldsymbol{U}_n^T\boldsymbol{b}\\
\boldsymbol{x} &= \boldsymbol{V}\boldsymbol{\Sigma}_{n\times n}^{-1}\boldsymbol{U}_n^T\boldsymbol{b}
\end{aligned}
$$
由此看出，使用 SVD 分解求线性最小二乘问题不仅需要求奇异值，还需要求解 SVD 两个矩阵中的前 n 列。
- QR 分解
对 $\boldsymbol{A}$ 进行 QR 分解有：
$$
\begin{aligned}
\boldsymbol{A} &= \boldsymbol{QR}\\
&= \begin{bmatrix}
\boldsymbol{Q}_n & \boldsymbol{Q}_{m - n}
\end{bmatrix}
\begin{bmatrix}
\boldsymbol{R}_n \\ \boldsymbol{0}
\end{bmatrix}
\end{aligned}
$$
同样，利用 $\boldsymbol{Q}$ 的正交性，进行以下变换：
$$
\begin{aligned}
&||\boldsymbol{Ax} - b||^2_2\\
=&||\boldsymbol{Q}^T(\boldsymbol{QR}\boldsymbol{x} - \boldsymbol{b})||^2_2\\
=&||\boldsymbol{Rx} - \boldsymbol{Q}^T\boldsymbol{b}||^2_2\\
=&\left|\left|
\begin{bmatrix}
\boldsymbol{R}_n \\ \boldsymbol{0}
\end{bmatrix}\boldsymbol{x} -
\begin{bmatrix}
\boldsymbol{Q}^T_n\\
\boldsymbol{Q}^T_{m - n}\\
\end{bmatrix}\boldsymbol{b}\right|\right|\\
=&\left|\left|
\begin{bmatrix}
\boldsymbol{R}_n\boldsymbol{x} - \boldsymbol{Q}_n^T\boldsymbol{b} \\
-\boldsymbol{Q}^T_{m - n}\boldsymbol{b}
\end{bmatrix}\right|\right|^2_2\\
=& ||\boldsymbol{R}_n\boldsymbol{x} - \boldsymbol{Q}_n^T\boldsymbol{b}||^2_2 + ||\boldsymbol{Q}^T_{m - n}\boldsymbol{b}||^2_2\\
\geq& ||\boldsymbol{Q}^T_{m - n}\boldsymbol{b}||^2_2\\
\end{aligned}
$$
上述不等式等号成立当且仅当:
$$
\begin{aligned}
\boldsymbol{R}_n\boldsymbol{x} - \boldsymbol{Q}_n^T\boldsymbol{b} &= \boldsymbol{0}\\
\boldsymbol{R}_n\boldsymbol{x} &= \boldsymbol{Q}_n^T\boldsymbol{b}
\end{aligned}
$$
由于 $\boldsymbol{R}_n$ 是上三角矩阵，因此求解该方程组非常简单。
- 通过正规方程求解
最后还能通过构建正规方程来求解，即求解以下方程组：
$$
\boldsymbol{A}^T\boldsymbol{A}\boldsymbol{x} = \boldsymbol{A}^T\boldsymbol{b}
$$
由于 $\boldsymbol{A}^T\boldsymbol{A}$ 显然是对称且正定的，因此可以应用 LLT 或 LDLT 分解来降低分解的计算量，然后利用其三角结构的特殊性来进行快速求解。虽然这种方式求解速度比较快，但是由于 $\boldsymbol{A}^T\boldsymbol{A}$ 的条件数是 $\boldsymbol{A}$ 的平方。因此使用正规方程最多丧失近两倍的精度，因此对于病态矩阵而言，不推荐使用这种方式来求解最小二乘问题。
注：当 $\boldsymbol{b} = \boldsymbol{0}$ 时，此时方程组变成了齐次的，此时可以应用特征值分解来求解，这部分留到后面整理。
使用 Eigen 求解最小二乘问题
下面基于上述提到的多项式拟合问题基于 Eigen 对三种方法求解进行简单比较，以下为测试：
#include <chrono>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
template <typename matrix_type, typename vector_type, typename F>
vector_type evaluate(const matrix_type& A,
const vector_type& b,
const vector_type& gt,
F solve,
const std::string& label) {
auto start = std::chrono::high_resolution_clock::now();
vector_type coeff = solve(A, b);
auto end = std::chrono::high_resolution_clock::now();
vector_type diff = coeff - gt;
std::cout << "--------- " << label << " ---------\n";
std::cout << "Time used: " << std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count() << " ms\n";
std::cout << "Ground Truth: " << gt.transpose() << std::endl;
std::cout << "Fitted: " << coeff.transpose() << std::endl;
std::cout << "Loss: " << diff.norm() << std::endl;
std::cout << "--------- " << label << " ---------\n";
return coeff;
}
int main(int argc, char** argv) {
const int order = 3;
// y = c[0] + c[1]x + c[2]x^2 + ...
Eigen::VectorXd gt_coeffcients(order + 1);
gt_coeffcients << 20.0, -0.002, 0.005, -0.0008;
int number_data = 5000;
Eigen::VectorXd x_data(number_data); // perfect x
Eigen::VectorXd y_data(number_data); // perfect y
Eigen::VectorXd b(number_data);
x_data.array() = Eigen::ArrayXd::LinSpaced(number_data, -60.0, 60.0);
Eigen::MatrixXd A(number_data, order + 1);
for (size_t i = 0; i <= order; ++i) {
A.col(i).array() = x_data.array().pow(i);
}
y_data = A * gt_coeffcients;
b.array() = y_data.array() * (1.0 + Eigen::ArrayXd::Random(number_data) / 10.0);
/******** SVD **************/
auto svd_solver = [](const Eigen::MatrixXd& A, const Eigen::VectorXd& b) -> Eigen::VectorXd {
return A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
};
Eigen::VectorXd svd_coeff = evaluate(A, b, gt_coeffcients, svd_solver, "SVD");
Eigen::VectorXd svd_y_data = A * svd_coeff;
/******** QR **********/
auto qr_solver = [](const Eigen::MatrixXd& A, const Eigen::VectorXd& b) -> Eigen::VectorXd {
return A.colPivHouseholderQr().solve(b);
};
Eigen::VectorXd qr_coeff = evaluate(A, b, gt_coeffcients, qr_solver, "QR");
Eigen::VectorXd qr_y_data = A * qr_coeff;
/******** Normal Equation *******/
auto normal_solver = [](const Eigen::MatrixXd& A, const Eigen::VectorXd& b) -> Eigen::VectorXd {
return (A.transpose() * A).ldlt().solve(A.transpose() * b);
};
Eigen::VectorXd normal_coeff = evaluate(A, b, gt_coeffcients, normal_solver, "Normal Equation");
Eigen::VectorXd normal_y_data = A * normal_coeff;
}
输出如下，可以看出，在精度要求不是特别高，且知道矩阵不是病态时，使用正规方程求解速度是最快的：
--------- SVD ---------
Time used: 9 ms
Ground Truth: 20 -0.002 0.005 -0.0008
Fitted: 19.894 0.000521196 0.0051483 -0.000801827
Loss: 0.106006
--------- SVD ---------
--------- QR ---------
Time used: 3 ms
Ground Truth: 20 -0.002 0.005 -0.0008
Fitted: 19.894 0.000521196 0.0051483 -0.000801827
Loss: 0.106006
--------- QR ---------
--------- Normal Equation ---------
Time used: 0 ms
Ground Truth: 20 -0.002 0.005 -0.0008
Fitted: 19.894 0.000521196 0.0051483 -0.000801827
Loss: 0.106006
--------- Normal Equation ---------
数据分布如下：
系数矩阵按块稀疏分布的
除了稠密的系数矩阵以外，还有一类常见系数矩阵是按块稀疏的。通常在位姿图优化问题会出现，常见的位姿图构建过程为：
- 对前后两帧位姿通过里程计构建约束，这些约束块会集中在信息矩阵的对角线上
- 通过回环检测找到历史帧之间某两帧的约束，这些约束会稀疏地分散在对角线的两侧
通过上述两个过程中，最后得到矩阵的结构为：对角线分布一系列块矩阵，分角线两侧稀疏且对称分布一些块矩阵，如下图所示：（数据来源：Intel Berkeley Research Lab Sensor Data ）
通常位姿图优化求解的是以下正规方程：
$$
\begin{aligned}
\boldsymbol{J}^T\boldsymbol{J}\Delta\boldsymbol{x} &= -\boldsymbol{J}^T\boldsymbol{e}\\
\boldsymbol{H}\Delta\boldsymbol{x} &= \boldsymbol{b}
\end{aligned}
$$
对于这个方程我们当然也可以使用上述求解稠密系数线性方程的方式求解，但是这里由于系数矩阵本身的稀疏性，可以利用这个性质进行更快速求解。
Eigen 中自带了一些稀疏矩阵求解器，这里我们使用几个在非线性优化中常用的求解方式，分别是：
- 稀疏 LLT 分解
- 共轭梯度求解
- SuiteSparse 库中的 LLT 分解
测试代码如下，测试用的矩阵大小为，3684 x 3684：
#include <chrono>
#include <iostream>
#include <Eigen/CholmodSupport>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseQR>
#include "utils/utils.hpp"
template <typename matrix_type, typename vector_type, typename F>
void evaluate(const matrix_type& H, const vector_type& b, F& solver, const std::string& label) {
auto start = std::chrono::high_resolution_clock::now();
solver.compute(H);
solver.solve(b);
auto end = std::chrono::high_resolution_clock::now();
std::cout << "--------- " << label << " ---------\n";
std::cout << "Time used: " << std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count() << " ms\n";
std::cout << "--------- " << label << " ---------\n";
}
int main(int argc, char** argv) {
// Fill H and b
Eigen::SparseMatrix<double> H_sparse;
Eigen::MatrixXd b;
std::cout << "Size of H: " << H.rows() << " x " << H.cols() << std::endl;
std::cout << "Size of b: " << b.rows() << " x " << b.cols() << std::endl;
// Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::AMDOrdering<int>> qr_solver;
// evaluate(H_sparse, b, qr_solver, "Sparse QR");
Eigen::SimplicialLLT<Eigen::SparseMatrix<double>> llt_solver;
evaluate(H_sparse, b, llt_solver, "Sparse LLT");
Eigen::ConjugateGradient<Eigen::SparseMatrix<double>> CG_solver;
evaluate(H_sparse, b, CG_solver, "Conjugate Gradient");
Eigen::CholmodSimplicialLLT<Eigen::SparseMatrix<double>> cholmod_llt_solver;
evaluate(H_sparse, b, cholmod_llt_solver, "Cholmod LLT");
}
输出结果如下，可以看出 Eigen 自带的稀疏 LLT 分解性能和专门的稀疏矩阵求解库比起来还是有一定差距，在用 G2O 和 Ceres 等非线性优化库时大部分也是使用 SuiteSparse 提供的稀疏线性求解器：
Size of H: 3684 x 3684
Size of b: 3684 x 1
--------- Sparse LLT ---------
Time used: 18 ms
--------- Sparse LLT ---------
--------- Conjugate Gradient ---------
Time used: 0 ms
--------- Conjugate Gradient ---------
--------- Cholmod LLT ---------
Time used: 1 ms
--------- Cholmod LLT ---------
系数矩阵按楔形分布
通过在视觉 SLAM 使用 Bundle Adjustment 中会出现这种分布，由于图中引入的约束有两种来源：特征和位姿、位姿和位姿。一般不会有特征和特征之间的约束，因此占优化变量大多数的特征是没有相对约束的。因此正规方程中系数矩阵会成楔形分布，如下所示。数据来自 Bundle Adjustment in the Large。
对于这种类型的矩阵，我们可以应用舒尔补来对矩阵进行分解，具体过程可以参考我之前写的博客：SLAM 中的边缘化以及滑动窗口算法。
构建特殊方程组的解
在一些特殊的使用场景下，对一个线性方程组：$\boldsymbol{A}_1\boldsymbol{A}_2...\boldsymbol{A}_n = \boldsymbol{B}$，我们可能会希望其中部分矩阵满足某种性质或者结构，这个时候也可以利用矩阵分解来完成。
构建 ICP 的解
ICP 算法可以参考：激光 SLAM 点云配准 - 基于 ICP 及其变种的方法，这里不详细介绍，在 ICP 的最后一步中，我们需要求解一个正交矩阵 $\boldsymbol{R}$，将一个矩阵：$\boldsymbol{RH}$ 转化为 $\boldsymbol{A}\boldsymbol{A}^T$ 即一个矩阵 $\boldsymbol{A}$ 乘上它的转置的形式，此时我们可以利用 SVD 分解来对其进行拼凑，如下所示：
$$
\begin{aligned}
\boldsymbol{H} &= \boldsymbol{U\Sigma V}^T
\end{aligned}
$$
令 $\boldsymbol{R} = \boldsymbol{V}\boldsymbol{U}^T$，则有：
$$
\boldsymbol{RH} = \boldsymbol{V}\boldsymbol{U}^T\boldsymbol{U\Sigma V}^T = \boldsymbol{V\Sigma V}^T = (\boldsymbol{V\Sigma}^{1/2})(\boldsymbol{V\Sigma}^{1/2})^T
$$
因此，我们得到了满足条件的解。
从本质矩阵中恢复旋转和平移
在视觉 SLAM 中，有时候我们需要从本质矩阵 $\boldsymbol{E} = \boldsymbol{t}^\wedge\boldsymbol{R}$ 中恢复旋转和平移，此时可以用 SVD 分解来进行拼凑，过程可以参考：本质矩阵E求解及运动状态恢复
利用 QR 分解估计内参矩阵和旋转
TODO·
数据集特征提取
有时候我们需要对数据集进行提取，例如提取一堆数据的法向量等。此时可以使用主成分分析来提取数据的主要特征方向，而这个过程需要使用到的矩阵的特征值分解。具体例子可以参考：[代码实践] 使用 PCA 进行平面和线特征拟合