SLAM问题:线性方程求解和矩阵分解
文章目录
关于矩阵分解的知识,很多内容已经在之前的博客提到,这里不再详细介绍了。
1 线性方程求解
AX = b
如何解,有多少解,需要分类讨论,QR和SVD那个计算效率高,超定方程如何解? AX=0
如何解?
1.1 Ax=b
A x = b ,其中 A ∈ R m ∗ n , x ∈ R n , b ∈ R m Ax=b,其中A\in R^{m*n},x\in R^n,b\in R^m Ax=b,其中A∈Rm∗n,x∈Rn,b∈Rm
①
m < n
, 欠定方程,有无穷多解②
m = n
,A非奇异,有唯一解;不可逆,无穷解或无解③
m > n
,超定方程,没有精确解,只有最小二乘解(Cholesky 分解、SVD分解、LU分解、QR分解—csdn十四讲笔记第6讲哪里把这部分都补充了),QR比SVD快,但SVD更准确④ SLAM中
Ax=b
问题,如海塞矩阵HΔx=b(m=n)
,不过这里一般会通过舒尔补进行边缘化操作;还有如直接法中HΔx=b的求解就会引入ldlt
分解(没有稀疏特性一般都这种,顺便提一句,直接法的雅可比也就是比间接法多了个像素梯度导数);如VINS初始化陀螺仪偏置b校准、初始化速度、重力g和尺度s等
SLAM里面一般都是超定问题,观测 >> 求解量,一般都是引入最小二乘解决!
- 构建最小二乘
f ( x ) = m i n ∥ A x − b ∥ 2 2 f(x)=min\parallel Ax-b\parallel_2^2 f(x)=min∥Ax−b∥22
- 对x求导,令f(x)一阶导数为0(求导参见):
A T A x = A T b A^TAx=A^Tb ATAx=ATb
上面这个式子又回到到了Ax=b
的求解(经典最小二乘优化求线性方程),只不过这个时候
A
n
×
n
A_{n×n}
An×n,剩下的就是利用矩阵分解的方法取求解了,比如VINS-Fuison
中调用LDLT
分解法进行求解。
x
=
(
A
T
A
)
.
l
d
l
t
(
)
.
s
o
l
v
e
(
A
T
b
)
x=(A^TA).\mathrm{ldlt}().\mathrm{solve}(A^Tb)
x=(ATA).ldlt().solve(ATb)
1.2 Ax=0
SLAM中求解Ax=0问题的场景很多,比如PnP中
DLT
直接线性变换求解投影矩阵,三角化
、求解单应矩阵H
、基础矩阵F
、VINS
初始化时估计外参 q b c q_{bc} qbc等等(对于基础矩阵比较特殊,会进行二次SVD分解,因为其秩为2,不满秩,进行二次SVD分解后,强制将最小的奇异值置为0)
求解都是利用SVD分解,然后x即V的最后一列,具体的推导参考之前三角化笔记
2 QR分解
任何矩阵都有QR分解,但是满秩矩阵QR分解唯一
2.1 对一个矩阵进行QR分解
- Givens吉文斯参考,下面代码就是对这个例子进行分解
#include <iostream>
#include <Eigen/Dense>
int main() {
// 任何矩阵都有QR分解,满秩矩阵的QR分解唯一
Eigen::MatrixXd A(3, 3);
A << 6, 5, 0,
5, 1, 4,
0, 4, 3;
/**************1 Householder 变换**********************/
Eigen::HouseholderQR<Eigen::MatrixXd> qr1(A);
Eigen::MatrixXd Q_Householder = qr1.householderQ(); // 6*6
// 用于获取矩阵的上三角部分
Eigen::MatrixXd R_Householder = qr1.matrixQR().triangularView<Eigen::Upper>(); // 6*3
/**************2 Givens吉文斯旋转变换**********************/
Eigen::ColPivHouseholderQR<Eigen::MatrixXd> qr2(A);
Eigen::MatrixXd Q_Givens = qr2.matrixQ();
Eigen::MatrixXd R_Givens = qr2.matrixR().template triangularView<Eigen::Upper>();
std::cout << "Q_Householder matrix:\n" << Q_Householder << std::endl;
std::cout << "R_Householder matrix:\n" << R_Householder << std::endl;
std::cout << "Q_Givens matrix:\n" << Q_Givens << std::endl;
std::cout << "R_Givens matrix:\n" << R_Givens << std::endl;
/**************3 验证QR乘积**********************/
Eigen::MatrixXd AA = Q_Householder * R_Householder;
Eigen::MatrixXd AB = Q_Givens * R_Givens;
std::cout << "AA matrix:\n" << AA << std::endl;
std::cout << "AB matrix:\n" << AB << std::endl;
return 0;
}
2.2 解线性方程
typedef Matrix<float,3,3> Matrix3x3;
Matrix3x3 m = Matrix3x3::Random();
Matrix3f y = Matrix3f::Random();
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Here is the matrix y:" << endl << y << endl;
Matrix3f x;
x = m.householderQr().solve(y);
assert(y.isApprox(m*x));
cout << "Here is a solution x to the equation mx=y:" << endl << x << endl;
Matrix3f m = Matrix3f::Random();
Matrix3f y = Matrix3f::Random();
cout << "Here is the matrix m:" << endl << m << endl;
cout << "Here is the matrix y:" << endl << y << endl;
Matrix3f x;
x = m.colPivHouseholderQr().solve(y);
assert(y.isApprox(m*x));
cout << "Here is a solution x to the equation mx=y:" << endl << x << endl;
3 SVD分解
3.1 分解矩阵
#include <iostream>
#include <Eigen/Dense>
int main() {
// 定义一个3x3的矩阵
Eigen::MatrixXd A(4, 5);
A << 1, 0, 0, 0, 2,
0, 0, 3, 0, 0,
0, 0, 0, 0, 0,
0, 4, 0, 0, 0;
// 求解A^T*A特征值和特征向量
Eigen::EigenSolver<Eigen::MatrixXd> solver1(A.transpose() * A);
Eigen::VectorXd eigenvaluesV = solver1.eigenvalues().real();
Eigen::MatrixXd eigenvectorsV = solver1.eigenvectors().real();
// 求解A*A^T特征值和特征向量
Eigen::EigenSolver<Eigen::MatrixXd> solver2(A * A.transpose());
Eigen::VectorXd eigenvaluesU = solver2.eigenvalues().real();
Eigen::MatrixXd eigenvectorsU = solver2.eigenvectors().real();
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
// 调用svd对象的singularValues()成员函数,矩阵D的奇异值
Eigen::VectorXd delta = svd.singularValues();
std::cout << "奇异值分解中的奇异值singularValues:" << delta.transpose() << std::endl;
// 取最小奇异值对应的右奇异向量
// Eigen::Vector4d u4 = svd.matrixU().col(3);
Eigen::MatrixXd U = svd.matrixU();
Eigen::MatrixXd V = svd.matrixV();
//
std::cout << "A^T*A 的特征值Eigenvalues:" << std::endl << eigenvaluesV << std::endl;
std::cout << "A^T*A 的特征向量Eigenvectors:" << std::endl << eigenvectorsV << std::endl;
std::cout << "A奇异值分解中的矩阵 V:" << std::endl << V << std::endl;
std::cout << "A*A^T 的特征值Eigenvalues:" << std::endl << eigenvaluesU << std::endl;
std::cout << "A*A^T 的特征向量Eigenvectors:" << std::endl << eigenvectorsU << std::endl;
std::cout << "A奇异值分解中的矩阵 U:" << std::endl << U << std::endl;
return 0;
}
// #include <iostream>
// #include <Eigen/Dense>
// int main() {
// Eigen::MatrixXd A(3, 2);
// A << 1, 2,
// 3, 4,
// 5, 6;
// // 计算 A^T A 的特征值和特征向量
// Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigensolver(A.transpose() * A);
// Eigen::VectorXd eigenvalues = eigensolver.eigenvalues();
// Eigen::MatrixXd eigenvectors = eigensolver.eigenvectors();
// // 进行奇异值分解
// Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);
// Eigen::MatrixXd U = svd.matrixU();
// Eigen::MatrixXd V = svd.matrixV();
// // 输出结果
// std::cout << "A^T A 的特征值:" << std::endl << eigenvalues << std::endl;
// std::cout << "A^T A 的特征向量:" << std::endl << eigenvectors << std::endl;
// std::cout << "奇异值分解中的矩阵 V:" << std::endl << V << std::endl;
// return 0;
// }
3.2 求线性方程组
这里直接和QR分解做对比
#include <iostream>
#include <Eigen/Dense>
#include <chrono>
using namespace Eigen;
using namespace std;
using namespace std::chrono;
int main() {
// 初始化随机矩阵A和向量b
MatrixXf A = MatrixXf::Random(100, 100);
VectorXf b = VectorXf::Random(100);
// QR分解求解Ax = b
auto start = high_resolution_clock::now();
VectorXf x_qr = A.householderQr().solve(b);
// VectorXf x_qr = A.colPivHouseholderQr().solve(b);
auto stop = high_resolution_clock::now();
auto duration = duration_cast<microseconds>(stop - start);
cout << "QR分解求解耗时: " << duration.count() << "微秒" << endl;
// SVD分解求解Ax = b
start = high_resolution_clock::now();
VectorXf x_svd = A.jacobiSvd(ComputeThinU | ComputeThinV).solve(b);
stop = high_resolution_clock::now();
duration = duration_cast<microseconds>(stop - start);
cout << "SVD分解求解耗时: " << duration.count() << "微秒" << endl;
// .norm()计算2范数
double res1 = (A * x_qr - b).norm();
double res2 = (A * x_svd - b).norm();
std::cout << "ColPivHouseholderQR残差: " << res1 << std::endl;
std::cout << "SVD残差: " << res2 << std::endl;
return 0;
}
测试结果:SVD肯定比QR慢,但是这里精确度也一般
QR分解求解耗时: 14508微秒
SVD分解求解耗时: 237052微秒
ColPivHouseholderQR残差: 2.91984e-05
SVD残差: 0.000174283
5 LU分解
LU 分解适用于一般矩阵,将矩阵 (A) 分解为一个下三角矩阵 (L) 和一个上三角矩阵 (U) 的乘积,即 (A = LU)。在某些情况下,还可能需要进行行交换,此时分解形式为 (PA = LU),其中 § 是置换矩阵。
限制:方阵!
6 Cholesky 分解
Cholesky 分解专用于对称正定矩阵
7 LDLT分解
若A为一对称矩阵且其任意一k阶主子阵均不为零,则A有如下惟一的分解形式:
A=LDL^T
其中L
为下三角形单位矩阵(即主对角线元素皆为1),D
为一对角矩阵(只在主对角线上有元素,其余皆为零),L^T
为L
的转置矩阵。
LDLT
分解法实际上是Cholesky
分解法的改进,因为Cholesky
分解法虽然不需要选主元,但其运算过程中涉及到开方问题,而LDLT
分解法则避免了这一问题