《视觉SLAM十四讲 第二版》笔记及课后习题（第三讲）

读书笔记：三维空间刚体运动

本讲介绍视觉 SLAM 的基本问题之一：一个刚体在三维空间中的运动是如何描述的。我们当然知道这由一次旋转加一次平移组成。平移确实没有太大问题，但旋转的处理是件麻烦事。我们将介绍旋转矩阵、四元数、欧拉角的意义,以及它们是如何运算和转换的。

旋转矩阵

一些线性代数的基本知识。
坐标系：

坐标变化：

变换矩阵：

这是一个数学技巧:我们把一个三维向量的末尾添加 1,变成了四维向量,称为齐次坐标。对于这个四维向量,我们可以把旋转和平移写在一个矩阵里面，使得整个关系变成了线性关系。该式中，矩阵 T 称为变换矩阵(Transform Matrix)。我们暂时用 ã 表示 a 的齐次坐标。

关于变换矩阵 T ,它具有比较特别的结构:左上角为旋转矩阵,右侧为平移向量,左下角为 0 向量,右下角为 1。这种矩阵又称为特殊欧氏群(Special Euclidean Group):

旋转向量和欧拉角

任意旋转都可以用一个旋转轴和一个旋转角来刻画。于是,我们可以使用一个向量,其方向与旋转轴一致,而长度等于旋转角。这种向量,称为旋转向量(或轴角, Axis-Angle)。这种表示法只需一个三维向量即可描述旋转。同样,对于变换矩阵,我们使用一个旋转向量和一个平移向量即可表达一次变换。这时的维数正好是六维。由旋转向量到旋转矩阵的过程由罗德里格斯公式(Rodrigues’s Formula )表明，由于推导过程比较复杂,我们不作描述，只给出转换的结果：

符号∧是向量到反对称的转换符，反之,我们也可以计算从一个旋转矩阵到旋转向量的转换。对于转角 θ，有：
关于转轴 n,由于旋转轴上的向量在旋转后不发生改变,说明

因此,转轴 n 是矩阵 R 特征值 1 对应的特征向量。求解此方程,再归一化,就得到了旋转轴。读者也可以从“旋转轴经过旋转之后不变”的几何角度看待这个方程。仍然剧透几句,这里的两个转换公式在下一章仍将出现,你会发现它们正是 SO(3) 上李群与李代数的对应关系。

欧拉角

ZY X 转角相当于把任意旋转分解成以下三个轴上的转角：

1. 绕物体的 Z 轴旋转,得到偏航角 yaw；
2. 绕旋转之后的 Y 轴旋转,得到俯仰角 pitch；
3. 绕旋转之后的 X 轴旋转,得到滚转角 roll。

此时,我们可以使用 [r, p, y] T 这样一个三维的向量描述任意旋转。这个向量十分的直观,我们可以从这个向量想象出旋转的过程。其他的欧拉角亦是通过这种方式,把旋转分解到三个轴上,得到一个三维的向量,只不过选用的轴,以及选用的顺序不一样。这里介绍的 rpy 角是比较常用的一种,只有很少的欧拉角种类会有 rpy 那样脍炙人口的名字。
欧拉角的一个重大缺点是会碰到著名的万向锁问题(Gimbal Lock)：
有关万向锁的更多详解

四元数

四元数是 Hamilton 找到的一种扩展的复数.。它既是紧凑的，也没有奇异性。如果说缺点的话，四
元数不够直观，其运算稍为复杂一些。有关四元数的部分，推荐直接看3Blue1Brown大神的四元数的可视化。我们只要掌握四元数的基本概念以及四元数到旋转矩阵的转换就行了，我们省略过程中的推导,直接给出四元数到旋转矩阵的转换方式。

课后习题

1. 验证旋转矩阵是正交矩阵。

直接看这一篇的证明

2. * 寻找罗德里格斯公式的推导过程并理解它。

