自学SLAM（4）《第二讲：三维物体刚体运动》作业

前言

小编研究生的研究方向是视觉SLAM，目前在自学，本篇文章为初学高翔老师课的第二次作业。

---

1.熟悉 Eigen 矩阵运算

设线性⽅程 Ax = b，在 A 为⽅阵的前提下，请回答以下问题：

1. 在什么条件下， x 有解且唯⼀？
2. ⾼斯消元法的原理是什么？
3. QR 分解的原理是什么？
4. Cholesky 分解的原理是什么？
5. 编程实现 A 为 100 × 100 随机矩阵时，⽤ QR 和 Cholesky 分解求 x 的程序。你可以参考本次课⽤到的 useEigen 例程。

1.当r(A)=r([A|b])=n时，也就是A满秩，A可逆，方程组有唯一解。

2.高斯消元法如图所示：

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

其实这题就是让我们自己建立一个AX=B，利用Eigen的内置函数求解X
代码如下：

#include 
using namespace std;
#include 
#include 
#include 
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的转置（正定）
    A = A * A.transpose();                     
    //建立一个float类型的动态矩阵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的语法规则，及其用法参考如下：

#include 
using namespace std;
#include 
// Eigen 部分
#include 
// 稠密矩阵的代数运算（逆，特征值等）
#include 

#define MATRIX_SIZE 50

/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/

int main( int argc, char** argv )
{
    // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
    // 声明一个2*3的float矩阵
    Eigen::Matrix<float, 2, 3> matrix_23;

    // 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
    // 例如 Vector3d 实质上是 Eigen::Matrix，即三维向量
    Eigen::Vector3d v_3d;
	// 这是一样的
    Eigen::Matrix<float,3,1> vd_3d;

    // Matrix3d 实质上是 Eigen::Matrix
    Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零
    // 如果不确定矩阵大小，可以使用动态大小的矩阵
    Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
    // 更简单的
    Eigen::MatrixXd matrix_x;
    // 这种类型还有很多，我们不一一列举

    // 下面是对Eigen阵的操作
    // 输入数据（初始化）
    matrix_23 << 1, 2, 3, 4, 5, 6;
    // 输出
    cout << matrix_23 << endl;

    // 用()访问矩阵中的元素
    for (int i=0; i<2; i++) {
        for (int j=0; j<3; j++)
            cout<<matrix_23(i,j)<<"\t";
        cout<<endl;
    }

    // 矩阵和向量相乘（实际上仍是矩阵和矩阵）
    v_3d << 3, 2, 1;
    vd_3d << 4,5,6;
    // 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
    // Eigen::Matrix result_wrong_type = matrix_23 * v_3d;
    // 应该显式转换
    Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
    cout << result << endl;

    Eigen::Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
    cout << result2 << endl;

    // 同样你不能搞错矩阵的维度
    // 试着取消下面的注释，看看Eigen会报什么错
    // Eigen::Matrix result_wrong_dimension = matrix_23.cast() * v_3d;

    // 一些矩阵运算
    // 四则运算就不演示了，直接用+-*/即可。
    matrix_33 = Eigen::Matrix3d::Random();      // 随机数矩阵
    cout << matrix_33 << endl << endl;

    cout << matrix_33.transpose() << endl;      // 转置
    cout << matrix_33.sum() << endl;            // 各元素和
    cout << matrix_33.trace() << endl;          // 迹
    cout << 10*matrix_33 << endl;               // 数乘
    cout << matrix_33.inverse() << endl;        // 逆
    cout << matrix_33.determinant() << endl;    // 行列式

    // 特征值
    // 实对称矩阵可以保证对角化成功
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33.transpose()*matrix_33 );
    cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
    cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

    // 解方程
    // 我们求解 matrix_NN * x = v_Nd 这个方程
    // N的大小在前边的宏里定义，它由随机数生成
    // 直接求逆自然是最直接的，但是求逆运算量大

    Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > matrix_NN;
    matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
    Eigen::Matrix< double, MATRIX_SIZE,  1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 );

    clock_t time_stt = clock(); // 计时
    // 直接求逆
    Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
    cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;
    
	// 通常用矩阵分解来求，例如QR分解，速度会快很多
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout <<"time use in Qr decomposition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

    return 0;
}

2.几何运算练习

