一、 Eigen 矩阵运算

Eigen 是常用的 C++ 矩阵运算库，具有很高的运算效率。大部分需要在 C++ 中使用矩阵运算的库，都会选用 Eigen 作为基本代数库，例如 Google Tensorflow，Google Ceres，GTSAM 等。本次习题，你需要使用 Eigen 库，编写程序，求解一个线性方程组。为此，你需要先 了解一些有关线性方程组数值解法的原理。 设线性方程 Ax = b ，在 A为方阵的前提下，请回答以下问题：

1）在什么条件下，x 有解且唯一?

A 是可逆的, 有 A A^{-1} = E

2）高斯消元法的原理是什么？

原理是通过逐次消元后，再回代求解。

3）QR 分解的原理是什么？

QR（正交三角）分解法是求一般矩阵全部特征值的最有效并广泛应用的方法，一般矩阵先经过正交相似变化成为Hessenberg矩阵，然后再应用QR方法求特征值和特征向量。它是将矩阵分解成一个正规正交矩阵Q与上三角形矩阵R，所以称为QR分解法，与此正规正交矩阵的通用符号Q有关。

4）Cholesky 分解的原理是什么？

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

5）编程实现 A 为 100×100 随机矩阵时，用 QR 和 Cholesky 分解求 x 的程序。你可以参考本次课 用到的 useEigen 例程。

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

#define MATRIX_SIZE 100

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

    Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> matrix_100;//100*100的动态矩阵
    // 设置数据长度
    matrix_100.conservativeResize(MATRIX_SIZE, MATRIX_SIZE);
    
    matrix_100 = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);//随机产生
    matrix_100 = matrix_100.transpose() * matrix_100;//产生对称矩阵 

    cout<<"det[matrix_100] = " << matrix_100.determinant() <<endl;//计算行列式 

    // 求解 matrix_NN * x = v_Nd 这个方程
    Eigen::Matrix<double,Eigen::Dynamic,1> v_Nd;
    v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1);
    Eigen::Matrix<double,Eigen::Dynamic,1> x;

    x = matrix_100.colPivHouseholderQr().solve(v_Nd);//QR分解的结果 
    cout<<"QR: x = " <<x<<endl;
    
    x = matrix_100.llt().solve(v_Nd); //Cholesky分解的结果
    cout<<"Cholesky: x= "<< x <<endl;     
    return 0;
}

运行结果：

det[matrix_100] = 1.02855e+108
QR: x =  311.894
-262.248
   34.83
-421.497
...
Cholesky: x=  311.894
-262.248
   34.83
-421.497
...

二、几何运算练习

下面我们来练习如何使用 Eigen/Geometry 计算一个具体的例子。 设有小萝卜一号和小萝卜二号位于世界坐标系中。小萝卜一号的位姿为：q_1 = [0.55,0.3,0.2,0.2], t_1 = [0.7,1.1,0.2]^T(q 的第一项为实部)。这里的 q 和 t 表达的是 T_{cw}，也就是世界到相机的变换关系。小萝卜二号的位姿为 q_2 = [−0.1,0.3,−0.7,0.2]，t_2 = [−0.1,0.4,0.8]^\wedge。现在，小萝卜一号看到某个点在自身的坐标系下，坐标为 p_1 = [0.5,−0.1,0.2]^T，求该向量在小萝卜二号坐标系下的坐标。请编程实现此事，并提交你的程序。

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

#include <iostream> 
#include <Eigen/Core> 
#include <Eigen/Dense> 
#include <Eigen/Geometry> 
using namespace std; 

int main( int argc, char** argv ) {
    //小萝卜一号的位姿
    Eigen::Quaterniond q1(0.55,0.3,0.2,0.2);  
    q1 = q1.normalized();
    Eigen::Matrix<double,3,1> t1; 
    t1<<0.7,1.1,0.2;
    
    //小萝卜二号的位姿
    Eigen::Quaterniond q2(-0.1,0.3,-0.7,0.2); 
    q2 = q2.normalized();
    Eigen::Matrix<double,3,1> t2;
    t2<<-0.1,0.4,0.8;
    
    //小萝卜一号在自身的坐标系中看到某个点p1
    Eigen::Matrix<double,3,1> p1;
    p1<<0.5,-0.1,0.2; 
    
    Eigen::Matrix<double,3,1> pw, p2; //p_w为p点的世界坐标点，p_2为小萝卜二号坐标系下的坐标
    
    Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity(); 
    T1.rotate(q1.toRotationMatrix());
    T1.pretranslate(t1);
    pw = T1.colPivHouseholderQr().solve(p1);//QR分解的结果
    
    Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
    T2.rotate(q2.toRotationMatrix());
    T2.pretranslate(t2);
    p2 = T2*p1;
    cout<<"小萝卜二号坐标系下的坐标为,"<<p2<<endl;
    cout<<"参考答案为[1.08228,0.663509,0.686957]^T"<<endl;
    return 0; 
} 

