视觉SLAM入门 -- 学习笔记 - Part2

熟悉 Eigen 矩阵运算

设线性方程 Ax = b，在 A 为 n x n ⽅阵的前提下，请回答以下问题：
1. 在什么条件下，x 有解且唯⼀？
当秩R(A，b) = R(A) = n（n为未知数个数）时有唯一解。

2. 高斯消元法的原理是什么？
通过用初等行变换将增广矩阵化为行阶梯形矩阵，然后回带求解线性方程组的解。

3. [数值计算] QR分解的原理是什么？
QR方法是用于求解矩阵所有特征值的算法

4. Cholesky分解法的原理是什么？

5. 编程实现 A 为 100 × 100 随机矩阵时，⽤ QR 和 Cholesky 分解求 x 的程序。

//
// Created by daybeha on 3/10/2021.
//

#include 
#include 
#include 
#include 

using namespace std;
using namespace Eigen;

#define MATRIX_SIZE 100

int main(int argc, char ** argv){

    Matrix<float, MATRIX_SIZE, MATRIX_SIZE> matrix_A =  Matrix<float, MATRIX_SIZE, MATRIX_SIZE>::Random();
    matrix_A = matrix_A.transpose() * matrix_A;  //制造实对称矩阵
    Matrix<float, MATRIX_SIZE, 1> matrix_b = Matrix<float, MATRIX_SIZE, 1>::Random();
    //    cout << matrix_A <
    //    cout << matrix_b <

    //inverse
    clock_t time_sst = clock();
    Matrix<float, MATRIX_SIZE, 1> x1 = matrix_A.inverse() * matrix_b;
    cout<<"normal inverse结果:"<<endl<<x1.transpose()<<endl;
    cout << "time use in normal inverse is" << 1000*(clock() - time_sst)/(double)CLOCKS_PER_SEC  << "ms" << endl;



    //QR
    time_sst = clock();
    Matrix<float, MATRIX_SIZE, 1> x2 = matrix_A.colPivHouseholderQr().solve(matrix_b);
    cout<<"QR result:"<<endl<<x2.transpose()<<endl;
    cout << "time use in QR decomposition is " << 1000*(clock() - time_sst)/(double)CLOCKS_PER_SEC  << "ms" << endl;


    //Cholesky 
    time_sst = clock();
    //llt()(正定矩阵），ldlt()（半正定/半负定）。所以代码里使用ldlt函数比较合理
    Matrix<float, MATRIX_SIZE, 1> x3 = matrix_A.ldlt().solve(matrix_b);
    cout<<"cholesky分解结果:"<<endl<<x3.transpose()<<endl;
    cout << "time use in Cholesky decomposition is " << 1000*(clock() - time_sst)/(double)CLOCKS_PER_SEC  << "ms" << endl;


    return  0;
}

运行结果：

几何运算练习

下⾯我们来练习如何使用 Eigen/Geometry 计算⼀个具体的例子。
在世界系W 下，存在一个运动的机器人R。按照固定的或者某些开发人员或者领导的特殊喜好，R系定义在机器人脚部的位置。但是机器人在设计的时候，又定义了B 系（Body 系，或本体系），位于机器人头部的位置。由于沟通不畅，标定人员把一台激光传感器和一台视觉传感器标定在了B 系下。我们称激光传感器为L 系，视觉传感器为C 系。现在请你完成以下工作：
1. 说明一个激光传感器下的看到的点应该如何计算它的世界坐标。

答：L系->B系->R系->W系。

提示：

1. 四元数在使用前需要归⼀化。
2. 请注意 Eigen 在使用四元数时的虚部和实部顺序。
3. 参考答案为 p2 = [1.08228, 0.663509, 0.686957]T。你可以用它验证程序是否正确。

//
// Created by daybeha on 4/10/2021.
//

//提⽰：
//1. 四元数在使⽤前需要归⼀化。
//2. 请注意Eigen 在使⽤四元数时的虚部和实部顺序。
//3. 参考答案为p2 = [1.08228, 0.663509, 0.686957] 。你可以⽤它验证程序是否正确。

#include 

#include 
#include 

using namespace Eigen;
using namespace std;
int main(int argc, char** argv){
    Quaterniond q1 = Quaterniond(0.55, 0.3, 0.2, 0.2).normalized();
    Vector3d t1 = Vector3d(0.7, 1.1, 0.2);
    Quaterniond q2 = Quaterniond (-0.1, 0.3, -0.7, 0.2).normalized();
    Vector3d t2 = Vector3d(-0.1, 0.4, 0.8);

    Vector3d p1 = Vector3d (0.5, -0.1, 0.2);

    Isometry3d T1 = Isometry3d::Identity();
    T1.rotate(q1);
    T1.pretranslate(t1);

    Isometry3d T2 = Isometry3d::Identity();
    T2.rotate(q2);
    T2.pretranslate(t2);

    Vector3d p2 = T2 * T1.inverse() * p1;

    cout<< p2.transpose() <<endl;

    return 0;
}

