視覺SLAM十四講第2版顯示機器人運動軌跡及相機位姿

基於3.7可視化演示，運行plotTrajectory.cpp操作實錄（詳細填坑流程）

最近學習無人駕駛，在網上看到了高翔老師的《視覺SLAM十四講》，感覺不錯，遂買來仔細研讀。前邊章節學習比較順利，學到3.7節的程序時，運行碰到極大困難，卡了一週，碰到各種坑，所幸一一填平，寫篇文章記錄下來，以觴來者。博文分爲四塊知識點，邊寫邊整理吧，歡迎留言交流。好了，閒話少述，直接進入正題。

一、前期準備

在之前的章節中，我們首先安裝了Ubuntu 1804，學習了CMake基礎語法及熟悉了VIM和IDE KDevelop的用法，下面給出所用資料鏈接（點擊右上方數字即可）：
1.Ubuntu 1804安裝¹，在window10安裝ubuntu雙系統，注意是非虛擬機安裝哦。
2.CMake教學材料，主要有兩部分：《cmake實踐》²及官方指導文檔³。
3.VIM命令學習⁴，幾篇文章湊在一起的，不想仔細讀的可以拉到後邊看看簡化版，掌握主要用法即可。
4.KDevelop⁵，這個稍微詳細些，不用多看，先用起來。

二、git管理子模塊及克隆源代碼

1.學習使用Git Submodule

爲了使用Git的submodule功能管理本書依賴的第三方庫，專門學習了Git Submodule使用完整教程⁶，通過實操，基本掌握了 git clone、checkout、pull、push及submodule的init、add、remove、commit、foreach等操作。教程中需要注意的有以下幾點：

clone時的參數–recursive改爲–recurse-submodules，可能是git版本問題，已移除–recursive。
libs/lib1與libs/lib1/lib1-features的pull和commit需要分別進行，並且會產生不同的commit id，比如你commit並push完libs1-features的修改後，還需要對lib1進行commit並push，一直沒想通爲什麼不能統一管理，有知道的朋友請告知下（lib2情況相同）。
牢記git foreach操作，當包含多個字模塊時，此命令可以省時省力的幫你完成任務。

git submodule foreach ls -l 
git submodule foreach git pull
git submodule foreach --recursive git submodule init 
git submodule foreach --recursive git submodule update

2.克隆源代碼

從github克隆源代碼時，會發現很多坑，諸如下載失敗、網速極慢、子模塊克隆失敗等問題，不急，一個一個的填。

1. 下載失敗

如果你沒對git進行過任何設置，開始可能由於限速和緩存不夠導致下載失敗，如下所示：

fatal: The remote end hung up unexpectedly
fatal: 過早的文件結束符（EOF）
fatal: index-pack 失敗

使用以下命令解決：

git config --global http.postBuffer 10000000
git config --global http.lowSpeedTime 0
git config --global http.lowSpeedTime 999999

2.github提速

正常下載後，發現速度很慢，峯值只有50kb左右，有四種方法可以提速：

1.修改 hosts

使用以下命令查找對應地址映射的IP，每個人根據地區而不同，比如我的如下所示：

simon@bert:~$ nslookup github.com 
Server:		127.0.0.53
Address:	127.0.0.53#53

Non-authoritative answer:
Name:	github.com
Address: 13.250.177.223

simon@bert:~$ nslookup github.global.ssl.fastly.net 
Server:		127.0.0.53
Address:	127.0.0.53#53

Non-authoritative answer:
Name:	github.global.ssl.fastly.net
Address: 31.13.80.17

simon@bert:~$ nslookup codeload.github.com
Server:		127.0.0.53
Address:	127.0.0.53#53

Non-authoritative answer:
Name:	codeload.github.com
Address: 54.251.140.56

    記錄下Address地址IP，並將映射添加到/etc/hosts中，如下所示：

sudo vim /etc/hosts

127.0.0.1       localhost
127.0.1.1       bert
0.0.0.0         account.jetbrains.com
# github config
13.250.177.223  github.com
31.13.80.17     github.global.ssl.fastly.net
54.251.140.56   codeload.github.com

    以下兩種命令均可重啓網絡服務（不用重啓機器即可生效）：

