视觉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.执行结果

四、显示相机的位姿

基于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，如下图所示：
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();
  }
}

shao918516

发布了7 篇原创文章 · 获赞 18 · 访问量 2166

私信关注

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 显示机器人运动轨迹

视觉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版显示机器人运动轨迹及相机位姿