运行结果：

旋转的表达

旋转矩阵与四元数是⽇常应⽤中常见的表达⽅式。

1. 设有旋转矩阵 R，证明
   

请参考：
学习笔记 --证明旋转矩阵为正交阵

2. 设有四元数 q，我们把虚部记为 ε，实部记为 η，那么 q = (ε, η)。请说明 ε 和 η 的维度。

   答：虚部 ε的维度为3， 实部 η的维度为1。

3. 定义运算 + 和 ⊕ 为：
   

其中运算 × 含义与 ∧ 相同，即取 ε 的反对称矩阵（它们都成叉积的矩阵运算形式），1 为单位矩阵。请证明对任意单位四元数 q1, q2，四元数乘法可写成矩阵乘法：

或者

我知道有些同学可能跟我一样看不懂上面的过程……
我打算等有时间再推详细一点……

罗德里格斯公式的证明

plus: 旋转向量的方向与转轴 n 一致，长度等于旋转角$\theta$

提⽰：参考https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula

四元数运算性质的验证

请参考：
学习笔记 – 四元数：验证v‘ =qvq-1后，v‘为虚四元数

证明本质： 使用四元数计算3d点的运动得到的结果必然也是一个3d点。

熟悉 C++11

设有类 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
表达式等内容：

[](const A&a1, const A&a2) {return a1.index

使用lamda表达式

for ( auto& a: avec )

使用序列for循环 和 auto自动类型推导

2020-12-13更新

矩阵论基础

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

给定一个大小为 $n\times n$ 的实对称矩阵$A$ ，若对于任意长度为 $n$ 的非零向量 $x$，有 $x^{T}Ax>0$ 恒成立，则矩阵 $A$ 是一个**正定矩阵**。
性质：
① 单位矩阵是正定矩阵 (positive definite)。
② 若矩阵A正定，则必有|A|（矩阵A的行列式）>0，所以正定矩阵可逆。

给定一个大小为 $n\times n$ 的实对称矩阵$A$ ，若对于任意长度为 $n$ 的非零向量 $x$，有 $x^{T}Ax\ge 0$ 恒成立，则矩阵 $A$ 是一个半正定矩阵。
性质：半正定矩阵 $\supset$ 正定矩阵

2. 对于方阵A，它的特征值是什么？特征向量是什么？特征值一定是实数吗？如何计算一个矩阵的特征值？
答：

①设A为n阶实方阵，如果存在某个数${\lambda }_0$及某个n维非零列向量$\eta$，使得$A\eta ={\lambda }_0\eta$，则称${\lambda }_0$是方阵A的一个特征值，$\eta$是方阵A的属于特征值${\lambda }_0$的一个特征向量。
补充：一个特征空间（eigenspace）是具有相同特征值的特征向量与一个同维数的零向量的集合，可以证明该集合是一个线性子空间，如$E_{\lambda }=\{u\in V\mid Au=\lambda u\}$ 即为线性变换$A$ 中以 $\lambda$ 为特征值的特征空间。

②实矩阵的特征值不一定都是实数，只有实对称矩阵的特征值才保证是实数。另外复矩阵的特征值也可能有实数。

③$A\eta ={\lambda }_0\eta ⟶（A-\lambda I）x=0$
​ 若方程有非零解，那么$A-\lambda I$是奇异的，也就是行列式为零。因此可以通过$det(A-\lambda I)=0$求出特征值，
然后，针对每个特征值，再通过求解$（A-\lambda I）x=0$ 来找到特征向量。

3. 什么是矩阵的相似性？相似性反映了什么几何意义？
答：

①设 $A,B$都是 $n$ 阶矩阵，若有可逆矩阵 $P$，使 $P^{-1}AP=B$， 则称 $B$ 是 $A$ 的相似矩阵，或说 $A$ 和$B$相似， 记为$A\sim B$
本质：相似的矩阵是同一个线性变换在不同基/坐标系下的的不同描述。同一个运动过程（线性变换）在不同坐标系（不同基）中的表示矩阵（相似矩阵）虽然不一样，但实质上是指的同一个线性变换。

②从机器人坐标变换的角度探究相似矩阵的几何意义：
三维空间中有一点$P$，其在世界坐标系$W$ 和 机器人坐标系$C$ 下的坐标分别为$P_w$, $P_c$， 从机器人坐标系到世界坐标系的变换矩阵为$T_{wc}$;
点$P$变换到新的位置$P^{\prime }$，其在两坐标系下的坐标分别为$P_w^{\prime }$, $P_c^{\prime }$，该变换在两坐标系下分别为$A$，$B$，则A，B相似。
推导：
$$\begin{gathered} P_w^{\prime }=A{P}_w \\ {T}_{wc}^{-1}{P}_c^{\prime }=A{T}_{wc}^{-1}{P}_c \\ {T}_{wc}^{-1}B{P}_c=A{T}_{wc}^{-1}{P}_c \\ {T}_{wc}^{-1}B=A{T}_{wc}^{-1} \\ {T}_{wc}^{-1}B{T}_{wc}=A \end{gathered}$$

4. 矩阵一定能对角化吗？什么样的矩阵能保证对角化？不能对角化的矩阵能够形成什么样的形式（Jordan标准形）？
答：

① 主对角线之外的元素皆为0的矩阵为对角矩阵，常写为diag（a1，a2,…,an)
对于矩阵$M$，如果存在一个矩阵$A$，使$A^{-1}MA$为对角矩阵，则称矩阵$A$将矩阵$M$对角化。
但矩阵不一定能对角化。