sudo /etc/init.d/networking restart
sudo service network restart

經測試，此方法可提高到200KiB左右。

2.Use SSH

使用SSH，在github克隆或下載界面，點擊Use SSH，如下圖所示：

會給出如何獲取SSH密鑰的鏈接，按照官方文檔一步步操作即可使用SSH下載。因爲設置時沒有截圖，寫文章時登出也找不到，故沒放圖片，有問題的可以在下方留言。
以上兩種方法均可帶來一定速度提升，Use SSH可能會快那麼一丟丟，不過設置複雜一些。

3.掛代理

關於掛代理的說明，首先需要自己搭建代理服務器，對於實驗性的同學來說沒必要啦，但是簡要說明下使用方法：
第一步，在VPS（Virtual Private Server 虛擬專用服務器）上搭建服務器端，現在普遍使用的搭建服務器端的vps主要包括3種：Linode/DigitalOcean/BandwagonHOST，推薦DigitalOcean；
第二步，使用本地shell程序連接代理服務器端；
第三步，在本地安裝Shadowsocks並配置連接；
第四步，使用如下命令指定或取消git代理網址：

#指定代理
git config --global http.https://github.com.proxy 'socks5://127.0.0.1:1080'
#取消代理
git config --global --unset https.proxy
#查看代理設置
git config --global -l

單單在機器上設置代理是會被拒絕訪問的哦，感興趣的童鞋可以按着上述步驟操作下。

4.碼雲

這篇文章發表後，學習opencv時，發現了一個很好用的github下載工具，碼雲，在此補充上。試過以上方法沒用的小夥伴這次一定可以幫到你。具體方法參考以下文章：《git與碼雲連接起來》（由於審覈問題，請小夥伴們自行搜索吧）。

3.執行克隆

克隆源代碼命令如下：

git clone --recurse-submodules https://github.com/gaoxiang12/slambook2.git slambook2

使用SSH請把網址改爲：[email protected]:gaoxiang12/slambook2.git。
克隆完成後，進入~/slambook2/3rdparty/Pangolin，眼前一黑，目錄竟然是空地，查看克隆日誌，發現錯誤提示：子模塊路徑…未註冊。反覆執行clone命令試錯之後放棄，然後遞歸初始化子模塊，命令及結果如下：

git submodule update --init --recursive
正克隆到 '/home/simon/slam/slambook2/3rdparty/DBoW3'...
error: RPC failed; curl 56 GnuTLS recv error (-54): Error in the pull function.
fatal: The remote end hung up unexpectedly
fatal: 過早的文件結束符（EOF）
fatal: index-pack 失敗
fatal: 無法克隆 'https://github.com/rmsalinas/DBow3' 到子模組路徑 '/home/simon/slam/slambook2/3rdparty/DBoW3'
克隆 '3rdparty/DBoW3' 失敗。按計劃重試

反覆執行幾次，還是被KO，鬱悶至極。最後想出了不得已的笨方法，針對每個子模塊分別clone，終於搞定。方法如下：打開文件~/slam/slambook2/.gitmodule：

simon@bert:~/slam/slambook2$ cat .gitmodules 
[submodule "3rdparty/Pangolin"]
	path = 3rdparty/Pangolin
	url = https://github.com/stevenlovegrove/Pangolin
[submodule "3rdparty/Sophus"]
	path = 3rdparty/Sophus
	url = https://github.com/strasdat/Sophus
[submodule "3rdparty/ceres-solver"]
	path = 3rdparty/ceres-solver
	url = https://github.com/ceres-solver/ceres-solver
[submodule "3rdparty/g2o"]
	path = 3rdparty/g2o
	url = https://github.com/RainerKuemmerle/g2o
[submodule "3rdparty/DBoW3"]
	path = 3rdparty/DBoW3
	url = https://github.com/rmsalinas/DBow3
[submodule "3rdparty/googletest"]
	path = 3rdparty/googletest
	url = https://github.com/google/googletest.git

以Pangolin爲例，在目錄~/slambook2/3rdparty，刪除子模塊中文件.git，並使用如下命令：

