視覺slam學習筆記以及課后習題《第二講三維物體剛體運動》

前言

這篇博客主要記錄了我在深藍學院視覺slam課程中的課后習題，因為是為了統計知識點來方便自己以后查閱，所以有部分知識可能不太嚴謹，如果給大家造成了困擾請見諒，大家發現了問題也可以私信或者評論給我及時改正，歡迎大家一起探討，其他章節的筆記和作業可以在我的首頁中查找對應文章，

整個作業的代碼和檔案都可以參考我的GitHub存盤庫https://github.com/1481588643/vslam.git

一.熟悉 Eigen 矩陣運算 (2 分，約 2 小時)

Eigen（http://eigen.tuxfamily.org）是常用的 C++ 矩陣運算庫，具有很高的運算效率，大部分需要在 C++ 中使用矩陣運算的庫，都會選用 Eigen 作為基本代數庫，例如 Google Tensorflow，Google Ceres，GTSAM 等，本次習題，你需要使用 Eigen 庫，撰寫程式，求解一個線性方程組，為此，你需要先了解一些有關線性方程組數值解法的原理，

設線性方程 Ax = b，在 A 為方陣的前提下，請回答以下問題：

1.在什么條件下，x 有解且唯一？

答：當A為方陣且A可逆（即A滿秩），x有解且唯一，

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

答：高斯消元法(Gaussian elimination)是求解線性方程組的一種演算法，它也可用來求矩陣的秩，以及求可逆方陣的逆矩陣，它通過逐步消除未知數來將原始線性系統轉化為另一個更簡單的等價的系統，

原理：通過初等行變化將線性方程組的增廣矩陣轉化為行階梯矩陣（上三角或下三角矩陣），通過初等行變換把A編程階梯矩陣求解

求解程序：

1.構造增廣矩陣，即系數矩陣A增加上常數向量b(A|b).

2.通過以交換行、某行乘以非負常數和兩行相加這三種初等變化將原系統轉化為更簡單的三角形式(**triangular form**) 注:這里的初等變化可以通過系數矩陣A乘上初等矩陣E來實作，

從而得到簡化的三角方陣組，這樣就容易解了

3.最后再使用向后替換演算法(Algorithm for Back Substitution)求解得，

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

答： 將A分解成正交矩陣和上三角矩陣的乘積A=QR，易解Ra=Q^Tb
原理：利用２個正交矩陣的乘積一定是正交矩陣，正交矩陣的逆矩陣也是正交矩陣的特點，對正交矩陣進行多次分解，相乘，QR分解的原理主要是把矩陣分解成一個列向量正交矩陣與一個上三角矩陣的跡，原理是將矩陣每個列作為一個基本單元，將其化為正交的基向量，與在這個基向量上的投影長度的QR分解，經常用來解決最小回歸問題，也是特征值演算法QR演算法的基礎，

求解程序：

1.對需要求解的特征值的矩陣A進行QR分解；

2.對分解出來的結果進行逆向相乘；

3.將相乘得到的矩陣進行QR分解；

4.對分解出來的結果進行逆向相乘；

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

答：

定義：如果矩陣A為n階對稱正定矩陣，則存在一個對角元素為正數的下三角實矩陣L，使得：\(A=LL^T\)，當限定L的對角元素為正時，這種分解是唯一的，稱為Cholesky分解，
cholesky分解是把一個對稱正定的矩陣表示成一個下三角矩陣L和其轉置的乘積的分解，它要求矩陣的所有特征值必須大于零，故分解的下三角的對角元也是大于零的，Cholesky分解法又稱平方根法，是當A為實對稱正定矩陣時，LU三角分解法的變形

5.編程實作 A 為 100 × 100 隨機矩陣時，用 QR 和 Cholesky 分解求 x 的程式，你可以參考本次課用到的 useEigen 例程，

提示：你可能需要參考相關的數學書籍或文章，請善用搜索引擎，Eigen 固定大小矩陣最大支持到 50， 所以你會用到動態大小的矩陣，

#include<iostream>
#include<Eigen/Core>
#include<Eigen/Dense>
using namespace Eigen;
using namespace std;
int size=100;

