自学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 <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) 
{//建立一个动态矩阵AMatrixXd A;//建立一个100*100的随机矩阵A                                      A = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE); //保证A为正定矩阵，A=A*A的转置（正定）A = A * A.transpose();                     //建立一个float类型的动态矩阵BMatrix<double, Dynamic, 1> B;   //建立一个100*1的随机矩阵BB = MatrixXd::Random(MATRIX_SIZE, 1);   //建立一个动态矩阵XMatrix<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 <iostream>
using namespace std;
#include <ctime>
// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>#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<double, 3, 1>，即三维向量Eigen::Vector3d v_3d;// 这是一样的Eigen::Matrix<float,3,1> vd_3d;// Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>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<double, 2, 1> 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<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * 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 <iostream>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;
int main(int argc, char ** argv)
{//创建小萝卜1和2的位姿q1和q2Quaterniond 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和t2Vector3d t1(0.7, 1.1, 0.2), t2(-0.1, 0.4, 0.8);//创建p1坐标Vector3d p1(0.5, -0.1, 0.2);  //欧式变换矩阵Tc1w和Tc2wIsometry3d Tc1w(q1), Tc2w(q2);Tc1w.pretranslate(t1);// 把平移向量设成(t1)Tc2w.pretranslate(t2);// 把平移向量设成(t2)//计算p2Vector3d p2 = Tc2w*Tc1w.inverse()*p1;cout << p2.transpose() << endl;return 0;
}

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

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

#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
// Eigen 几何模块
#include <Eigen/Geometry>/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/int main ( int argc, char** argv )
{// Eigen/Geometry 模块提供了各种旋转和平移的表示// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3fEigen::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::IsometryEigen::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+tcout<<"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 <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表达式等内容。

该程序中使用了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<a2.index;是函数体，返回值是布尔型的大小比较结果。

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