git clone --recurse-submodules [email protected]:stevenlovegrove/Pangolin.git Pangolin
正克隆到 'Pangolin'...
...#省略無關日誌
接收對象中: 100% (361/361), 147.50 KiB | 245.00 KiB/s, 完成.
處理 delta 中: 100% (151/151), 完成.
子模組路徑 'external/pybind11/tools/clang'：檢出 '6a00cbc4a9b8e68b71caf7f774b3f9c753ae84d5'

其他子模塊雷同，不再累述。
寫這篇文章時，實驗發現在第一條克隆命令下，子模塊也克隆成功了，看來這個要看運氣氣氣…

三、編譯及連接源代碼

爲實現教材中plotTrajectory.cpp，以支持3D繪圖的程序庫Pangolin爲例進行編譯。

1.安裝庫

首先，編譯中需要安裝很多庫，以下三條命令搞定（兄弟我一個庫踩一遍）：

#sudo
sudo apt install libglew-dev libxkbcommon-dev wayland-protocols python3-pip doxygen graphviz graphviz-doc
sudo apt install libboost-dev libboost-thread-dev libboost-filesystem-dev
#pip3
pip3 install pytest

2.編譯

然後，切換到目錄～/slambook2/3rdparty/Pangolin，執行以下命令：

simon@bert:~/slam/slambook2/3rdparty/Pangolin$ mkdir build
simon@bert:~/slam/slambook2/3rdparty/Pangolin$ cd build/
simon@bert:~/slam/slambook2/3rdparty/Pangolin/build$ cmake ../
#or make，此步可根據報錯調試程序
simon@bert:~/slam/slambook2/3rdparty/Pangolin/build$ cmake --build .
simon@bert:~/slam/slambook2/3rdparty/Pangolin/build$ sudo make install

3.修改CMakeLists.txt

修改cmake文件，並在代碼目錄執行cmake和make，我的CMakeLists.txt內容如下所示：

# 聲明要求的cmake最低版本
cmake_minimum_required(VERSION 2.8)

# 聲明一個cmake工程
project(plotTrajectory)

include_directories("/usr/include/eigen3")

find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})

# 添加一個可執行程序
# 語法：add_executable(程序名 源代碼文件)
add_executable(plotTrajectory plotTrajectory.cpp)

target_link_libraries(plotTrajectory ${Pangolin_LIBRARIES})

# 把工程調爲Debug編譯模式
set(CMAKE_BUILD_TYPE "Debug")

4.執行結果

運行目標程序，在build目錄下，執行：./plotTrajectory，運行結果截圖爲：

四、顯示相機的位姿

除了顯示軌跡，我們也可以顯示3D窗口中相機的位姿。在此程序中，我們以可視化的形式演示相機位姿的各種表達方式。當讀者用鼠標操作相機時，左側的方框會實時顯示相機位姿對應的旋轉矩陣、平移、歐拉角和四元數，可以看到數據是如何變化的。
程序在目錄/slambook2/ch3/visualizeGeometry/，CMakeLists.txt文件內容爲：

cmake_minimum_required( VERSION 2.8 )
project( visualizeGeometry )

set(CMAKE_CXX_FLAGS "-std=c++11")

# 添加Eigen頭文件
include_directories( "/usr/include/eigen3" )

# 添加Pangolin依賴
find_package( Pangolin )
include_directories( ${Pangolin_INCLUDE_DIRS} )

add_executable( visualizeGeometry visualizeGeometry.cpp )
target_link_libraries( visualizeGeometry ${Pangolin_LIBRARIES} )

編譯步驟如下：

simon@bert:~/slam/slambook2/ch3/visualizeGeometry$ mkdir build
simon@bert:~/slam/slambook2/ch3/visualizeGeometry$ cd build/
simon@bert:~/slam/slambook2/ch3/visualizeGeometry/build$ cmake ..
simon@bert:~/slam/slambook2/ch3/visualizeGeometry/build$ make

執行目標文件./visualizeGeometry，如下圖所示：
至此，整篇完結，有問題歡迎留言討論。

附1：plotTrajectory.cpp程序代碼：

