第二章作業
作者：曾是少年
二 熟悉Eigen矩陣運算
Eigen是常用的 C++ 矩陣運算庫，具有很高的運算效率。大部分需要在 C++ 中使用矩陣運算的庫，都會選用 Eigen 作為基本代數庫，例如 Google Tensorflow，Google Ceres，GTSAM 等。本次習題，你需要使用 Eigen 庫，編寫程序，求解一個線性方程組。為此，你需要先了解一些有關線性方程組數值解法的原理。
設線性方程Ax = b，在A 為方陣的前提下，請回答以下問題：
1. 在什么條件下，x 有解且唯一？
答：當A為方陣且A可逆（即A滿秩），x有解且唯一；
2. 高斯消元法的原理是什么？
答：高斯消元法(Gaussian elimination)是求解線性方程組的一種算法，它也可用來求矩陣的秩，以及求可逆方陣的逆矩陣。它通過逐步消除未知數來將原始線性系統轉化為另一個更簡單的等價的系統。
原理：通過初等行變化將線性方程組的增廣矩陣轉化為行階梯矩陣（上三角或下三角矩陣）。通過初等行變換把A編程階梯矩陣求解
求解過程
- 構造增廣矩陣，即系數矩陣A增加上常數向量
b(A|b). - 通過以交換行、某行乘以非負常數和兩行相加這三種初等變化將原系統轉化為更簡單的三角形式(
**triangular form**) 注:這里的初等變化可以通過系數矩陣A乘上初等矩陣E來實現。 - 從而得到簡化的三角方陣組，這樣就容易解了
- 最后再使用向后替換算法(Algorithm for Back Substitution)求解得。
總結
原線性方程組\(\large \rightarrow\) 高斯消元法\(\large \rightarrow\) 下三角或上三角形式的線性方程組\(\large \rightarrow\)前向替換算法求解(對於上三角形式，采用后向替換算法).
3. QR 分解的原理是什么？
答：QR分解
定義：一個矩陣\(A\in R^{m\times n},m\geq n\)可以被分解A=QR,其中：
- \(Q\in R^{m\times m}\)是正交矩陣
- \(R=\left[\begin{array}{cccc}\hat{R}\\0\end{array}\right]\in R^{m\times n}\)
- \(\hat{R}\in R^{n \times n}\)是上三角矩陣
可以看出，A=QR 這一過程將矩陣A分解為Q和R兩部分，其中Q是標准正交矩陣，R是一個上三角矩陣。利用矩陣的QR分解能夠簡化計算，以線性系統的計算為例。
其中，Q 是一個正交矩陣，因此\(Q^T\)非常好計算的，R是一個上三角矩陣（相當於Gauss-Jordan消元法的前向過程結束），從下往上推就可以很快計算出線性系統的結果。
答案： 原理：將A分解成正交矩陣和上三角矩陣的乘積A=QR，易解Ra=Q^Tb
原理：利用２個正交矩陣的乘積一定是正交矩陣，正交矩陣的逆矩陣也是正交矩陣的特點，對正交矩陣進行多次分解，相乘。QR分解的原理主要是把矩陣分解成一個列向量正交矩陣與一個上三角矩陣的跡，原理是將矩陣每個列作為一個基本單元，將其化為正交的基向量，與在這個基向量上的投影長度的QR分解，經常用來解決最小回歸問題，也是特征值算法QR算法的基礎。
求解過程：
- 對需要求解的特征值的矩陣A進行QR分解；
- 對分解出來的結果進行逆向相乘；
- 將相乘得到的矩陣進行QR分解；
- 對分解出來的結果進行逆向相乘；
意義：使用QR分解有助於加快解方程或求解速度即收斂速度
4. Cholesky 分解的原理是什么？
答：cholesky分解
定義：如果矩陣A為n階對稱正定矩陣，則存在一個對角元素為正數的下三角實矩陣L，使得：\(A=LL^T\)，當限定L的對角元素為正時，這種分解是唯一的，稱為Cholesky分解。
cholesky分解是把一個對稱正定的矩陣表示成一個下三角矩陣L和其轉置的乘積的分解。它要求矩陣的所有特征值必須大於零，故分解的下三角的對角元也是大於零的。Cholesky分解法又稱平方根法，是當A為實對稱正定矩陣時，LU三角分解法的變形。
將A分解成A=LLT，其中L是下三角矩陣。然后先求解Lc=b,c為中間結果；再求解LTa=c，得到最終結果。此過程要求A實對稱且正定
5. 編程實現A 為100 \(\times\) 100 隨機矩陣時，用QR 和Cholesky 分解求x 的程序。
你可以參考本次課用到的useEigen 例程。提示：你可能需要參考相關的數學書籍或文章。請善用搜索引擎。Eigen 固定大小矩陣最大支持到50，所以你會用到動態大小的矩陣。
答：
Eigen中有現成的代數庫,因此可以直接調用,關於矩陣分解的函數如下:
整體過程如下:
- 創建並初始化隨機矩陣, (使用動態矩陣創建)
// 對A,b初始化;
int rows = 100;
int cols = 100;
static default_random_engine e(time(0));
static normal_distribution<double> n(0,10);
Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> A(rows,cols);
Eigen::Matrix<double,Eigen::Dynamic,1> b(rows,1);
//隨機賦值
for(int x = 0;x<rows;x++)
{
b(x,0) = random()%10;
for(int y = 0;y<cols;y++)
A(x,y) = abs(n(e));
}
生成隨機矩陣的第二種方法
//生成100*100的隨機矩陣
Matrix<double, Dynamic, Dynamic> matrix 100
matrix 100 =Matrixxd: Random(MATRIX SIZE, MATRIX SIZE)matrix 100 =matrix 100 transpose(*matrix 100
//生成100*1的隨機矩陣
Matrixdouble, Dynamic, 1> v Nd;
v Nd = Matrixxd: Random(MATRIX SIZE, 1);
Matrixxdouble, Dynamic, 1> x
- 調用QR分解求解方程組代碼如下:
Eigen::Matrix<double,Eigen::Dynamic,1> x_qr(rows,1);
x_qr = A.colPivHouseholderQr().solve(b);
cout<<"original: x_qr:"<<endl<<x_qr<<endl;
- 調用Cholesky分解求解方程組代碼如下:
Eigen::Matrix<double,Eigen::Dynamic,1> x_cholesky1(rows,1);
x_cholesky1 = A.llt().solve(b);
cout<<"original : x_cholesky1:"<<endl<<x_cholesky1<<endl;
實驗結果:以3*3的矩陣測試為例:
輸出結果如下:
guoben@guoben-WRT-WX9:~/Project/useEigen/bin$ ./useEigen
A:
-3.63839 -0.279217 8.07804
-22.7538 14.1149 5.92393
-11.9139 -3.94803 1.81077
b:
3
6
7
orginal: x
-0.444675
-0.358359
0.158707
original: x_qr:
-0.444675
-0.358359
0.158707
original : x_cholesky1:
10.4221
-0.609405
-1.94972
此時觀察結果發現,這個隨機矩陣通過QR分解和通過chlosky分解求解得到的矩陣結果並不相同;這是因為A不是對稱正定矩陣.於是我重新生成一個對稱正定矩陣.重新運行程序,結果如下:
A = A*A.transpose(); //加入該行保證A的半正定性
得到結果如下:
A:
1245.09 165.77 790.353
165.77 30.5606 112.37
790.353 112.37 618.909
b:
3
6
7
orginal: x
-0.0872187
0.656756
0.00344748
original: x_qr:
-0.0872187
0.656756
0.00344748
original : x_cholesky1:
-0.0872187
0.656756
0.00344748
此時, 就可以獲得一致的求解結果。
三 幾何運算練習
下面我們來練習如何使用 Eigen/Geometry 計算一個具體的例子。
設有小蘿卜1一號和小蘿卜二號位於世界坐標系中。小蘿卜一號的位姿為：\(q1 = [0.55, 0.3, 0.2, 0.2]\), \(t1 = [0.7, 1.1, 0.2]^T\)（q 的第一項為實部）。這里的 q 和 t 表達的是 \(T_{cw}\)，也就是世界到相機的變換關系。小蘿卜二號的位姿為 \(q2 = [−0.1, 0.3, −0.7, 0.2], t2 = [−0.1, 0.4, 0.8]^T\) 。現在，小蘿卜一號看到某個點在自身的坐標系下，坐標為 \(p_1 = [0.5, −0.1, 0.2]^T\) ，求該向量在小蘿卜二號坐標系下的坐標。請編程實現此事，並提交你的程序。
提示：
- 四元數在使用前需要歸一化。
- 請注意
Eigen在使用四元數時的虛部和實部順序。 - 參考答案為 \(p2 = [1.08228, 0.663509, 0.686957]^T\)。你可以用它驗證程序是否正確。
答:
本題中整題過程如下:
- 對四元數進行歸一化;
- 求解兩個機器人之間的變換矩陣;
涉及到的變換過程可通過下式看出：
代碼實現如下所示:
#include <eigen3/Eigen/Geometry>
#include<iostream>
using namespace std;
using namespace Eigen;
int main()
{
//機器人一號的位置姿態
Quaterniond q1(0.55,0.3,0.2,0.2);
Vector3d t1(0.7,1.1,0.2);
//機器人二號的位置姿態
Quaterniond q2(-0.1,0.3,-0.7,0.2);
Vector3d t2(-0.1,0.4,0.8);
//四元數的歸一化
q1.normalize();
q2.normalize();
Vector3d p1(0.5,-0.1,0.2);
Vector4d p1a;
p1a<<p1,1;
//構建T_cw_1
Matrix4d T_cw_1;
T_cw_1 << q1.toRotationMatrix() ,t1,
0,0,0,1;
cout<<"T_cw_1:"<<endl<<T_cw_1<<endl;
//構建T_cw_2
Matrix4d T_cw_2;
T_cw_2 <<q2.toRotationMatrix(),t2,
0,0,0,1;
cout<<"T_cw_2:"<<endl<<T_cw_2<<endl;
Vector4d p2a;
// p2a = T_cw_2.inverse() * T_cw_1 *p1a;
p2a = T_cw_2 * T_cw_1.inverse() *p1a;
Vector3d p2;
p2 = p2a.block(0,0,3,1);
cout<<"p2:"<<endl<<p2<<endl;
return 0;
}
程序結果如下:
guoben@guoben-WRT-WX9:~/Project/useGeometry/bin$ ./useGeometry
T_cw_1:
0.661376 -0.21164 0.719577 0.7
0.719577 0.449735 -0.529101 1.1
-0.21164 0.867725 0.449735 0.2
0 0 0 1
T_cw_2:
-0.68254 -0.603175 0.412698 -0.1
-0.730159 0.587302 -0.349206 0.4
-0.031746 -0.539683 -0.84127 0.8
0 0 0 1
p2:
1.08228
0.663509
0.686957
視覺SLAM十四講中采用了Isometry3d,值得學習.
其中,構造變換矩陣的方法更加簡單方便 ,如下:
Isometry3d T1w(q1), T2w(q2);
T1w.pretranslate(t1);
T2w.pretranslate(t2);
p2 = T2w * T1w.inverse() * p1;
四 旋轉的表達
課程中提到了旋轉可以用旋轉矩陣、旋轉向量與四元數表達，其中旋轉矩陣與四元數是日常應用中常見的表達方式。請根據課件知識，完成下述內容的證明。
1. 設有旋轉矩陣 R，證明 \(R^T R = I\) 且 \(det R = +1^2\)。
證明： 首先考慮旋轉。設某個單位正交基\((e1; e2; e3)\) 經過一次旋轉變成了\((e'_1; e'_2; e'_3)\)。則旋轉矩陣R如下所示：
對於任意i,有\(e_i^T*e_i=1\),且 \(e_i^T*e_j=0,(i\neq j)\)，因此有
由於：\(RR^T=I\), 因此 \(|I|=|RR^T|=|R||R^T|=|R|^2\),所以\(det(R)=\pm 1\)
2. 設有四元數 q，我們把虛部記為 ε，實部記為 η，那么 \(q = (ε, η)\)。請說明 ε 和 η 的維度。
答：四元數 q 有三個虛部和一個實部。即\(q=q_0+q_1i+q_2j+q_3k\)
因此，\(\varepsilon\) 的維度為 3，\(\eta\) 的維度為 1 。
3, 定義運算 + 和 ⊕ 為：
其中運算 × 含義與 ∧ 相同，即取 ε 的反對稱矩陣（它們都成叉積的矩陣運算形式），1 為單位矩陣。請證明對任意單位四元數 q1, q2，四元數乘法可寫成矩陣乘法：
或者
證明：
其中:
\(\varepsilon^T_2*\varepsilon_1 = \varepsilon^T_1*\varepsilon_2\) (1*1矩陣，轉置等於其本身)；
$ \varepsilon_1^{\times}*\varepsilon_2 = \varepsilon_1 \times \varepsilon_2$
$ - \varepsilon_2^{\times}*\varepsilon_1 = - \varepsilon_2 \times \varepsilon_1 = \varepsilon_1 \times \varepsilon_2 $
五 羅德里格斯公式的證明
羅德里格斯公式描述了從旋轉向量到旋轉矩陣的轉換關系。設旋轉向量長度為 θ，方向為 n，那么旋轉矩陣 R 為：
\(R = cos θI + (1 − cos θ)n n^T + sin θ n^∧\).
1. 證明此式。提示：參考鏈接。
證明：
考慮旋轉一個向量，其中 \(v\) 是原向量，三維的單位向量 \(k=[k_x,k_y,k_z]\) 是旋轉軸， \(θ\) 是旋轉角度，\(v_{rot}\)是旋轉后的向量。
先通過點積得到 \(v\) 在 \(k\)方向的平行分量 \(v_∥=(vk)k\) ，再通過叉乘得到與 \(k\) 正交的兩個向量 \(v_{\perp}\)和\(w\) 。
這樣，我們就得到了3個相互正交的向量。不難得出：
再引入叉積矩陣的概念：記 K 為 \(k=[k_x,k_y,k_z]^T\) 的叉積矩陣。顯然 K是一個反對稱矩陣。
他有如下性質：\(k \times v = K v\)
為了利用該性質，需要將 $v_{rot} $ 代換為 $v $ 與 $k $ 的叉積關系，先根據（1）式做代換：
然后得到：
根據叉積矩陣性質：
最后將 \(v、v_{rot}\) 換為 B、C，就是羅德里格斯公式的標准形式。
為了得到題目中的形式，可通過如下過程得到：
用n替換k可以得到：
2. 請使用此式證明 \(R^{−1}=R^T\) 。
證明：
其中\(n^{\times}\)是反對稱矩陣，因此\({n^{\times}}^{T}+n^{\times} = 0\),
其中,設\(n=[a,b,c]^T\),則
所以：\(nn^T+n^{\times}(n^{\times})^T)=nn^T-n^{\times}(n^{\times}))=0\)
所以：\(RR^T=I\)
因此：\(R^{-1}=R^T\)
六 四元數運算性質的驗證
課程中介紹了單位四元數可以表達旋轉。其中，在談論用四元數 q 旋轉點 p 時，結果為：
此時 \(p′\)必定為虛四元數（實部為零）。請你驗證上述說法。
驗證：
設\(q = (s,v)\) 其中\(s\)為實部，\(v\)為虛部，\(p = (0,x)\)則：
其中實部為
\(Re(p') = -s v^{T} x - (sx+v\times x)^{T}(-v)=-s v^{T}x+ s x^Tv = 0\)
此外，上式亦可寫成矩陣運算：\(p′ = Qp\)。
請根據你的推導，給出矩陣 Q。
注意此時 p 和 \(p′\) 都是四元數形式的變量，所以 Q 為 4 × 4 的矩陣。
提示：如果使用第 4 題結果，那么有：
從而可以導出四元數至旋轉矩陣的轉換方式：
其中 Im 指虛部的內容。
答：
附加題 七 熟悉C++11
C++ 是一門古老的語言，但它的標准至今仍在不斷發展。在 2011 年、2014 年和 2017 年，C++ 的標准又進行了更新，被稱為 C++11，C++14，C++17。其中，C++11 標准是最重要的一次更新，讓 C++ 發生了重要的改變，也使得近年來的 C++ 程序與你在課本上（比如譚浩強）學到的 C++ 程序有很大的不同。你甚至會驚嘆這是一種全新的語言。C++14 和 C++17 則是對 11 標准的完善與擴充。
越來越多的程序開始使用 11 標准，它也會讓你在寫程序時更加得心應手。本題中，你將學習一些 11標准下的新語法。請參考本次作業 books/目錄下的兩個 pdf，並回答下面的問題。
設有類 A，並有 A 類的一組對象，組成了一個 vector。現在希望對這個 vector 進行排序，但排序的方式由 A.index 成員大小定義。那么，在 C++11 的語法下，程序寫成：
#include <iostream>
#include <vector>
#include <algorithm>
using namespace std;
class A {
public:
A(const int& i ) : index(i) {};
int index = 0;
};
int main() {
A a1(3), a2(5), a3(9);
vector<A> avec{a1, a2, a3};
std::sort(avec.begin(), avec.end(), [](const A&a1, const A&a2) {return a1.index<a2.index;});
for ( auto& a: avec ) cout<<a.index<<" ";
cout<<endl;
return 0;
}
請說明該程序中哪些地方用到了 C++11 標准的內容。提示：請關注范圍 for 循環、自動類型推導、lambda表達式等內容。
答：
第9行：使用了初始化列表來初始化字段；
A(const int& i ) : index(i) {};
等同於以下字段
A(const int& i )
{
index = i;
}
第15行：使用了初始化列表來初始化對象： C++11 把初始化列表的概念綁定到了類型上，並將其稱之為 std::initializer_list，允許構造函數或其他函數像參數一樣使用初始化列表，這就為類對象的初始化與普通數組和 POD 的初始化方法提供了統一的橋梁。
第16行：使用了lambda表達式來比較元素大小，其中：const A&a1, const A&a2是參數列表，return a1.index<a2.index;是函數體，返回值是布爾型的大小比較結果。
第17行：用auto關鍵字實現了自動類型推導，讓編譯器自動設置變量a的類型；
第17行：C++引入了基於范圍的for循環，不用下標就能訪問元素；
補充內容：C++11的新特征，主要內容包括：
-
nullptr：用來區別NULL和0
-
自動類型推導：用auto和delctype實現
-
區間迭代：基於范圍的 for 循環
-
初始化列表：
-
模板增強：擴充了原來的強制編譯器在特定位置實例化模板的語法，使得能夠顯式的告訴編譯器何時進行模板的實例化：
-
構造函數：C++11 引入了委托構造的概念，這使得構造函數可以在同一個類中一個構造函數調用另一個構造函數，從而達到簡化代碼的目的：
-
lambda表達式：提供了一個類似匿名函數的特性，而匿名函數則是在需要一個函數，但是又不想費力去命名一個函數的情況下去使用的。
-
新增容器：std::array 保存在棧內存中，相比堆內存中的 std::vector，我們能夠靈活的訪問這里面的元素，從而獲得更高的性能。
-
正則表達式：C++11 提供的正則表達式庫操作 std::string 對象，對模式 std::regex (本質是 std::basic_regex)進行初始化，通過 std::regex_match 進行匹配，從而產生 std::smatch （本質是 std::match_results 對象）。
-
語言級線程支持：代碼編譯需要使用 -pthread 選項
std::thread std::mutex/std::unique_lock std::future/std::packaged_task std::condition_variable -
右值引用和move語義：通過重載直接使用右值參數。我們所要做的就是寫一個以右值引用為參數的構造函數：