运行结果

小萝卜二号坐标系下的坐标为, -0.298413
-0.0936508
  0.669841
参考答案为[1.08228,0.663509,0.686957]^T

三、旋转的表达

1.三维空间变换

F_0 的坐标系，上有一点a=[x,y,z]^T，则当F_0 坐标系开始旋转变化时，有如下的变换关系

• 绕X轴逆时针旋转\theta角
  R_x(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0& \cos\theta & -\sin\theta\\ 0 & \sin\theta & \cos\theta \end{bmatrix}

  经过变换后的新空间坐标点为
  a'=[x,y,z]^TR_x(\theta)

• 绕Y轴逆时针旋转\theta角
  R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0& 1 & 0\\ -\sin\theta & 0 & \cos\theta \end{bmatrix}

• 绕Z轴逆时针旋转\theta角
  R_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0\\ 0& 0 &1 \end{bmatrix}

则旋转矩阵为
M = R_x(\theta)R_y(\theta)R_z(\theta)

2. 旋转矩阵、旋转向量与四元数表达

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

1） 设有旋转矩阵 R，证明 R^TR = I 且 \det R = +I

设旋转矩阵R = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix}, 则 R^T = \begin{bmatrix} -\sin\theta &\cos\theta \\ \cos\theta & \sin\theta \end{bmatrix},

那么 R^TR=I，且\det R = + I

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

因为四元数q可以表示为s+xi+yj+zk, 其中，实部为 \eta = [s] 维度为1 \times 1; 而虚部为\varepsilon =[x,y,z]^T维度为3 \times 1 .

3） 定义运算 + 和 \oplus为：

q^+=\begin{bmatrix} \eta I+\varepsilon^\times &\varepsilon - \varepsilon^T & \eta \end{bmatrix}

q \oplus=\begin{bmatrix} \eta I-\varepsilon^\times &\varepsilon - \varepsilon^T & \eta \end{bmatrix}

其中运算 \times含义与 \wedge 相同，即取 \varepsilon 的反对称矩阵（它们都成叉积的矩阵运算形式）,I 为单位矩阵。请证明对任意单位四元数 q_1, q_2，四元数乘法可写成矩阵乘法：q_1q_2 = q_1^+q_2或者 q_1q_2 = q_2\oplus q_1

四、罗德里格斯公式的证明

罗德里格斯公式描述了从旋转向量到旋转矩阵的转换关系。设旋转向量长度为 \theta，方向为 n，那么旋 转矩阵 R 为：

R=\cos\theta I +(1-\cos\theta)nn^T+\sin\theta n^\wedge

1）假设该式成立，但没有给出证明。请证明此式。提示：参考。

2）请使用此式证明R^{−1} = R^T。

1）证：

由李群李代数可知R = \exp(\phi^\wedge)，其中\phi 是三维向量，我们可以定义它的模长为a，方向为\theta, 于是有 \phi = \theta a, 这里a^\wedge有两个性质，分别为 a^\wedge a^\wedge = aa^T - I 和 a^\wedge a^\wedge a^\wedge = -a^\wedge。

根据泰勒展开有