#include<pangolin/pangolin.h>
#include<eigen3/Eigen/Core>
#include<unistd.h>
using namespace std;
using namespace Eigen;

string trajectory_file = "../trajectory.txt";

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

int main(int argc, char **argv){
    vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
    ifstream fin(trajectory_file);
    if (!fin) {
        cout << "cannot find trajectory file at " << trajectory_file <<endl;
        return 1;
    }
    
    while (!fin.eof()) {
        double time, tx, ty, tz, qx, qy, qz, qw;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Isometry3d Twr(Quaterniond(qw, qx, qy, qz));
        Twr.pretranslate(Vector3d(tx, ty, tz));
        poses.push_back(Twr);
    }
    cout << "read total"<<poses.size()<<" pose entries"<<endl;
    
    DrawTrajectory(poses);
    return 0;
}

void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
    pangolin::CreateWindowAndBind("Trajectory viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    
    pangolin::OpenGlRenderState s_cam(pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
                                      pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0));
    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));
        
    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
        glLineWidth(2);
        for (size_t i=0; i<poses.size(); i++) {
            Vector3d Ow = poses[i].translation();
            Vector3d Xw = poses[i]*(0.1*Vector3d(1, 0, 0));
            Vector3d Yw = poses[i]*(0.1*Vector3d(0, 1, 0));
            Vector3d Zw = poses[i]*(0.1*Vector3d(0, 0, 1));
            glBegin(GL_LINES);
            glColor3f(1.0, 0.0, 0.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Xw[0], Xw[1], Xw[2]);
            glColor3f(0.0, 1.0, 0.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Yw[0], Yw[1], Yw[2]);
            glColor3f(0.0, 0.0, 1.0);
            glVertex3d(Ow[0], Ow[1], Ow[2]);
            glVertex3d(Zw[0], Zw[1], Zw[2]);
            glEnd();
        }
        
        for (size_t i=0; i<poses.size(); i++) {
            glColor3f(0.0, 0.0, 0.0);
            glBegin(GL_LINES);
            auto p1=poses[i], p2=poses[i+1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        
        pangolin::FinishFrame();
        usleep(5000);
    }
}

附2：機器人座標文件：trajectory.txt
附3：visualizeGeometry.cpp程序代碼：

#include <iostream>
#include <iomanip>

using namespace std;

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

using namespace Eigen;

#include <pangolin/pangolin.h>

struct RotationMatrix {
  Matrix3d matrix = Matrix3d::Identity();
};

ostream &operator<<(ostream &out, const RotationMatrix &r) {
  out.setf(ios::fixed);
  Matrix3d matrix = r.matrix;
  out << '=';
  out << "[" << setprecision(2) << matrix(0, 0) << "," << matrix(0, 1) << "," << matrix(0, 2) << "],"
      << "[" << matrix(1, 0) << "," << matrix(1, 1) << "," << matrix(1, 2) << "],"
      << "[" << matrix(2, 0) << "," << matrix(2, 1) << "," << matrix(2, 2) << "]";
  return out;
}

istream &operator>>(istream &in, RotationMatrix &r) {
  return in;
}

struct TranslationVector {
  Vector3d trans = Vector3d(0, 0, 0);
};

ostream &operator<<(ostream &out, const TranslationVector &t) {
  out << "=[" << t.trans(0) << ',' << t.trans(1) << ',' << t.trans(2) << "]";
  return out;
}

istream &operator>>(istream &in, TranslationVector &t) {
  return in;
}

struct QuaternionDraw {
  Quaterniond q;
};

ostream &operator<<(ostream &out, const QuaternionDraw quat) {
  auto c = quat.q.coeffs();
  out << "=[" << c[0] << "," << c[1] << "," << c[2] << "," << c[3] << "]";
  return out;
}

istream &operator>>(istream &in, const QuaternionDraw quat) {
  return in;
}

