《SLAM十四讲》第二讲讲后作业

一、熟悉Eigen矩阵运算

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

1. 在什么条件下，x 有解且唯一？
2. 高斯消元法的原理是什么？
3. QR 分解的原理是什么？
4. Cholesky 分解的原理是什么？
5. 编程实现A 为100 X100 随机矩阵时，用QR 和Cholesky 分解求x 的程序。

解答：

1、非齐次线性方程在A的秩：R(A)与[A|B]的秩：R(A,B)相同时方程有解，即为：R(A)=R(A,B)=n时方程有唯一解。

2、高斯消元法的原理是什么？
原理：高斯消元法的作用是又来求解线性方程组的解，其原理是将方程组进行加减消元，然后求出未知数X。

3、QR 分解的原理是什么？
原理：将一个稀疏矩阵分解成一个正交矩阵和一个上三角矩阵A=QR， A左乘一个Householder反射矩阵Hj, Hn…H2H1A=QTA=[R 0]T

4、Cholesky 分解的原理是什么？
与SLAM问题相关的优化问题，可以很简洁的用稀疏线性代数的方式表达，要么将信息矩阵分解为平方根的形式，要么将观测雅可比矩阵A分解为平方根。原理：乔利斯基分解通过求解正规方程，然后分解信息矩阵得到一种对称正定矩阵（LU分解的百变种）得到一个nn的上三角形矩阵。A=LLT。
5、编程实现A 为100 X100 随机矩阵时，用QR 和Cholesky 分解求x 的程序。

#include
#include
#include
const int SIZE=100;
using namespace std;
using namespace Eigen;
int main(){
        //AX=B
        Matrix<double, Dynamic, Dynamic>A; //建立一个动态矩阵
        A=MatrixXd::Random(SIZE,SIZE);//建立一个100*100的随机矩阵
        A=A.transpose()*A;//乔利斯基分解需要正定矩阵
        Matrix<double, Dynamic,1>B;//建立一个动态矩阵B
        B=MatrixXd::Random(SIZE,1);//B为100*1的随机矩阵
        Matrix<double, Dynamic,1>X;//建立一个X
        X=MatrixXd::Random(SIZE,1);//X为100*1的随机矩阵
        X=A.llt().solve(B);//乔利斯基分解
        cout<<"LLT result:\n"<<X<<endl;//显示结果
        X=A.colPivHouseholderQr().solve(B);//QR分解
        cout<<"QR result:\n"<<X<<endl;//显示结果
        return 0;
}

二、几何运算练习

解答：

#include
#include
#include
using namespace std;
using namespace Eigen;
int main()
{
        Quaterniond q1(0.55,0.3,0.2,0.2);//定义四元数Q1
        Quaterniond q2(-0.1,0.3,-0.7,0.2);
        q1=q1.normalized();//四元素归一化
        q2=q2.normalized();
        Matrix<double,3,1> t1;//定义平移矩阵
        Matrix<double,3,1> t2;
        Matrix<double,3,1> p1;
        Matrix<double,3,1> p2;
        t1<<0.7,1.1,0.2;
        t2<<-0.1,0.4,0.8;
        p1<<0.5,-0.1,0.2;//Pcw相对于Tcw的位姿

        Isometry3d Tcw1=Isometry3d::Identity();//创建欧式群
        Tcw1.rotate(q1.toRotationMatrix());//四元数转化为旋转矩阵
        Tcw1.pretranslate(t1);//四元数转化为平移矩阵
        p1=Tcw1.inverse()*p1;//P1=Tcw1*Pw ==>Pw=P1*inverse(Tcw1)

        Isometry3d Tcw2=Isometry3d::Identity();//创建Tcw2的欧式群
        Tcw2.rotate(q2.toRotationMatrix());
        Tcw2.pretranslate(t2);
        p2=Tcw2*p1;//P2=Tcw2*Pw(Pw=P1*inverse(Tcw1))
        cout<<p2<<endl;
        return 0;

}

![在这里插入图片描述](https://img-blog.csdnimg.cn/20200520232141627.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzQzMjQ3NDM5,size_16,color_FFFFFF,t_70
解答：

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

证明：

参考链接：https://www.cnblogs.com/indulge-code/p/10492209.html
https://www.cnblogs.com/liuhuacai/p/12093770.html