當你下定決心開始學習SLAM時，看到《視覺SLAM十四講》的你是否欣喜若狂？
當你學完理論知識準備在實踐章節大顯身手時，發現原始碼完全不會跑的你又是否一臉懵逼？
不要怕！
和李哈哈一起刷夜，我們一起康康每講的實踐部分，到底怎么玩，
寫在前面的話
和很多初學SLAM的小伙伴一樣，我對SLAM、對Linux，甚至對C++都不清楚😴
看了看SLAM十四講感覺突然有點感覺的時候，實踐部分又完全無從下手😟
針對以上這種情況，我決定將自己完成實踐部分作業的程序分享出來😚
希望那些和我一樣剛開始學習SLAM的小伙伴，能夠與我產生共鳴😝
如果對你能有一丟丟幫助，🏀??觀眾老爺們點個贊👍👍👍👍👍👍
其他各講實踐索引
你可以從這里直接跳轉到其他講的實踐講解:
視覺SLAM十四講原始碼的正確打開方式：第1講 預備知識
視覺SLAM十四講原始碼的正確打開方式：第2講 初識SLAM
視覺SLAM十四講原始碼的正確打開方式：第3講 三維空間剛體運動
實踐3.2：Eigen
把從GitHub上下載的原始碼打開，把其中的ch3檔案夾拷貝到桌面
如果不知道原始碼在哪里/怎么下載，可以從索引查看第1講的內容
我習慣將需要用到的檔案拷貝到桌面或桌面的某個檔案夾進行測驗
本節將講解如何使用Eigen表示矩陣和向量，在下一節的實踐引申至旋轉矩陣與變換矩陣的計算，
本節的代碼在ch3/useEigen中
Eigen 是一個 C++ 開源線性代數庫，它提供了快速的有關矩陣的線性代數運算，還
包括解方程等功能，許多上層的軟體庫也使用Eigen進行矩陣運算，包括 g2o、Sophus 等，
安裝Eigen
安裝Eigen的方法書上寫的直接用就沒啥問題（我就是這么裝的）
終端輸入：
sudo apt-get install libeigen3-dev
Eigen的頭檔案默認位置為"/usr/include/eigen3"，可以命令列輸入以下指令查看電腦安裝的位置：
sudo updatedb
locate eigen3
作者提到，Eigen的特殊支出在于，它是純用頭檔案搭建起來的庫（一般的庫應該有頭檔案&庫檔案）
所以，在使用時，只需要引入Eigen的頭檔案，而不需要鏈接庫檔案
編譯工程
說到這一步，我就來精神了昂！
從何說起呢？😘
很久很久之前，當我開心地打開ch3這個檔案夾，以為簡單的編譯運行就能跑了
然而這個檔案的結構完全讓我不知所措
最關鍵的是，書上寫的用來指導編譯這個程式的CMakeList.txt只有一句話：
include_directories("/usr/include/eigen3")
但是我打開的CMakeList.txt長這個樣子：
cmake_minimum_required(VERSION 2.8)
project(useEigen)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-O3")
# 添加Eigen頭檔案
include_directories("/usr/include/eigen3")
add_executable(eigenMatrix eigenMatrix.cpp)
更離譜的是，ch3里邊有四個實踐的檔案夾：
剛剛提到的CMakeList.txt是useEigen這個檔案夾里的，外邊居然還有一個CMakeList.txt！😥
這和我在第2講看到的工程結構完全不一樣啊！
直覺告訴我，作者把一整講的所有原始碼都放在了一起，用最外層的CMakeList.txt統一調度
后來看來確實如此
看到這里的時候，我果斷停下滾去學CMake了
學習的成果在這里：（這篇文章我覺得自己寫的還是不錯的，起碼花里胡哨的😏）
CMake自學記錄，看完保證你知道CMake怎么玩！！！
非常希望大家粗略的看一下上文，找到里邊提到的教程自己做做demo試試看
畢竟遲早要學的嘛
學成歸來，開始編譯作業，我的方法是在根目錄創建一個build檔案夾，用來存放編譯生成的中間檔案：（根目錄就是打開ch3之后所在的地方）
以useEigen目錄為例，此時該目錄檔案如下：
在build目錄下打開終端，使用編譯指令：
cmake ..
該指令后邊的兩個.是指對上一級目錄進行cmake
此時build目錄下生成了useEigen目錄，其內容如下：
之后我們仍在build目錄下，使用編譯指令：
make
這時build目錄下的useEigen目錄如下：
可以看到生成了可執行檔案eigenMatrix
至此運行前的編譯準備程序就完成了
簡單說一下這兩個指令在干嘛
cmake：根據CMakeList.txt的內容生成編譯檔案CMakeFiles
make：根據CMakeFiles以及源檔案生成可執行檔案
由于我們創建了一個build檔案夾，所以編譯的中間檔案及生成的可執行檔案保存在了buil目錄下
這樣說可能不是完全正確，因為我對在build目錄下編譯的原理還不是很清楚，后續搞懂了回來填坑
應該要再創建一個bin目錄來把可執行檔案放進去統一管理，但這里先不考慮這個了，抓住主要矛盾就好啦🚩
運行useEigen
使用./命令來運行二進制可執行檔案，具體運行方法可以輸入以下指令：
./useEigen/eigenMatrix
在命令列中輸入指令效果如下：
關于代碼的講解可以參照這篇還不存在的文章🚩
實踐3.2：Eigen
師傅別念了，已經在寫了啦😟
關于本節的代碼，我加了一丟丟注釋，貼在下邊啦：
#include <iostream>
using namespace std; //使用std名稱空間
#include <ctime>
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩陣的代數運算（逆，特征值等）
#include <Eigen/Dense>
using namespace Eigen; //使用Eigen名稱空間
#define MATRIX_SIZE 50
/****************************
* 本程式演示了 Eigen 基本型別的使用
****************************/
int main(int argc, char **argv)
{
// Eigen 中所有向量和矩陣都是Eigen::Matrix，它是一個模板類，它的前三個引數為：資料型別，行，列
// 宣告一個2*3的float矩陣
Matrix<float, 2, 3> matrix_23;
// 同時，Eigen 通過 typedef 提供了許多內置型別，不過底層仍是Eigen::Matrix
// 例如 Vector3d 實質上是 Eigen::Matrix<double, 3, 1>，即三維向量
Vector3d v_3d; //注意這里的元素是double型別哦，這里的d表示double，改為f就是float
// 這是一樣的
Matrix<float, 3, 1> vd_3d;
// Matrix3d 實質上是 Eigen::Matrix<double, 3, 3>
Matrix3d matrix_33 = Matrix3d::Zero(); //初始化為零
// 如果不確定矩陣大小，可以使用動態大小的矩陣
Matrix<double, Dynamic, Dynamic> matrix_dynamic;
// 更簡單的
MatrixXd matrix_x;
// 這種型別還有很多，我們不一一列舉
// 下面是對Eigen陣的操作
// 輸入資料（初始化）
matrix_23 << 1, 2, 3, 4, 5, 6;
// 輸出
cout << "matrix 2x3 from 1 to 6: \n"
<< matrix_23 << endl;
// 用()訪問矩陣中的元素，元素從0開始計數
cout << "print matrix 2x3: " << endl;
for (int i = 0; i < 2; i++)
{
for (int j = 0; j < 3; j++)
cout << matrix_23(i, j) << "\t";
cout << endl;
}
// 矩陣和向量相乘（實際上仍是矩陣和矩陣）
v_3d << 3, 2, 1;
v_3d(0,0) = 100;//!自己寫的賦值
vd_3d << 4, 5, 6;
// 但是在Eigen里你不能混合兩種不同型別的矩陣，像這樣是錯的
// Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;
// 應該顯式轉換
Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
cout << "[1,2,3;4,5,6]*[100,2,1]=" << result.transpose() << endl;//此時列向量v_3d第一個元素是100
Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2.transpose() << endl;
// 同樣你不能搞錯矩陣的維度
// 試著取消下面的注釋，看看Eigen會報什么錯
// Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;
// 一些矩陣運算
// 四則運算就不演示了，直接用+-*/即可，
matrix_33 = Matrix3d::Random(); // 亂數矩陣
cout << "random matrix: \n"
<< matrix_33 << endl;
cout << "transpose: \n"
<< matrix_33.transpose() << endl; // 轉置
cout << "sum: " << matrix_33.sum() << endl; // 各元素和
cout << "trace: " << matrix_33.trace() << endl; // 跡
cout << "times 10: \n"
<< 10 * matrix_33 << endl; // 數乘
cout << "inverse: \n"
<< matrix_33.inverse() << endl; // 逆
cout << "det: " << matrix_33.determinant() << endl; // 行列式
// 特征值
// 實對稱矩陣可以保證對角化成功
SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
cout << "Eigen values = \n"
<< eigen_solver.eigenvalues() << endl;
cout << "Eigen vectors = \n"
<< eigen_solver.eigenvectors() << endl;
// 解方程
// 我們求解 matrix_NN * x = v_Nd 這個方程
// N的大小在前邊的宏里定義，它由亂數生成
// 直接求逆自然是最直接的，但是求逆運算量大
Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
matrix_NN = matrix_NN * matrix_NN.transpose(); // 保證半正定
Matrix<double, MATRIX_SIZE, 1> v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);
clock_t time_stt = clock(); // 計時
// 直接求逆
Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
cout << "time of normal inverse is "
<< 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
cout << "x = " << x.transpose() << endl;
// 通常用矩陣分解來求，例如QR分解，速度會快很多
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout << "time of Qr decomposition is "
<< 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
cout << "x = " << x.transpose() << endl;
// 對于正定矩陣，還可以用cholesky分解來解方程
time_stt = clock();
x = matrix_NN.ldlt().solve(v_Nd);
cout << "time of ldlt decomposition is "
<< 1000 * (clock() - time_stt) / (double)CLOCKS_PER_SEC << "ms" << endl;
cout << "x = " << x.transpose() << endl;
return 0;
}
我覺得這里主要列舉了Eigen表示矩陣或向量及基本運算的一些方法
后面可以回來看看這里的東西，感覺只有真的手撕起來才知道怎么玩
實踐3.6：Eigen幾何模塊
3.6.1 Eigen幾何模塊的資料演示
這一節的實踐展示了一些幾何模塊：旋轉矩陣、旋轉向量、歐拉角、四元數的Eigen代碼實作
用這幾種旋轉方式旋轉一個向量v，最后獲得了一樣的結果
這里不太想看原始碼具體怎么實作的了，以后用的時候再回來學習補充🚩
把作者告訴我們的注意事項說一下：
程式代碼通常和數學表示有一些細微的差別，比如運算子多載
我們可以在程式中直接計算四元數和三維向量的乘法，但是數學上應把向量轉化為虛四元數再相乘
代碼運行結果如下：
3.6.2 實際的坐標變換例子
在這一節作者給了我們一個實際的坐標變換的例子
簡單來說就是已知兩個位姿資訊和一個位姿下某點的坐標資訊，來求另一個位姿下該點的坐標
具體的問題描述就看書吧，我就不用重新打一遍了吧嘻嘻
而打開源代碼，發現頭都大了
這一節的代碼完全沒有注釋！！！
不要怕，我把我注釋好的放在這里，供大家食用：
#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
using namespace Eigen;
int main(int argc, char **argv)
{
Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);//定義兩個小蘿卜自身姿態的兩個四元數
q1.normalize();//?對兩個四元數進行歸一化
q2.normalize();
//定義兩個小蘿卜的位置的兩個三維坐標
Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
Vector3d p1(0.5, 0, 0.2);//用來表示小蘿卜一號坐標系下該點坐標（題干給出）
Vector3d p2; //用來表示小蘿卜二號坐標系下該點坐標
//!下面分別用四元數以及變換矩陣得到小蘿卜二號的該點觀測資訊
//以下用四元數求解p2坐標
p2 = q2 * q1.inverse() * (p1 - t1) + t2;//求解p2坐標
//這里的inverse是相反的意思，也就是求q1的逆矩陣
//公式為：p2 = q2 * q1^-1 * (p1 - t1) + t2
cout << "四元數求得的p2坐標" << endl;
cout << p2 << endl;//列向量顯示
cout << p2.transpose() << endl;//行向量顯示，transpose是轉置矩陣的意思
//以下用歐拉矩陣求解p2坐標
Isometry3d T1w(q1), T2w(q2);//歐式變換矩陣Isometry（雖然稱為3d，實質上是4＊4的矩陣）
T1w.pretranslate(t1);//設定平移向量，我的理解是加入這個平移向量
T2w.pretranslate(t2);
Vector3d p3;//用來表示小蘿卜二號坐標系下該點坐標（變換矩陣方法）
p3 = T2w * T1w.inverse() * p1;//求解p3坐標
cout << "變換矩陣/歐拉矩陣求得的p2坐標" << endl;
cout << p3.transpose() << endl;
return 0;
}
基本上代碼怎么回事看我的注釋和代碼就完全沒問題了
一定好好看注釋啊親人們
當然細心的你應該已經發現了，我同時寫了四元數求解和變換矩陣的求解結果，結果如下：
可以看到，二者的結果完全相同
非常有必要的是，你要看看這里的公式推導，戳下邊的鏈接哦：
視覺SLAM十四講 3.6.2 實際的坐標變換例子代碼決議
實踐3.7 可視化演示
3.7.1 顯示運動軌跡
后邊這兩節實踐還沒搞清楚，貼出來演示效果圖吧：
這是很有成就感的一張圖，開心！
但是這個程式在運行時遇到了一個bug🐛🐛🐛
直接運行可執行檔案，我這里會出現如下的錯誤：
報錯資訊為：
cannot find trajectory file at ./example/trajectory.txt
可以理解為程式找不到這個路徑下的這個.txt檔案
問題就出在該程式的源檔案上，也就是基本的.cpp檔案，其內部有如下的一句代碼：
string trajectory_file = "./examples/trajectory.txt";
他沒有在這個路徑下找到這個保存有軌跡資訊的檔案，所以產生了錯誤
我感覺問題來源應該是我們創建了build檔案夾并在里邊編譯的原因
我的解決辦法是直接把檔案的完整路徑放進去，如下所示：
//這里是一個坑，要把完整路徑寫上去哦
// path to trajectory file
string trajectory_file = "/home/lihaha/桌面/ch3/examples/trajectory.txt";
這樣再運行就沒問題啦！
3.7.2 顯示相機的位姿
如果你都看到這里了，那說明你一定是個有水平的小可愛😝
優秀的人都喜歡點贊哦，親人們給個贊吧，這對我很重要！！！
👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍
轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/241001.html
標籤：其他