视觉SLAM理论与实践
二 熟悉 Eigen 矩阵运算
1. 在什么条件下， x 有解且唯⼀？
2. ⾼斯消元法的原理是什么？
3. QR 分解的原理是什么？
4. Cholesky 分解的原理是什么？
5. 编程实现 A 为 100 × 100 随机矩阵时，⽤ QR 和 Cholesky 分解求 x 的程序。你可以参考本次课⽤到的 useEigen 例程。
三 几何运算练习
四 旋转的表达
1. 设有旋转矩阵 R，证明 RTR = I 且 det R = +1。(若⾏列式为-1，通常称为瑕旋转（inproper rotation，对应物理当中旋转 + 镜像）。 det R = +1 主要由定义给出。)
2. 设有四元数q，我们把虚部记为ε，实部记为 η，那么 q = (ε; η)。请说明ε和 η 的维度。
3. 定义运算 + 和 ⊕ 为：
五 罗德里格斯公式的证明
六 四元数运算性质的验证
七 * 熟悉 C++11
二 熟悉 Eigen 矩阵运算
Eigen（http://eigen.tuxfamily.org）是常⽤的 C++ 矩阵运算库，具有很⾼的运算效率。⼤部分需要在 C++ 中使⽤矩阵运算的库，都会选⽤ Eigen 作为基本代数库，例如 Google Tensorflow， GoogleCeres， GTSAM 等。本次习题，你需要使⽤ Eigen 库，编写程序，求解⼀个线性⽅程组。为此，你需要先了解⼀些有关线性⽅程组数值解法的原理。设线性⽅程 Ax = b，在 A 为⽅阵的前提下，请回答以下问题：

1. 在什么条件下， x 有解且唯⼀？
当R(A)=R(A|B)时方程组有解，当R(A)=R(A,B)=n（n为未知数个数）时方程组有唯一解。

2. ⾼斯消元法的原理是什么？
高斯消元法是线性代数中的一种算法，可用于求解线性方程组的解，以及求出矩阵的秩，和求出可逆矩阵的逆矩阵。高斯消元法的原理是：通过用初等行变换将增广矩阵化为行阶梯阵，然后通过回带求解线性方程组的解。

3. QR 分解的原理是什么？
QR方法是用于求解矩阵所有特征值的算法，QR分解就是把矩阵分解成一个正交矩阵和一个上三角矩阵。

4. Cholesky 分解的原理是什么？
Cholesky 分解是把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解。它要求矩阵的所有特征值必须大于零，故分解的下三角的对角元也是大于零的。Cholesky分解法又称平方根法，是当A为实对称正定矩阵时，LU三角分解法的变形。

5. 编程实现 A 为 100 × 100 随机矩阵时，⽤ QR 和 Cholesky 分解求 x 的程序。你可以参考本次课⽤到的 useEigen 例程。
提⽰：你可能需要参考相关的数学书籍或⽂章。请善⽤搜索引擎。 Eigen 固定⼤⼩矩阵最⼤⽀持到 50，所以你会⽤到动态⼤⼩的矩阵。
test1.cpp:

#include <iostream>

using namespace std;

#include <ctime>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;

#define MATRIX_SIZE 100

int main(int argc, char **argv) {
    
    //建立一个动态矩阵A
    MatrixXd A;
    //建立一个100*100的随机矩阵A                                      
    A = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE); 

    //保证A为正定矩阵
    A = A * A.transpose();    
                       
    //建立一个动态矩阵B
    Matrix<double, Dynamic, 1> B;   
    //建立一个100*1的随机矩阵B
    B = MatrixXd::Random(MATRIX_SIZE, 1); 
          
    //建立一个动态矩阵X
    Matrix<double, Dynamic, 1> X;
    //建立一个100*1的随机矩阵X                    
    X = MatrixXd::Random(MATRIX_SIZE, 1);            
    
    //QR分解
    X = A.colPivHouseholderQr().solve(B);            
    cout << "QR: X = " << X.transpose() << endl;

    //cholesky分解
    X = A.ldlt().solve(B);    
                       
    cout << "cholesky: X = " << X.transpose() << endl;
    
    return 0;
}

