《视觉SLAM十四讲课后作业》第二讲

1.设线性⽅程 Ax = b，在 A 为⽅阵的前提下，请回答以下问题：
1. 在什么条件下，x 有解且唯⼀？

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

2. ⾼斯消元法的原理是什么？

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

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

原理：将一个稀疏矩阵分解成一个正交矩阵和一个上三角矩阵A=QR， A左乘一个Householder反射矩阵Hj, H_n...H₂H₁A=Q^TA=[R 0]^T

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

与SLAM问题相关的优化问题，可以很简洁的用稀疏线性代数的方式表达，要么将信息矩阵分解为平方根的形式，要么将观测雅可比矩阵A分解为平方根

原理：乔利斯基分解通过求解正规方程，然后分解信息矩阵得到一种对称正定矩阵（LU分解的百变种）得到一个n*n的上三角形矩阵。A=L*L^T

5. 编程实现 A 为 100 × 100 随机矩阵时，⽤ QR 和 Cholesky 分解求 x 的程序。

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
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;
}
                      

2.设有⼩萝⼘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，求该向量在⼩萝⼘⼆号坐标系下的坐标。请编程实现此事，并提交
你的程序。

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
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,,> t1;//定义平移矩阵
        Matrix<double,,> t2;
        Matrix<double,,> p1;
        Matrix<double,,> 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 ;

}