视觉SLAM十四讲：从理论到实践（二）——三维空间的刚体运动

沉鱼叶

已于 2023-04-07 14:56:17 修改

阅读量154

点赞数

分类专栏：视觉SLAM十四讲文章标签：算法

于 2023-03-02 19:59:28 首次发布

本文链接：https://blog.csdn.net/weixin_45811693/article/details/129296531

版权

视觉SLAM十四讲专栏收录该内容

2 篇文章 0 订阅

订阅专栏

二、熟悉Eigen矩阵运算

Eigen（http://eigen.tuxfamily.org）是常用的 C++ 矩阵运算库，具有很高的运算效率。大部分需要在 C++ 中使用矩阵运算的库，都会选用 Eigen 作为基本代数库，例如 Google Tensorflow，Google Ceres，GTSAM 等。本次习题，你需要使用 Eigen 库，编写程序，求解一个线性方程组。为此，你需要先了解一些有关线性方程组数值解法的原理。

设线性方程 $\mathbf{A}$ $\mathbf{x}$ = $\mathbf{b}$ ，在 $\mathbf{A}$ 为方阵的前提下，请回答以下问题：

1. 在什么条件下，x 有解且唯一？

rank(A) = rank(A, b) = n

2. 高斯消元法的原理是什么？
用逐次消去未知数的方法，把原线性方程组Ax = b化为与其等价的三角形线性方程组，而求解三角形线性方程组可用回代的方法求解。换句话说，高斯消元法就是用初等行变换将原线性方程组系数矩阵化为上三角矩阵，从而将求解原线性方程组的问题转化为求解简单方程组的问题。

3. QR 分解的原理是什么？
若 $\mathbf{A}$ 为非奇异矩阵，则存在正交矩阵 $\mathbf{Q}$ 与上三角矩阵 $\mathbf{R}$ ，使 $\mathbf{A}$ = $\mathbf{Q}$ $\mathbf{R}$ 。（当限定 $\mathbf{R}$ 的对角元素为正时，这种分解是唯一的）

4. Cholesky 分解（平方根法）的原理是什么？
若 $\mathbf{A}$ 为对称正定矩阵，则存在下三角矩阵 $\mathbf{L}$ ，使 $\mathbf{A}$ = $\mathbf{L}$ $\mathbf{L}^\mathrm{T}$ 。（当限定 $\mathbf{L}$ 的对角元素为正时，这种分解是唯一的）

5. 编程实现 A 为 100 × 100 随机矩阵时，用 QR 和 Cholesky 分解求 x 的程序。你可以参考本次课用到的 useEigen 例程。

#include <ctime>
#include <iostream>
using namespace std;

#include <eigen3/Eigen/core>
#include <eigen3/Eigen/Dense>
using namespace Eigen;

int main()
{
	MatrixXd matrix_X = MatrixXd::Random(100, 100);
	VectorXd v_Xd = VectorXd::Random(100);
	// QR decomposition
    clock_t time_start = clock();
    x = matrix_X.colPivHouseholderQr().solve(v_Xd);
    cout << "time of QR decomposition is: " << 1000 * (clock() - time_start) / double(CLOCKS_PER_SEC) << " ms" << endl;
    cout << "x = " << x.transpose() << endl;
    //Cholesky decomposition
    time_start = clock();
    x = matrix_X.ldlt().solve(v_Xd);
    cout << "time of Cholesky decomposition is: " << 1000 * (clock() - time_start) / double(CLOCKS_PER_SEC) << " ms" << endl;
    cout << "x = " << x.transpose() << endl;
    return 0;
}

提示：你可能需要参考相关的数学书籍或文章。请善用搜索引擎。Eigen 固定大小矩阵最大支持到 50，所以你会用到动态大小的矩阵。

三、矩阵论基础

除了我们本科学过的基础线性代数之外，大部分研究生课程还会开设矩阵论课程，以作为对本科阶段知识的扩充。对于很多线性问题（SLAM 里也会碰到许多线性问题），了解一些矩阵论基础知识是很有好处的。我们在附件中为大家提供了张贤达老师的《矩阵分析与应用》。请参考该书内容（或者你能找到的其他书籍），回答以下问题。