三 几何运算练习
下⾯我们来练习如何使⽤ Eigen/Geometry 计算⼀个具体的例⼦。
设有⼩萝⼘ 1⼀号和⼩萝⼘⼆号位于世界坐标系中。⼩萝⼘⼀号的位姿为： q1 = [0:55; 0:3; 0:2; 0:2]; t1 =[0:7; 1:1; 0:2]T（q 的第⼀项为实部）。这⾥的 q 和 t 表达的是 Tcw，也就是世界到相机的变换关系。⼩萝⼘⼆号的位姿为 q2 = [−0:1; 0:3; −0:7; 0:2]; t2 = [−0:1; 0:4; 0:8]T。现在，⼩萝⼘⼀号看到某个点在⾃⾝的坐标系下，坐标为 p1 = [0:5; −0:1; 0:2]T，求该向量在⼩萝⼘⼆号坐标系下的坐标。请编程实现此事，并提交你的程序。
提⽰：

四元数在使⽤前需要归⼀化。
请注意 Eigen 在使⽤四元数时的虚部和实部顺序。
参考答案为 p2 = [1:08228; 0:663509; 0:686957]T。你可以⽤它验证程序是否正确。
test2.cpp:

#include <iostream>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace Eigen;

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

    //创建小萝卜1和2的位姿q1和q2
    Quaterniond q1(0.55, 0.3, 0.2, 0.2), q2(-0.1, 0.3, -0.7, 0.2);
    //四元数归一化
    q1.normalize();
    q2.normalize();
    
    //平移向量t1和t2
    Vector3d t1(0.7, 1.1, 0.2), t2(-0.1, 0.4, 0.8);
    //p1坐标
    Vector3d p1(0.5, -0.1, 0.2);  
    
    //变换矩阵Tc1w和Tc2w
    Isometry3d Tc1w(q1), Tc2w(q2);
    Tc1w.pretranslate(t1);
    Tc2w.pretranslate(t2);
    
    //计算p2
    Vector3d p2 = Tc2w*Tc1w.inverse()*p1;
    cout << p2.transpose() << endl;
    
    return 0;
}

输出结果与答案一致：

四 旋转的表达

课程中提到了旋转可以⽤旋转矩阵、旋转向量与四元数表达，其中旋转矩阵与四元数是⽇常应⽤中常见的表达⽅式。请根据课件知识，完成下述内容的证明。

1. 设有旋转矩阵 R，证明 RTR = I 且 det R = +1。(若⾏列式为-1，通常称为瑕旋转（inproper rotation，对应物理当中旋转 + 镜像）。 det R = +1 主要由定义给出。)

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

答：ε的维度是3，的维度是1。

3. 定义运算 + 和 ⊕ 为：

五 罗德里格斯公式的证明
罗德⾥格斯公式描述了从旋转向量到旋转矩阵的转换关系。设旋转向量长度为 θ，⽅向为 n，那么旋转矩阵 R 为：

我们在课程中仅指出了该式成⽴，但没有给出证明。请你证明此式。提⽰：参考https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula。

六 四元数运算性质的验证

七 * 熟悉 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 的语法下，程序写成：

#include <iostream> 
#include <vector> 
#include <algorithm>

using namespace std;

class A {
public:
    A(const int& i ) : index(i) {};
    int index = 0;
};

int main() {
    A a1(3), a2(5), a3(9);
    vector<A> avec{a1, a2, a3};
    std::sort(avec.begin(), avec.end(), [](const A&a1, const A&a2) {return a1.index<a2.index;}); 
    for ( auto& a: avec ) cout<<a.index<<" ";
    cout<<endl;
    return 0;
}

（1）第15行：使用了初始化列表来初始化对象。
（2）第16行：使用了lambda表达式来比较元素大小，其中const A&a1, const A&a2是参数列表，return a1.index<a2.index;是函数体，返回值是布尔型的大小比较结果。
（3）第17行：用auto关键字实现了自动类型推导。auto的自动类型推导，用于从初始化表达式中推断出变量的数据类型。
（4）第17行：使用了序列 for循环，可以用于遍历数组，容器，string以及由begin和end函数定义的序列。