《視覺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,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;

}