1. 什么是正定矩阵和半正定矩阵？
特征值均为正（非负）的矩阵是（半）正定矩阵。

几何意义：向量 ${x}$ 经过矩阵 $A$ 代表的线性变换 $T$ 后变成 ${Ax}$ ，如果变换前后的向量夹角小于90°，即 ${x}^TAx>0$ （ ${x}^TAx$ 是变换前后的向量作内积），则矩阵 $A$ 为正定矩阵，如果变换前后的向量夹角小于等于90°，即 ${x}^TAx\ge0$ ，则矩阵 $A$ 为半正定矩阵。

从特征值的几何意义来看，也比较好理解，如果特征值均为正，代表对于特征向量的变换是同向的伸缩变换（没有发生翻转），那么从特征基坐标系来看，所有向量经过变换后，与原向量的夹角肯定都是锐角，所以特征值均为正的矩阵是正定矩阵。

2. 对于方阵 A，它的特征值是什么？特征向量是什么？特征值一定是实数吗？如何计算一个矩阵的特征值？
一个 $n\times n$ 的矩阵 $A$ 的特征值和特征向量是该方程得到的： $A\mathbf{v}=\lambda\mathbf{v}$

其中， $\lambda$ 是特征值， $\mathbf{v}$ 是对应的特征向量（非零）。通过求解特征多项式方程可以计算特征值，可以这个方程的解可以是实数或复数。然而，如果一个矩阵 $A$ 是实对称矩阵，即 $A=A^T$ ，则其特征值一定是实数。一般通过解特征方程求得矩阵的特征值。

特征值和其对应的特征向量为观察线性变换提供了最清晰的视角，在几何上来说，对于一个线性变换，大部分向量都既有伸缩、也有旋转，但是特征向量经过变换后只进行了伸缩，伸缩的倍率就是特征值。

3. 什么是矩阵的相似性？相似性反映了什么几何意义？
如果存在可逆矩阵 $P$ ，使得 $P^{-1}AP=B$ ，则有A~B。
几何意义：相似的两个矩阵是同一个线性变换在不同坐标系下的描述。

4. 矩阵一定能对角化吗？什么样的矩阵能保证对角化？不能对角化的矩阵能够形成什么样的形式（Jordan 标准形）？
有n个线性无关的特征向量的n维方阵一定可对角化，线性无关特征向量不足n个的其余n维方阵可以化为Jordan标准型。

5. 奇异值分解（SVD）是什么意思？
奇异值分解是方阵特征分解在非方矩阵上的推广，目的是通过分解，找出该矩阵对应线性变换的简洁表达。

奇异值分解的定义为： $A=U\Sigma V^{-T}$
其中A是MxN的矩阵，U是MxM的正交矩阵，V是NxN的正交矩阵， $\Sigma$ 是MxN的矩阵，除对角线上是从大到小排列的奇异值，其他位置都是0。

6. 矩阵的伪逆是什么意思（Pseudo inverse）？莫尔——彭多斯逆是如何定义的？怎么计算一个矩阵的伪逆？

7. 对于超定方程：Ax = b 且 A 不可逆时，我们通常计算最小二乘解：x = arg min∥Ax − b∥。线性方程的最小二乘解在代数意义上是可以解析地写出来的。请回答以下小问题：

(1)在 b ̸= 0 时，x 的解是什么形式？事实上，我们可以对 A 求奇异值或对于 ATA 求特征值。请阐述两者之间的关系。

(2)当 b = 0 时，我们希望求 x 的非零解。请说明如何求解 x。

(3)请谈谈你对上述解法在几何意义上的理解。该问题为开放问题。
经过线性变换A后，向量 $x^*$ 变成了向量 $Ax^*$ ，它与向量 $b$ 是最接近的。

四、几何运算练习

在这里插入图片描述

#include <iostream>
using namespace std;

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
using namespace Eigen;