② 对于任意一个$n\times n$矩阵，若其存在 n 个线性不相关的特征向量，则该矩阵可被对角化。

③ 不能对角化的矩阵能够形成Jodan型矩阵：上双对角矩阵，其上对角线的元素为1或0。这种标准型总能够实现，但不一定数值稳定。该标准型是一组“几乎对角的”矩阵。具体形式参考《矩阵分析与应用》张贤达这本书p237页。
百度的这个解释感觉比较清晰：

5. 奇异值分解（SVD）是什么意思？
答：

假设一个m×n的矩阵A，则定义矩阵A的SVD为：
$$A=U\Sigma V^T$$
其中$U\in R^{m\times m},\Sigma \in R^{m\times n}$且除了主对角线上的元素以外全为0，主对角线上的每个元素都称为奇异值，且已按大小排好序，$V\in R^{n\times n}$。
$U$和 $V$都是酉矩阵，即满足$$V^{T}V=I，U^{T}U=I$$
同时,$U$的列向量即是$A{A}^T$的特征向量，一般我们将$U$中的每个特征向量叫做$A{A}^T$的左奇异向量；$V$的列向量即是$A^{T}A$的特征向量，一般我们将$V$中的每个特征向量叫做$A^{T}A$的右奇异值。

也可以参考《视觉SLAM十四讲》第二版p196-198

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

①
$D^{+}$是$D$非零元素取倒数后再转置得到的。

② 莫尔-彭多斯逆（Moore-Penrose广义逆矩阵）:
设 $A\in C^{m\times n}$ ， 如 果 $G\in C^{m\times n}$满足
(1) AGA = A;
(2) GAG = G;
(3) $(AG{)}^H=AG$;
(4) $(GA{)}^H=GA$;
则G 为A 的Moore-Penrose广义逆矩阵。

③ 计算矩阵的伪逆:
计算矩阵$A_{m\times n}$ 的满秩分解A=FG。
求广义逆矩阵，也就是矩阵的伪逆$A^{-}=G^T(F^{T}A{G}^T{)}^{-1}{F}^T$。
另可用SVD计算伪逆.

7. 对于超定方程：Ax = b 且A 不可逆时，我们通常计算最小二乘解：$x=arg{\min }_x||Ax-b||$。线性方程的最小二乘解在代数意义上是可以解析地写出来的。
(a)在b ≠ 0 时，x 的解是什么形式？事实上，我们可以对A 求奇异值或对于$A^{T}A$ 求特征值。请阐述两者之间的关系。
答：

① 如果增广矩阵的阶梯型矩阵出现 0|常数，则方程组无解；
如果非零行个数r<方程组未知数的个数n，则方程组有无穷多个解
如果非零行个数r=方程组未知量的个数n，则方程组有唯一解，解就是化简的增广矩阵最后一列。
② 特征值和奇异值的关系：对于非奇异矩阵进行奇异值分解（SVD），得到的奇异值其实就是特征值。对于奇异矩阵，就需要进行奇异值分解，对应奇异值。对于奇异矩阵，计算$A^{T}A$得到一个方阵，再求特征值。

(b)当b = 0 时，请说明如何求非零解x。
答：

对于超定方程Ax = 0的解就是 $A^{T}A$ 最小特征值对应的特征向量。

©（开放问题）请谈谈你对上述解法在几何意义上的理解。

参考：

视觉SLAM理论与实践学习笔记（二）
深蓝学院视觉SLAM第二次习题
深蓝学院-视觉SLAM课程学习课后题
SLAM十四讲第二次作业-深蓝学院
Eigen官网
高翔等 《视觉SLAM十四讲》

深蓝学院-视觉SLAM十四讲-第二章作业
视觉slam学习笔记以及课后习题《第二讲三维物体刚体运动》

关于四元数的推荐：
四元数的可视化
四元数和三维转动，可互动的探索式视频