翻译自*
假设v是R3中的一个向量，并且k是一个描述旋转轴的单位向量，v根据右手规则旋转角度θ，则Rodrigues公式为：
另一种说法是将轴向量作为定义旋转平面的任意两个非零向量a和b的交叉乘积a×b，并且角度θ的方向是从a向b测量的。令α表示这些向量之间的角度，两个角度θ和α不需要等同，但是它们的测量方式相同。 然后可以单位轴向量可以写为

当涉及定义平面的两个向量时，这种形式可能更有用。物理学中的一个例子是托马斯岁差，它包括由罗德里格斯公式给出的旋转，用两个非共线助推速度表示，旋转轴垂直于它们的平面。

求导

令k是定义旋转轴的单位矢量，令v是以角度θ围绕k旋转的任意向量（右手定则，图中逆时针）。

使用点乘和叉乘，向量v可以分解成与轴k平行和垂直的分量，

与k平行的分量是

称为v在k上的向量投影，垂直于k的分量为

称为k从v的向量拒绝。

矢量k×v可以看作是v⊥逆时针旋转90°左右的副本，因为它们的大小相等，但是方向是垂直的。同样，向量k×(k×v），v⊥的一个副本逆时针旋转180°到k左右，使得k×（k×v）和v⊥的大小相等，但方向相反（即它们是互为负数，因此减号）。扩展矢量三重乘积建立了平行分量和垂直分量之间的连接，参考公式为a×（b×c）=（a·c）b - （a·b）c 给定任意三个向量a，b，c。

平行于轴的分量在旋转下不会改变幅度和方向，

根据以上分析，只有垂直分量才会改变方向，但保持其大小

并且由于k和v||是平行的，所以它们的叉积是零 k×v|| = 0，因此

并且

这种旋转是正确的，因为矢量v⊥和k×v具有相同的长度，并且k×v是v⊥围绕k逆时针旋转90°。使用三角函数正弦和余弦对v⊥和k×v进行适当的缩放给出旋转的垂直分量。旋转分量的形式类似于笛卡尔基的2D平面极坐标（r，θ）中的径向向量

其中ex，ey是它们指示方向上的单位向量。

现在完整的旋转矢量是

用等式结果中的v ||rot和v⊥rot的定义代替


罗德里格斯（Rodrigues）的旋转公式通过将v分解为与k平行且垂直的分量，并仅旋转垂直分量，使v围绕矢量k旋转角度θ。

Rodrigues旋转公式的矢量几何，以及分解为平行和垂直分量。

矩阵表示

将v和k×v表示为列矩阵，叉积可以表示为矩阵乘积

令K表示单位向量k的“叉积矩阵

矩阵方程可以表示为

对于任何向量v（实际上，K是具有这个性质的独特矩阵，它具有特征值0和±i）。

迭代右边的交叉乘积相当于乘以左边的交叉乘积矩阵，特别是

而且，由于k是单位向量，所以K具有单位2-范数。 因此矩阵语言中的上一个旋转公式是

请注意，在这个表示法中，主要术语的系数现在是1。

将v分解可以实现紧凑表达式

这里

是围绕轴k逆时针旋转角度θ的旋转矩阵，I是3×3单位矩阵。矩阵R是旋转群SO（3）的一个元素，K是李代数的一个元素，所以so(3)生成李群（注意K是负对称的，这表征了so(3)的特性）。 就矩阵指数而言，

为了理解最后的恒等式，要注意到

单参数子群的特征，即指数，并且公式与无穷小θ相匹配。

对于基于这种指数关系的替代推导，请参阅从so（3）到SO（3）的指数映射。 对于逆映射，请参见从SO（3）到so（3）的日志映射。

3. 验证四元数旋转某个点后,结果是一个虚四元数(实部为零),所以仍然对应到一个三维空间点(式 3.34)

参见这篇博客

4. 画表总结旋转矩阵、轴角、欧拉角、四元数的转换关系。

5. 假设我有一个大的 Eigen 矩阵,我想把它的左上角 3 × 3 的块取出来,然后赋值为I_3×3 。请编程实现此事。

提取大矩阵左上角3x3矩阵，有两种方式:
1、直接从0-2循环遍历大矩阵的前三行和三列
2、用矩阵变量.block(0,0,3,3)//从左上角00位置开始取3行3列

具体代码实现:

#include<iostream>

//包含Eigen头文件
#include<Eigen/Core>
#include<Eigen/Geometry>

#define MATRIX_SIZE 30
using namespace std;

int main(int argc,char **argv)
{
    //设置输出小数点后3位
    cout.precision(3);
    Eigen::Matrix<double,MATRIX_SIZE, MATRIX_SIZE> matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    Eigen::Matrix<double,3,3>matrix_3d1 = Eigen::MatrixXd::Random(3,3);//3x3矩阵变量
    Eigen::Matrix3d matrix_3d = Eigen::Matrix3d::Random();//两种方式都可以
/*方法1：循环遍历矩阵的三行三列   */
    for(int i = 0;i < 3; i ++){
        for(int j = 0 ;j < 3;j++){
            matrix_3d(i,j) = matrix_NN(i,j);
            cout<<matrix_NN(i,j)<<" ";
        }
              cout<<endl;
    }
    matrix_3d = Eigen::Matrix3d::Identity();
    cout<<"赋值后的矩阵为:"<<matrix_3d<<endl;

/*方法2：用.block函数   */
/*
    cout<<"提取出来的矩阵块为:"<<endl;
    cout<< matrix_NN.block(0,0,3,3)    <<endl;

    //提取后赋值为新的元素
    matrix_3d = matrix_NN.block(0,0,3,3);
    matrix_3d = Eigen::Matrix3d::Identity();
    cout<<"赋值后的矩阵为:"<<endl<<matrix_3d;
*/
    return 0;
}

6. * 一般线程方程 Ax = b 有哪几种做法?你能在 Eigen 中实现吗?

转自这位优秀的同学！用了6种方法，太秀了！
线性方程组Ax = b的解法 :
1、直接法：(1,2,3,4,5）
2、迭代法:如Jacobi迭代法(6)
其中只有2 3方法不要求方程组个数与变量个数相等

下面简略说明下Jacobi迭代算法:
由迭代法求解线性方程组的基本思想是将联立方程组的求解归结为重复计算一组彼此独立的线性表达式，这就使问题得到了简化，类似简单迭代法转换方程组中每个方程式可得到雅可比迭代式
迭代法求解方程组有一定的局限性，比如下面Jacobi函数程序实现的过程中，会判断迭代矩阵的谱半径是不是小于1，如果小于1表示Jacobi迭代法收敛，如果求不出来迭代矩阵即D矩阵不可逆的话，无法通过收敛的充要条件来判断是不是收敛，此时可以试着迭代多次，看看输出结果是否收敛，此时Jacobi迭代算法并不一定收敛，只能试着做下，下面的程序实现过程仅仅处理了充要条件，迭代法同时有十分明显的优点——算法简单，因而编制程序比较容易，所以在实际求解问题中仍有非常大利用价值。

具体代码实现 如下：

#include<iostream>
#include<ctime>
#include <cmath>
#include <complex>
/*线性方程组Ax = b的解法(直接法（1,2,3,4,5）+迭代法(6))其中只有2 3方法不要求方程组个数与变量个数相等*/

//包含Eigen头文件
//#include <Eigen/Dense>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include <Eigen/Eigenvalues>

//下面这两个宏的数值一样的时候 方法1 4 5 6才能正常工作
#define MATRIX_SIZE 3   //方程组个数
#define MATRIX_SIZE_ 3  //变量个数
//using namespace std;
typedef  Eigen::Matrix<double,MATRIX_SIZE, MATRIX_SIZE_>  Mat_A;
typedef  Eigen::Matrix<double ,MATRIX_SIZE,1 >              Mat_B;

//Jacobi迭代法的一步求和计算
double Jacobi_sum(Mat_A   &A,Mat_B   &x_k,int i);

//迭代不收敛的话 解向量是0
Mat_B Jacobi(Mat_A   &A,Mat_B   &b,  int &iteration_num, double &accuracy );

int main(int argc,char **argv)
{
    //设置输出小数点后3位
    std::cout.precision(3);
    //设置变量
    Eigen::Matrix<double,MATRIX_SIZE, MATRIX_SIZE_> matrix_NN = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE_);
    Eigen::Matrix<double ,MATRIX_SIZE,1 > v_Nd = Eigen::MatrixXd::Random(MATRIX_SIZE,1);

    //测试用例
    matrix_NN << 10,3,1,2,-10,3,1,3,10;
    v_Nd <<14,-5,14;

    //设置解变量
    Eigen::Matrix<double,MATRIX_SIZE_,1>x;

    //时间变量
    clock_t tim_stt = clock();

/*1、求逆法	很可能没有解 仅仅针对方阵才能计算*/
#if (MATRIX_SIZE == MATRIX_SIZE_)
    x = matrix_NN.inverse() * v_Nd;
    std::cout<<"直接法所用时间和解为："<< 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC
        <<"MS"<< std::endl << x.transpose() << std::endl;
#else
    std::cout<<"直接法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endif

/*2、QR分解解方程组	适合非方阵和方阵 当方程组有解时的出的是真解，若方程组无解得出的是近似解*/
    tim_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    std::cout<<"QR分解所用时间和解为："<<1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC
        << "MS" << std::endl << x.transpose() << std::endl;

/*3、最小二乘法	适合非方阵和方阵，方程组有解时得出真解，否则是最小二乘解(在求解过程中可以用QR分解 分解最小二成的系数矩阵) */
    tim_stt = clock();
    x = (matrix_NN.transpose() * matrix_NN ).inverse() * (matrix_NN.transpose() * v_Nd);
    std::cout<<"最小二乘法所用时间和解为:"<< 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC
        << "MS" << std::endl  << x.transpose() << std::endl;

/*4、LU分解方法	只能为方阵（满足分解的条件才行）    */
#if (MATRIX_SIZE == MATRIX_SIZE_)
    tim_stt = clock();
    x = matrix_NN.lu().solve(v_Nd);
    std::cout<< "LU分解方法所用时间和解为:" << 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC
        << "MS" << std::endl << x.transpose() << std::endl;
#else
    std::cout<<"LU分解法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endif

/*5、Cholesky	分解方法  只能为方阵 (结果与其他的方法差好多)*/
#if (MATRIX_SIZE == MATRIX_SIZE_)
    tim_stt = clock();
    x = matrix_NN.llt().solve(v_Nd);
    std::cout<< "Cholesky 分解方法所用时间和解为:" << 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC
        << "MS"<< std::endl<< x.transpose()<<std::endl;
#else
    std::cout<< "Cholesky法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endif

/*6、Jacobi迭代法  */
#if (MATRIX_SIZE == MATRIX_SIZE_)
    int Iteration_num = 10 ;
    double Accuracy =0.01;
    tim_stt = clock();
    x= Jacobi(matrix_NN,v_Nd,Iteration_num,Accuracy);
    std::cout<< "Jacobi 迭代法所用时间和解为:" << 1000*(clock() - tim_stt)/(double)CLOCKS_PER_SEC
        << "MS"<< std::endl<< x.transpose()<<std::endl;
#else
    std::cout<<"LU分解法不能解!(提示:直接法中方程组的个数必须与变量个数相同，需要设置MATRIX_SIZE == MATRIX_SIZE_)"<<std::endl;
#endif

    return 0;
}

//迭代不收敛的话 解向量是0
Mat_B Jacobi(Mat_A  &A,Mat_B  &b, int &iteration_num, double &accuracy ) {
    Mat_B x_k = Eigen::MatrixXd::Zero(MATRIX_SIZE_,1);//迭代的初始值
    Mat_B x_k1;         //迭代一次的解向量
    int k,i;            //i,k是迭代算法的循环次数的临时变量
    double temp;        //每迭代一次解向量的每一维变化的模值
    double R=0;         //迭代一次后，解向量每一维变化的模的最大值
    int isFlag = 0;     //迭代要求的次数后，是否满足精度要求

    //判断Jacobi是否收敛
    Mat_A D;            //D矩阵
    Mat_A L_U;          //L+U
    Mat_A temp2 = A;    //临时矩阵获得A矩阵除去对角线后的矩阵
    Mat_A B;            //Jacobi算法的迭代矩阵
    Eigen::MatrixXcd EV;//获取矩阵特征值
    double maxev=0.0;   //最大模的特征值
    int flag = 0;       //判断迭代算法是否收敛的标志 1表示Jacobi算法不一定能收敛到真值

    std::cout<<std::endl<<"欢迎进入Jacobi迭代算法！"<<std::endl;
    //對A矩陣進行分解 求取迭代矩陣 再次求取譜半徑 判斷Jacobi迭代算法是否收斂
    for(int l=0 ;l < MATRIX_SIZE;l++){
        D(l,l) = A(l,l);
        temp2(l,l) = 0;
        if(D(l,l) == 0){
            std::cout<<"迭代矩阵不可求"<<std::endl;
            flag =1;
            break;
        }
    }
    L_U = -temp2;
    B = D.inverse()*L_U;

    //求取特征值
    Eigen::EigenSolver<Mat_A>es(B);
    EV = es.eigenvalues();
//    cout<<"迭代矩阵特征值为:"<<EV << endl;

    //求取矩陣的特征值 然後獲取模最大的特徵值 即爲譜半徑
    for(int index = 0;index< MATRIX_SIZE;index++){
        maxev = ( maxev > __complex_abs(EV(index)) )?maxev:(__complex_abs(EV(index)));
    }
    std::cout<< "Jacobi迭代矩阵的谱半径为："<< maxev<<std::endl;

    //谱半径大于1 迭代法则发散
    if(maxev >= 1){
        std::cout<<"Jacobi迭代算法不收敛！"<<std::endl;
        flag =1;
    }

    //迭代法收敛则进行迭代的计算
    if (flag == 0 ){
        std::cout<<"Jacobi迭代算法谱半径小于1,该算法收敛"<<std::endl;
        std::cout<<"Jacobi迭代法迭代次数和精度： "<< std::endl << iteration_num<<" "<<accuracy<<std::endl;

        //迭代计算
        for( k = 0 ;k < iteration_num ; k++ ){
            for(i = 0;i< MATRIX_SIZE_ ; i++){
                x_k1(i) = x_k(i) + ( b(i) - Jacobi_sum(A,x_k,i) )/A(i,i);
                temp = fabs( x_k1(i) - x_k(i) );
                if( fabs( x_k1(i) - x_k(i) ) > R )
                    R = temp;
            }

            //判断进度是否达到精度要求 达到进度要求后 自动退出
            if( R < accuracy ){
                std::cout <<"Jacobi迭代算法迭代"<< k << "次达到精度要求."<< std::endl;
                isFlag = 1;
                break;
            }

            //清零R，交换迭代解
            R = 0;
            x_k = x_k1;
        }
        if( !isFlag )
            std::cout << std::endl <<"迭代"<<iteration_num<<"次后仍然未达到精度要求，若不满意该解，请再次运行加大循环次数！"<< std::endl;
        return x_k1;
    }
    //否则返回0
    return  x_k;
}

//Jacobi迭代法的一步求和计算
double Jacobi_sum(Mat_A  &A,Mat_B &x_k,int i) {
    double sum;
    for(int j = 0; j< MATRIX_SIZE_;j++){
        sum += A(i,j)*x_k(j);
    }
    return sum;
}