深蓝学院SLAM课程第二讲作业

第一题：熟悉Eigen矩阵运算

1. r(A)=n:矩阵A的秩等于未知数的个数。
2. ⾼斯消元法：通过用初等行变换将增广矩阵化为行阶梯阵，然后通过回代求解线性方程组的解。原理是将方程组中每个方程含有的未知数的个数降到最低，并且最下面的方程含有的未知数的个数最少。
3. QR分解：把矩阵分解成一个列向量正交矩阵与一个上三角矩阵的积。原理是将矩阵每个列作为一个基本单元，将其化为正交的基向量与在这个基向量上的投影长度的积。参考了这个博主（传送门）
4. Cholesky 分解：将一个对称正定矩阵分解成一个下三角矩阵与其共轭转置之乘积。
5. 代码如下：

#include 
#include 
#include 
#include 

#define MATRIX_SIZE 10
using namespace std;

int main() {
    std::cout << "Hello, World!" << std::endl;

    //Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_A(MATRIX_SIZE,MATRIX_SIZE);
    //Eigen::MatrixXd matrix_A,matrix_x,matrix_b;

    Eigen::MatrixXd matrix_A = Eigen::MatrixXd::Random(MATRIX_SIZE,MATRIX_SIZE);
    Eigen::MatrixXd matrix_x = Eigen::MatrixXd::Random(MATRIX_SIZE,1);
    Eigen::MatrixXd matrix_b = Eigen::MatrixXd::Random(MATRIX_SIZE,1);


    cout << matrix_A << endl;
    cout << "======" << endl;
    cout << matrix_x << endl;
    cout << "======" << endl;
    cout << matrix_b << endl;
    
    //使用QR分解
    matrix_x = matrix_A.colPivHouseholderQr().solve(matrix_b);
    cout << "======" << endl;
    cout << "======" << endl;
    cout << "======" << endl;
    cout << matrix_b.transpose() << endl;

    //使用cholesky分解
    matrix_x = matrix_A.llt().solve(matrix_b);
    cout << "======" << endl;
    cout << "======" << endl;
    cout << "======" << endl;
    cout << matrix_b.transpose() << endl;

    return 0;
}

对应的Cmake文件：

cmake_minimum_required(VERSION 3.14)
project(UseEigen)

set(CMAKE_CXX_STANDARD 11)
set( CMAKE_BUILD_TYPE "Debug" )

# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )

add_executable(UseEigen main.cpp)

第二题：几何运算练习

#include 
#include 
#include 

using namespace std;

int main() {
    std::cout << "Hello, World!" << std::endl;

    //定义并归一化四元数

    Eigen::Quaterniond q1 = Eigen::Quaterniond(0.55, 0.3, 0.2, 0.2).normalized();
    Eigen::Quaterniond q2 = Eigen::Quaterniond(-0.1, 0.3, -0.7, 0.2).normalized();
    Eigen::Vector3d t_1 = Eigen::Vector3d (0.7, 1.1, 0.2);
    Eigen::Vector3d t_2 = Eigen::Vector3d (-0.1, 0.4, 0.8);

    Eigen::Vector3d p_w;  //世界坐标系下的P点
    Eigen::Vector3d p_c1 = Eigen::Vector3d (0.5, -0.1, 0.2);  //1号萝卜坐标系下的P点
    Eigen::Vector3d p_c2;   //2号萝卜坐标系下的P点

    //转化到世界坐标系方式
    Eigen::Isometry3d T_c1w=Eigen::Isometry3d::Identity();       // 虽然称为3d，实质上是4＊4的矩阵
    T_c1w.rotate ( q1 );
    T_c1w.pretranslate ( t_1 );
    Eigen::Isometry3d T_c2w=Eigen::Isometry3d::Identity();
    T_c2w.rotate ( q2 );
    T_c2w.pretranslate ( t_2 );

    p_w = T_c1w.inverse()*p_c1;
    cout << "P点在世界坐标系下的坐标是："<< p_w << endl;

    p_c2 = T_c2w*p_w;
    cout << "P点在2号小萝卜下的坐标是："<< p_c2 << endl;


    return 0;
}

对应的CMake文件和上一题类似。

第三题：旋转的表达

1. 略
2. 虚部是三维度的，实部是一维的
3. 证明如下：
   

第四题：罗德里格斯公式的证明

首先做如下的示意图：

设k是定义旋转轴的单位矢量，并且令v是任何矢量以k为角度θ旋转（右手规则，图中为逆时针）。
使用点和叉积，矢量v可以分解为平行和垂直于轴k的分量则有：

其中与k平行的分量是:

其中k是单位矢量，(v·k)k = （ |v| |k| cos） k = |v| k (k表示矢量方向)，称为矢量投影的v上k。
同样，我们也可以表示出与k垂直的分量：

矢量k × v可以被看作是v ⊥由90°逆时针旋转ķ，所以它们的大小相等，但方向是垂直的（图中的w向量）。
矢量k ×（k × v）表示v ⊥通过逆时针旋转180°（推导过程利用了向量的叉乘性质，因为反向的，所以加负号）

则平行于k轴的分量在旋转下不会改变幅度或方向：

垂直分量会改变方向但保持其大小：

那么，现在完全旋转的矢量是：

通过联立上述方程可替换变量如下：

对于R和旋转向量来说来说，即为：

最后贴一个很形象的图：

第五题：四元数运算性质的验证