int main(int argc, char const *argv[])
{

Matrix<double, Dynamic, Dynamic>A;
A=MatrixXd::Random(size,size);
A=A*A.transpose();
 Matrix<double, Dynamic, 1> B;
B=MatrixXd::Random(size,1);
Matrix<double,Dynamic,1>X;
X=A.colPivHouseholderQr().solve(B);
     cout<<"QR:X="<<X.transpose()<<endl;
     X=A.ldlt().solve(B);
     cout<<"CHOLESKY:X="<<X.transpose()<<endl;
 //    Eigen::MatrixXd A=MatrixXd::Random()
     return 0;
}

二.矩陣論基礎 （ 分，約 2 小時）

除了我們本科學過的基礎線性代數之外，大部分研究生課程還會開設矩陣論課程，以作為對本科階段 知識的擴充，對于很多線性問題（SLAM 里也會碰到許多線性問題），了解一些矩陣論基礎知識是很有好處的，我們在附件中為大家提供了張賢達老師的《矩陣分析與應用》，請參考該書內容（或者你能找到的其 他書籍），回答以下問題，

1.什么是正定矩陣和半正定矩陣？

答：給定一個n乘以n的實對稱矩陣A，若對于任意長度為 n 的非零向量 x，有 x的轉置乘以A再乘以x恒大于0，則矩陣 A 是一個正定矩陣，

給定一個n乘以n的實對稱矩陣A，若對于任意長度為 n 的非零向量 x，有 x的轉置乘以A再乘以x恒大于等于0，則矩陣 A 是一個正定矩陣，

2.對于方陣 A，它的特征值是什么？特征向量是什么？特征值一定是實數嗎？如何計算一個矩陣的特征值？

答：設A為n階實方陣，如果存在某個數 及某個n維非零列向量，使得，則稱是方陣A的一個特征值，是方陣A的屬于特征值的一個特征向量，

設A為n階實方陣， 為一個引數，稱n階方陣為A的特征方陣，它的行列式稱為A的特征多項式，把稱為A的特征方程，把特征方程或特征多項式的根稱為A的特征根，

特征值不一定是實數，也可能是虛數，

3.什么是矩陣的相似性？相似性反映了什么幾何意義？

答：在線性代數中，相似矩陣（英語：similar matrix）是指存在相似關系的矩陣，相似關系是兩個矩陣之間的一種等價關系，兩個n×n矩陣A與B為相似矩陣當且僅當存在一個n×n的可逆矩陣P，使得：{P^{-1}AP=B}

P被稱為矩陣A與B之間的相似變換矩陣，

相似的矩陣是同一個線性變換在不同基/坐標系下的的不同描述，
線性變換若粗略看成一個剛體的特定運動，剛體的特定運動是同一個，但坐標系改變的話這個運動的描述函式就會不一樣，如果這個函式可用矩陣等價替代的話，一個坐標系就對應著一個矩陣，因此這些矩陣就不同，但這些矩陣必有關系，這個關系就是相似，

4.矩陣一定能對角化嗎？什么樣的矩陣能保證對角化？不能對角化的矩陣能夠形成什么樣的形式（Jor- dan 標準形）？

答：矩陣不一定能對角化，n階方陣可對角化的充分必要條件是它有n個線性無關的特征向量，不能對角化的矩陣一定具有多重特征值，對于不能對角化的矩陣也希望找到某種標準形式，使之盡量接近對角化的形式——Jordan標準形，

5.奇異值分解（SVD）是什么意思？

答：奇異值分解（SVD）是一種用于將矩陣歸約成其組成部分的矩陣分解方法，以使后面的某些矩陣計算更簡單，

X=UΣV?,X=UΣV?,
其中U是m×m階酉矩陣；Σ是m×n階非負實數對角矩陣；而V*，即V的共軛轉置，是n×n階酉矩陣，這樣的分解就稱作X的奇異值分解，

6.矩陣的偽逆是什么意思（Pseudo inverse）？莫爾——彭多斯逆是如何定義的？怎么計算一個矩陣的偽逆？