int main()
{
    Quaterniond q_BC(0.8, 0.2, 0.1, 0.1), q_BL(0.3, 0.5, 0, 20.1);
    q_BC.normalize();
    q_BL.normalize();
    Vector3d t_BC(0.5, 0.1, 0.5), t_BL(0.4, 0, 0.5);
    Vector3d p_C(0.3, 0.2, 1.2);

    Isometry3d T_BC(q_BC), T_BL(q_BL);
    T_BC.pretranslate(t_BC);
    T_BL.pretranslate(t_BL);

    Vector3d p_L = T_BL.inverse() * T_BC * p_C;
    cout << p_L.transpose() << endl; // p_L = (-0.641551, 0.137248, 1.10744)

    Quaterniond q_RB(0.99, 0, 0, 0.01), q_WR(0.55, 0.3, 0.2, 0.2);
    q_RB.normalize();
    q_WR.normalize();
    Vector3d t_RB(0.05, 0, 0.5), t_WR(0.1, 0.2, 0.3);

    Isometry3d T_WR(q_WR), T_RB(q_RB);
    T_WR.pretranslate(t_WR);
    T_RB.pretranslate(t_RB);

    Vector3d p_W = T_WR * T_RB * T_BC * p_C;
    cout << p_W.transpose() << endl; // p_W = (2.37806,-0.134779, 0.873485)
    return 0;
}

八、* 熟悉C++11

C++ 是一门古老的语言，但它的标准至今仍在不断发展。在 2011 年、2014 年和 2017 年，C++ 的标准又进行了更新，被称为 C++11，C++14，C++17。其中，C++11 标准是最重要的一次更新，让 C++发生了重要的改变，也使得近年来的 C++ 程序与你在课本上（比如谭浩强）学到的 C++ 程序有很大的不同。你甚至会惊叹这是一种全新的语言。C++14 和 C++17 则是对 11 标准的完善与扩充。越来越多的程序开始使用 11 标准，它也会让你在写程序时更加得心应手。本题中，你将学习一些 11标准下的新语法。请参考本次作业 books/目录下的两个 pdf，并回答下面的问题。

设有类 A，并有 A 类的一组对象，组成了一个 vector。现在希望对这个 vector 进行排序，但排序的
方式由 A.index 成员大小定义。那么，在 C++11 的语法下，程序写成：

1 #include <iostream>
2 #include <vector>
3 #include <algorithm>
4
5 using namespace std;
6
7 class A {
8 public:
9 A(const int& i ) : index(i) {}
10 int index = 0;
11 };
12
13 int main() {
14 A a1(3), a2(5), a3(9);
15 vector<A> avec{a1, a2, a3};
16 std::sort(avec.begin(), avec.end(), [](const A&a1, const A&a2) {return a1.index<a2.index;});
17 for ( auto& a: avec ) cout<<a.index<<" ";
18 cout<<endl;
19 return 0;
20 }

请说明该程序中哪些地方用到了 C++11 标准的内容。提示：请关注范围 for 循环、自动类型推导、lambda
表达式等内容。

范围for循环（参考《C++ Primer Plus》P125）

for ( auto& a: avec ) cout<<a.index<<" ";

C++11新增了一种循环：基于范围（range-based）的for循环。这简化了一种常见的循环任务：对数组（或容器类，如vector和array）的每个元素执行相同的操作。以上语句中，a最初表示数组avec的第一个元素a1，打印一次a.index后，进行下一次循环，a表示数组avec的第二个元素a2，不断执行循环，a依次表示数组的每个元素。但由于使用了&引用变量，所以其实在三次循环中是直接对a1、a2、a1进行操作的。

自动类型推导（参考《C++ Primer Plus》P56）

for ( auto& a: avec ) cout<<a.index<<" ";

auto和范围for循环经常组合使用，这让我们不用显式指定每次循环的元素的具体类型，auto可以进行自动推断。在本例中，由于循环的每个元素是class A类型，所以写成下面形式也是一样的。

for (A & a: avec)

lambda（参考《C++ Primer Plus》P662）

std::sort(avec.begin(), avec.end(), [](const A&a1, const A&a2) {return a1.index<a2.index;});

lambda表达式常用于写一些简短的匿名函数，语法组成为 [] (参数列表) {函数体}，对于接受函数指针或函数符的函数，都可以使用匿名函数定义（lambda）作为其参数。本例中的sort()方法，前两个参数接受一个区间，第三个参数是一个不接受任何参数的函数对象。（函数对象包含函数指针、函数符和lambda）