\begin{align*} \exp(\phi ^{\wedge }) &= \exp(\theta a ^{\wedge}) = \sum^{\infty }_{n=0}(\theta a ^{\wedge})^n \\ & = I + \theta a ^{\wedge} + \frac{1}{2!}\theta ^{2} a^{\wedge}a^{\wedge}+\frac{1}{3!}\theta ^{3} (a^{\wedge})^3+\frac{1}{4!}\theta ^{4} (a^{\wedge})^4 + \cdots \\ & = aa^T - a^{\wedge}a^{\wedge}+ \theta a^{\wedge}+\frac{1}{2!}\theta ^{2} a^{\wedge}a^{\wedge} - \frac{1}{3!}\theta ^{3} a^{\wedge}- \frac{1}{4!}\theta ^{4} (a^{\wedge})^2+ \cdots \\ & = aa^T + (\theta - \frac{1}{3!}\theta^3 + \frac{1}{5!}\theta^5 - \cdots)a^{\wedge}-(1-\frac{1}{2!}\theta^2 + \frac{1}{4!}\theta^4 - \cdots)a^{\wedge}a^{\wedge} \\ & = a^{\wedge}a^{\wedge}+ I + \sin \theta a^{\wedge}- \cos \theta a^{\wedge}a^{\wedge} \\ & = (1- \cos \theta)a^{\wedge}a^{\wedge} + I + \sin \theta a^{\wedge} \\ & = \cos \theta I + (1-\cos \theta)aa^T + \sin \theta a^{\wedge} \end{align*}

则 \exp(\phi^\wedge) = \cos \theta I + (1−\cos\theta )aa^T +\sin\theta a^\wedge,

所以R = \cos \theta I + (1−\cos\theta)nn^T +\sin \theta n^\wedge 得证。

2） 假设某个单位正交基 (e_1,e_2,e_3) 经过一次旋转变成了 (e_1',e_2', e_3') ，则对同一个向量\overrightarrow{a}，在两个坐标系下的坐标分别为[a_1,a_2,a_3]^T和[a_1', a_2', a_3']^T ，则有：

(e_1,e_2,e_3)[a_1,a_2,a_3]^T = (e_1',e_2', e_3')[a_1', a_2', a_3']^T

对上述等式的左右两边同时左乘[e_1^T, e_2^T, e_3^T]^T，则左边的系数就化为单位矩阵，即

\begin{bmatrix} a_1\\ a_2\\ a_3 \end{bmatrix} = \begin{bmatrix} e_1^Te_1' & e_1^Te_2' & e_1^Te_3'\\ e_2^Te_1' & e_2 ^Te_2' & e_2^Te_3'\\ e_3^Te_1' & e_3^Te_2' & e_3^Te_3' \end{bmatrix}\begin{bmatrix} a_1'\\ a_2'\\ a_3' \end{bmatrix}= Ra'

所以可得 a'=R^{-1}a=R^Ta， R^{−1} = R^T 得证。

五、四元数运算性质的验证

课程中介绍了单位四元数可以表达旋转。其中，在谈论用四元数 q 旋转点 p 时，结果为 p′ = qpq−1 。

我们说，此时 p′ 必定为虚四元数（实部为零）。请你验证上述说法。

此外，上式亦可写成矩阵运算：p′ = Qp。请根据你的推导，给出矩阵 Q。注意此时 p 和 p′ 都是四元数形式的变量，所以 Q 为 4 \times 4 的矩阵。

提示：如果使用第 3 题结果，那么有：
p^′ = qpq^{−1} = q^{+}p^{+}q^{−1} = q^{+}q^{−1}\oplus p

从而可以导出四元数至旋转矩阵的转换方式：

\R = Im(q^+q^{−1}\oplus)

其中 Im 指取出虚部的内容。

证：

假设

q =\begin{bmatrix} \varepsilon \\ \eta \end{bmatrix} , q^{-1} =\begin{bmatrix} -\varepsilon \\ \eta \end{bmatrix} , 令 p =\begin{bmatrix} r_b \\ 0 \end{bmatrix}, p^{-1} =\begin{bmatrix} r_b' \\ 0 \end{bmatrix}

因为

p′ = qpq^{−1} = q^+p^+q^{−1} = q^+q^{−1}\oplus p \\ \ \ \ =\begin{bmatrix} \eta I + \varepsilon^\times & \varepsilon \\ −\varepsilon T & \eta \end{bmatrix}\begin{bmatrix} \eta I - \varepsilon^\times & \varepsilon\\ −\varepsilon^T & \eta \end{bmatrix}\begin{bmatrix} r_b \\ 0 \end{bmatrix} \\ \ \ \ =\begin{bmatrix} -(\eta I - \varepsilon^\times)^2 & 0 \\ 0 & -1 \end{bmatrix}\begin{bmatrix} r_b \\ 0 \end{bmatrix}

所以可以得到

r_b' = (-(\eta I - \varepsilon^×)^2 - \varepsilon\varepsilon^T)r_b

R = -(\eta I - \varepsilon^\times )^2 - \varepsilon\varepsilon^T

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