SLAM问题：线性方程求解和矩阵分解

兔子不吃草～

已于 2024-04-13 16:46:00 修改

阅读量749

点赞数 12

分类专栏： SLAM问题文章标签：矩阵线性代数 SLAM中线性方程求解

于 2024-03-30 11:32:41 首次发布

本文链接：https://blog.csdn.net/qq_49561752/article/details/137169002

版权

SLAM问题专栏收录该内容

3 篇文章 0 订阅

订阅专栏

SLAM问题：线性方程求解和矩阵分解

文章目录

SLAM问题：线性方程求解和矩阵分解
1 线性方程求解
- 1.1 Ax=b
- 1.2 Ax=0
2 QR分解
- 2.1 对一个矩阵进行QR分解
- 2.2 解线性方程
3 SVD分解
- 3.1 分解矩阵
- 3.2 求线性方程组
5 LU分解
6 Cholesky 分解
7 LDLT分解

关于矩阵分解的知识，很多内容已经在之前的博客提到，这里不再详细介绍了。

1 线性方程求解

AX = b 如何解，有多少解，需要分类讨论，QR和SVD那个计算效率高，超定方程如何解？ AX=0如何解？

1.1 Ax=b

$Ax=b，其中A\in R^{m*n},x\in R^n,b\in R^m$

① 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)=min\parallel Ax-b\parallel_2^2$

对x求导，令f(x)一阶导数为0（求导参见）：

$A^TAx=A^Tb$

上面这个式子又回到到了Ax=b的求解（经典最小二乘优化求线性方程），只不过这个时候 $A_{n×n}$ ，剩下的就是利用矩阵分解的方法取求解了，比如VINS-Fuison中调用LDLT分解法进行求解。
$x=(A^TA).\mathrm{ldlt}().\mathrm{solve}(A^Tb)$

1.2 Ax=0

SLAM中求解Ax=0问题的场景很多，比如PnP中DLT直接线性变换求解投影矩阵，三角化、求解单应矩阵H、基础矩阵F、VINS初始化时估计外参 $q_{bc}$ 等等（对于基础矩阵比较特殊，会进行二次SVD分解，因为其秩为2，不满秩，进行二次SVD分解后，强制将最小的奇异值置为0）

求解都是利用SVD分解，然后x即V的最后一列，具体的推导参考之前三角化笔记

2 QR分解

任何矩阵都有QR分解，但是满秩矩阵QR分解唯一

2.1 对一个矩阵进行QR分解

Givens吉文斯参考，下面代码就是对这个例子进行分解

在这里插入图片描述

Householder 变换

#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 解线性方程

Householder

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;

colPivHouseholderQr

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分解法则避免了这一问题

兔子不吃草～

关注

12
点赞
踩
15

收藏

觉得还不错? 一键收藏
0
评论
SLAM问题：线性方程求解和矩阵分解

关于矩阵分解的知识，很多内容已经在之前的博客提到，这里不再详细介绍了。
复制链接

扫一扫

专栏目录