答：對于矩陣A，如果存在一個矩陣B，使得AB=BA=I，其中I為與A,B同維數的單位陣，就稱A為可逆矩陣（或者稱A可逆），并稱B是A的逆矩陣，簡稱逆陣，（此時的逆稱為凱利逆）矩陣A可逆的充分必要條件是|A|≠0，奇異矩陣陣或非方陣的矩陣不存在逆矩陣，但可以用函式pinv(A)求其偽逆矩陣，基本語法為X=pinv(A),X=pinv(A,tol),其中tol為誤差：max(size(A))*norm(A)*eps，函式回傳一個與A的轉置矩陣A'同型的矩陣X，并且滿足：AXA=A,XAX=X.此時，稱矩陣X為矩陣A的偽逆，也稱為廣義逆矩陣，pinv(A)具有inv(A)的部分特性，但不與inv(A)完全等同，

設 A∈Cm*n,若存在矩陣G∈Cn*m（C為復數域）,使得
(1) AGA = A;
(2) GAG = G;
(3) (AG)H = AG;
(4) (GA)H = GA;
則稱 G 為 A 的Moore-Penrose廣義逆或加號廣義逆，簡稱為A的M-P逆，A的任意M-P逆記為A+，

7. 對于超定方程：Ax = b A 不可逆時，我們通常計算最小二乘解：x = arg minx

Ax ? b ，線性方程的最小二乘解在代數意義上是可以決議地寫出來的，請回答以下小問題：

7.1 在 b ?= 0 時，x 的解是什么形式？事實上，我們可以對 A 求奇異值或對于 ATA 求特征值，請闡述兩者之間的關系，

答：對于超定方程Ax = 0的解就是 ATA 最小特征值對應的特征向量，

簡單的講　所有的矩陣都可以進行奇異值分解，不管其是否是方陣以及對稱矩陣，當所給的矩陣是對稱的方陣，A(T)=A，二者的結果是相同的，也就是說對稱矩陣的特征值分解是所有奇異值分解的一個特例，

但是二者還是存在一些小的差異，奇異值分解需要對奇異值從大到小的排序，而且全部是大于等于零，

特征值用來描述方陣，可看做是從一個空間到自身的映射，這也表現在了名字eigenvalue中，奇異值可以描述長方陣或奇異矩陣，可看做是從一個空間到另一個空間的映射，

7.2 當 b = 0 時，我們希望求 x 的非零解，請說明如何求解 x，

答：對于超定方程Ax = 0的解就是 ATA 最小特征值對應的特征向量，

7.3 請談談你對上述解法在幾何意義上的理解，該問題為開放問題，

答：首先考慮一個簡單的超定方程組

該方程組的右端向量是三維向量，系數矩陣的每一列也是三維向量，但待求的未知向量卻是二維向量，將系數矩陣按列分塊，G =[a1，a2]，記右端向量為b，則方程組求解問題可表示為求組合系數x和y使

xa1 + ya2 = b

的向量的線性組合問題，由于兩個向量a1，a2不構成三維空間的一組基，所以一般情況下這一問題無解，而由向量a1，a2張成的子空間span{a1，a2}是一張平面，記為p，則超定方程組的最小二乘解實際上是求X*，使GX* 恰好等于b 在平面p上的投影，而最小二乘解所對應的殘差向量則垂直于向量GX*，事實上，由正規方程組

GTGX=GTb

得

GT (b –GX * ) = 0

上式的幾何意義可解釋為：最小二乘解的殘差向量與超定方程組的系數矩陣G的所有列向量正交，從而

(X*)T GT (b –GX * ) = 0

所以

(GX *，b –GX * ) = 0

三.幾何運算練習 (2 分，約 1 小時)

下面我們來練習如何使用 Eigen/Geometry 計算一個具體的例子，

一個機器人上通常會安裝許多不同的傳感器，而 這些傳感器之間還存在固連關系，我們舉一個典型的例子，

在世界系 W 下，存在一個運動的機器人 R，按照固定的或者某些開發人員或者領導的特殊喜好，R 系定義在機器人腳部的位置，但是機器人在設計的時候，又定義了 B 系（Body 系，或本體系），位于機器人頭部的位置，由于溝通不暢，標定人員把一臺激光傳感器和一臺視覺傳感器標定在了 B 系下，我們稱激光傳感器為 L 系，視覺傳感器為 C 系，現在請你完成以下作業：