int main(int argc, char **argv) {
  pangolin::CreateWindowAndBind("visualize geometry", 1000, 600);
  glEnable(GL_DEPTH_TEST);
  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1000, 600, 420, 420, 500, 300, 0.1, 1000),
    pangolin::ModelViewLookAt(3, 3, 3, 0, 0, 0, pangolin::AxisY)
  );

  const int UI_WIDTH = 500;

  pangolin::View &d_cam = pangolin::CreateDisplay().
    SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -1000.0f / 600.0f).
    SetHandler(new pangolin::Handler3D(s_cam));

  // ui
  pangolin::Var<RotationMatrix> rotation_matrix("ui.R", RotationMatrix());
  pangolin::Var<TranslationVector> translation_vector("ui.t", TranslationVector());
  pangolin::Var<TranslationVector> euler_angles("ui.rpy", TranslationVector());
  pangolin::Var<QuaternionDraw> quaternion("ui.q", QuaternionDraw());
  pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));

  while (!pangolin::ShouldQuit()) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    d_cam.Activate(s_cam);

    pangolin::OpenGlMatrix matrix = s_cam.GetModelViewMatrix();
    Matrix<double, 4, 4> m = matrix;

    RotationMatrix R;
    for (int i = 0; i < 3; i++)
      for (int j = 0; j < 3; j++)
        R.matrix(i, j) = m(j, i);
    rotation_matrix = R;

    TranslationVector t;
    t.trans = Vector3d(m(0, 3), m(1, 3), m(2, 3));
    t.trans = -R.matrix * t.trans;
    translation_vector = t;

    TranslationVector euler;
    euler.trans = R.matrix.eulerAngles(2, 1, 0);
    euler_angles = euler;

    QuaternionDraw quat;
    quat.q = Quaterniond(R.matrix);
    quaternion = quat;

    glColor3f(1.0, 1.0, 1.0);

    pangolin::glDrawColouredCube();
    // draw the original axis
    glLineWidth(3);
    glColor3f(0.8f, 0.f, 0.f);
    glBegin(GL_LINES);
    glVertex3f(0, 0, 0);
    glVertex3f(10, 0, 0);
    glColor3f(0.f, 0.8f, 0.f);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 10, 0);
    glColor3f(0.2f, 0.2f, 1.f);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 0, 10);
    glEnd();

    pangolin::FinishFrame();
  }
}

SLAM 顯示機器人運動軌跡

視覺SLAM十四講第2版顯示機器人運動軌跡及相機位姿

基於3.7可視化演示，運行plotTrajectory.cpp操作實錄（詳細填坑流程）

一、前期準備

二、git管理子模塊及克隆源代碼

1.學習使用Git Submodule

2.克隆源代碼

1. 下載失敗

2.github提速

1.修改 hosts

2.Use SSH

3.掛代理

4.碼雲

3.執行克隆

三、編譯及連接源代碼

1.安裝庫

2.編譯

3.修改CMakeLists.txt

4.執行結果

四、顯示相機的位姿

如何基於surging跨網關跨語言進行緩存降級

2024合集

程序員天天 CURD，怎麼才能成長，職業發展的思考(2)

移位操作搞定兩數之商

教你用Perl實現Smgp協議

如何通過前端表格控件在10分鐘內完成一張分組報表？

win11關閉自動檢測病毒刪文件

通用代碼生成器簡介

lightdb 單機模式下數據庫平移

千兆寬帶實際網速能到達多少？

三維空間剛體運動1：旋轉矩陣與變換矩陣

SLAM 顯示機器人運動軌跡

JupyterNotebook設置不同Python啓動環境

[轉載]拉格朗日乘子法如何理解？

三維空間剛體運動2：旋轉向量與羅德里格斯公式

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結

SLAM 顯示機器人運動軌跡

視覺SLAM十四講 第2版 顯示機器人運動軌跡及相機位姿

基於3.7可視化演示，運行plotTrajectory.cpp操作實錄（詳細填坑流程）

一、前期準備

二、git管理子模塊及克隆源代碼

1.學習使用Git Submodule

2.克隆源代碼

1. 下載失敗

2.github提速

1.修改 hosts

2.Use SSH

3.掛代理

4.碼雲

3.執行克隆

三、編譯及連接源代碼

1.安裝庫

2.編譯

3.修改CMakeLists.txt

4.執行結果

四、顯示相機的位姿

視覺SLAM十四講第2版顯示機器人運動軌跡及相機位姿