二 熟悉 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函数定义的序列。