1. 說明一個激光傳感器下的看到的點應該如何計算它的世界坐標，

2. 取：qW R = [0.55, 0.3, 0.2, 0.2], tW R = [0.1, 0.2, 0.3]T ，qRB = [0.99, 0, 0, 0.01], tRB = [0.05, 0, 0.5]T ，

qBL = [0.3, 0.5, 0, 20.1], tBL = [0.4, 0, 0.5]T , qBC = [0.8, 0.2, 0.1, 0.1], tBC = [0.5, 0.1, 0.5]T ，現在假

設相機傳感器觀察到自身坐標系下的點 [0.3, 0.2, 1.2]，請計算：

(a)這個點在激光系下的坐標； (b)這個點在世界系下的坐標，

提示：

1. 本題的資料是隨意取的，沒有真實意義，產生什么結果都不要奇怪，
2. 四元數在使用前需要歸一化，
3. 請注意 Eigen 在使用四元數時的虛部和實部順序，

PS:注意這道題的坐標變換公式，要理清給的轉換矩陣是那個坐標系到哪個坐標系的轉換矩陣，比如T_WR就是機器人到世界坐標系下的轉換矩陣，如果我知道一個在機器人坐標系下的點P_R，那么在世界坐標系下該點的坐標就是T_WR*P_R.

這道題給出的公式是這個點在激光系下的坐標為：P_L=T_BL.inverse()*T_BC*P_C

在世界坐標系下的轉換公式為：P_W=T_WR*T_RB*T_BC*P_C

#include <iostream>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace Eigen;

int main(int argc, char ** argv){

    //創建視覺傳感器和激光傳感器的四元數
    Quaterniond q_BL(0.3, 0.5, 0,20.1), q_BC(0.8, 0.2, 0.1, 0.1);
    //四元數歸一化
    q_BC.normalize();
    q_BL.normalize();

    //平移向量t1和t2
    Vector3d t_BL(0.4, 0, 0.5), t_BC(0.5, 0.1, 0.5);
    //p1坐標
    Vector3d p1(0.3, 0.2, 1.2);

    //變換矩陣Tc1w和Tc2w
    Isometry3d T_BL(q_BL), T_BC(q_BC);
    T_BL.pretranslate(t_BL);
    T_BC.pretranslate(t_BC);

    //計算p2
    Vector3d p2 = T_BL.inverse()*T_BC*p1;
    cout <<"這個點在激光系下的坐標"<< p2.transpose() << endl;
       Vector3d p23= T_BL*T_BC.inverse()*p1;
    cout <<"這個點在激光系下的坐標"<< p23.transpose() << endl;

  //創建世界系和機器人本體的四元數
    Quaterniond q3(0.55, 0.3, 0.2,0.2), q4(0.99, 0, 0, 0.01);
    //四元數歸一化
    q3.normalize();
    q4.normalize();

    //平移向量t3和t4
    Vector3d t3(0.1, 0.2, 0.3), t4(0.05, 0, 0.5);


    //變換矩陣Tc3w和Tc4w
    Isometry3d Tc3w(q3), Tc4w(q4);
    Tc3w.pretranslate(t3);
    Tc4w.pretranslate(t4);

    //計算p3
    Vector3d p3 = Tc4w*Tc3w*Tc*p1;
    cout <<"這個點在世界系下的坐標"<< p3.transpose() << endl;


    return 0;
                                                                                                                                                                                          49,1         頂端

四.旋轉的表達 （2 分，約 1 小時）

1.

2.答：ε的維度是3，η的維度是1.

3.

五.羅德里格斯公式的證明 (1 分，約 1 小時)

羅德里格斯公式描述了從旋轉向量到旋轉矩陣的轉換關系，設旋轉向量長度為 θ，方向為 n，那么旋轉矩陣 R 為：

R = cos θI + (1 ? cos θ)nnT + sin θn∧. (4)

1.我們在課程中僅指出了該式成立，但沒有給出證明，請你證明此式，提示：參考https://en.wikipedia. org/wiki/Rodrigues%27_rotation_formula，

2.請使用此式請明 R?1 = RT ，

六.四元數運算性質的驗證 (1 分，約 1 小時)