下⾯我们来练习如何使⽤ 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，求该向量在⼩萝⼘⼆号坐标系下的坐标。请编程实现此事，并提交你的程序。
提⽰：

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

#include 
using namespace std;
#include 
#include 
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);
    //四元数归一化(高博的SLAM视屏中的代码解释没有归一化)
    q1.normalize();
    q2.normalize();
    //创建小萝卜1和2的另一个位姿量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);// 把平移向量设成(t1)
    Tc2w.pretranslate(t2);// 把平移向量设成(t2)
    //计算p2
    Vector3d p2 = Tc2w*Tc1w.inverse()*p1;
    cout << p2.transpose() << endl;
    
    return 0;
}

运行结果如下，我们可以看到答案正确：

Eigen/四元数的语法规则，及其用法参考如下：

#include 
#include 
using namespace std;
#include 
// Eigen 几何模块
#include 

/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/

int main ( int argc, char** argv )
{
    // Eigen/Geometry 模块提供了各种旋转和平移的表示
    // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
    Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
    // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
    Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) );     //沿 Z 轴旋转 45 度
    cout .precision(3);
    cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl;                //用matrix()转换成矩阵
    // 也可以直接赋值
    rotation_matrix = rotation_vector.toRotationMatrix();
    // 用 AngleAxis 可以进行坐标变换
    Eigen::Vector3d v ( 1,0,0 );
    Eigen::Vector3d v_rotated = rotation_vector * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
    // 或者用旋转矩阵
    v_rotated = rotation_matrix * v;
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
    Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序
    cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

    // 欧氏变换矩阵使用 Eigen::Isometry
    Eigen::Isometry3d T=Eigen::Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
    T.rotate ( rotation_vector );                                     // 按照rotation_vector进行旋转
    T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) );                     // 把平移向量设成(1,3,4)
    cout << "Transform matrix = \n" << T.matrix() <<endl;

    // 用变换矩阵进行坐标变换
    Eigen::Vector3d v_transformed = T*v;                              // 相当于R*v+t
    cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

    // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

    // 四元数
    // 可以直接把AngleAxis赋值给四元数，反之亦然
    Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
    // 也可以把旋转矩阵赋给它
    q = Eigen::Quaterniond ( rotation_matrix );
    cout<<"quaternion = \n"<<q.coeffs() <<endl;
    // 使用四元数旋转一个向量，使用重载的乘法即可
    v_rotated = q*v; // 注意数学上是qvq^{-1}
    cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

    return 0;
}

3.旋转的表达

1. 设有旋转矩阵 R，证明 RT R = I 且 det R = +1²。

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

四元数q有三个虚部和一个实部。
即q=q₀+q₁i+q₂j+q₃k
因此ε的维度为3， η的维度为1。

3.第三问

4.罗德里格斯公式的证明

罗德⾥格斯公式描述了从旋转向量到旋转矩阵的转换关系。设旋转向量长度为 θ，⽅向为 n，那么旋转矩阵 R 为：
R = cos θI − (1 − cos θ)nn^T + sin θn^. ------------------------------------------------------------(4)
我们在课程中仅指出了该式成⽴，但没有给出证明。请你证明此式。

参考：
链接: 罗德里格斯公式的证明

5.四元数运算性质的验证

6.熟悉 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 
2#include 
3#include 
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表达式等内容。

该程序中使用了C++11标准的以下内容：

iostream：使用了C++11中引入的iostream库，用于输入输出流操作。

vector：使用了C++11中引入的vector容器，用于存储和操作A类的实例。

algorithm：使用了C++11中引入的algorithm库，其中的sort函数用于对vector容器中的元素进行排序。

using namespace std;：使用了C++11中引入的namespace别名声明，用于简化对std命名空间的使用。

class A：使用了C++11中引入的类初始化列表语法，用于对A类的成员变量进行初始化。

vector avec{a1, a2, a3}; ：使用了C++11中引入的列表初始化语法，用于初始化vector容器avec并添加元素。

[] (const A&a1, const A&a2) { return a1.index < a2.index; }：使用了C++11中引入的lambda表达式，用作sort函数的排序准则。其中：const A&a1, const A&a2是参数列表，return a1.index

for (auto& a : avec)：使用了C++11中引入的范围for循环语法，用于遍历vector容器avec中的元素。用auto关键字实现了自动类型推导，让编译器自动设置变量a的类型；C++引入了基于范围的for循环，不用下标就能访问元素；