三维空间刚体运动5:详解SLAM中显示机器人运动轨迹及相机位姿(原理流程)
序:本篇系列文章参照高翔老师《视觉SLAM十四讲从理论到实践》,讲解三维空间刚体运动,为读者打下坚实的数学基础。博文将原第三讲分为五部分来讲解,其中四元数部分较多较复杂,又分为四部分。如果读者急于实践,可直接阅读第五部分的机器人运动轨迹,此部分详细讲解了安装准备工作。此系列总体目录如下:
- 旋转矩阵和变换矩阵;
- 旋转向量表示旋转;
- 欧拉角表示旋转;
- 四元数包括以下部分:
4-1. 四元数表示变换;
4-2. 四元数线性插值方法:LinEuler/LinMat/Lerp/Nlerp/Slerp;
4-3. 四元数多点插值方法:Squad;
4-4. 四元数多点连续解析解插值方法:Spicv;
4-5. 四元数多点离散数值解插值方法:Sping。 - 实践:SLAM中显示机器人运动轨迹及相机位姿。
最近学习无人驾驶,在网上看到了高翔老师的《视觉SLAM十四讲》,感觉不错,遂买来仔细研读。前边章节学习比较顺利,学到3.7节的程序时,运行碰到极大困难,卡了一周,碰到各种坑,所幸一一填平,写篇文章记录下来,以觞来者。博文分为六块知识点,边写边整理吧,欢迎留言交流。好了,闲话少述,直接进入正题。
一、显示运动轨迹原理讲解
在实验开始前,先了解一下实验的原理。
如果你是第一次接触旋转和平移这些概念,可能会觉得它们的形式看起来很复杂,因为每种表达方式都可以与其他方式互相转换,而转换公式有时还比较长,这部分知识可参考博主的三维空间刚体运动系列文章。虽然旋转矩阵、变换矩阵的数值可能不够直观,但我们可以很容易地把它们画在窗口里。
本篇演示两个可视化的例子。首先,假设通过某种方式记录了一个机器人的运动轨迹,现在想把它画在一个窗口中。假设运动轨迹存储于文件trajectory.txt,每一行用下面的格式存储:
t
i
m
e
,
t
x
,
t
y
,
t
z
,
q
x
,
q
y
,
q
z
,
q
w
time,t_{x},t_{y},t_{z},q_{x},q_{y},q_{z},q_{w}
time,tx,ty,tz,qx,qy,qz,qw其中,
t
i
m
e
time
time指该位姿记录的时间,
t
t
t为平移,
q
q
q为旋转四元数,均是以世界坐标系到机器人坐标系的记录。下面我们从文件中提取这些轨迹,并显示到一个窗口中。原则上,如果只是谈论“机器人的位姿”,那么你可以使用
T
W
R
T_{WR}
TWR或者
T
R
W
T_{RW}
TRW,事实上它们也只差一个逆而已。如果你想要存储机器人的轨迹,那么可以存储所有时刻的
T
W
R
T_{WR}
TWR或者
T
R
W
T_{RW}
TRW,区别不大。
在画轨迹的时候,我们可以把“轨迹”画成一系列点组成的序列,严格说来,这其实是机器人(相机)坐标系的原点在世界坐标系中的坐标。考虑机器人坐标系的原点
O
R
O_{R}
OR,此时的
O
W
O_{W}
OW就是这个原点在世界坐标系下的坐标:
O
W
=
T
W
R
O
R
=
t
W
R
O_{W}=T_{WR}O_{R}=t_{WR}
OW=TWROR=tWR这正是
T
W
R
T_{WR}
TWR的平移部分。因此,可以从
T
W
R
T_{WR}
TWR中直接看到相机在何处,这也是
T
W
R
T_{WR}
TWR更直观的原因。因此,在可视化程序里,轨迹文件存储了
T
W
R
T_{WR}
TWR而不是
T
R
W
T_{RW}
TRW。
最后,我们需要一个支持3D绘图的程序库。有许多库都支持3D绘图,比如大家熟悉的MATLAB,Python的Matploblib、OpenGL等。在Linux中,一个常见的库是基于OpenGL的Pangolin库,它在支持OpenGL的绘图操作基础之上还提供了一些GUI的功能。本书中,我们使用Git的submodule功能管理本书依赖的第三方库。读者可以进入3rdparty文件夹直接安装所需的库,Git保证作者和你的库是一致的。
二、前期准备
在之前的章节中,我们首先安装了Ubuntu 1804,学习了CMake基础语法及熟悉了VIM和IDE KDevelop的用法,下面给出所用资料链接(点击右上方数字即可):
1.Ubuntu 1804安装1,在window10安装ubuntu双系统,注意是非虚拟机安装哦。
2.CMake教学材料,主要有两部分:《cmake实践》2及官方指导文档3。
3.VIM命令学习4,几篇文章凑在一起的,不想仔细读的可以拉到后边看看简化版,掌握主要用法即可。
4.KDevelop5,这个稍微详细些,不用多看,先用起来。
三、git管理子模块及克隆源代码
1.学习使用Git Submodule
为了使用Git的submodule功能管理本书依赖的第三方库,专门学习了Git Submodule使用完整教程6,通过实操,基本掌握了 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.使用git的Use SSH
使用SSH,在github克隆或下载界面,点击Use SSH,如下图所示:
会给出如何获取SSH密钥的链接,按照官方文档一步步操作即可使用SSH下载。因为设置时没有截图,写文章时登出也找不到,故没放图片,有问题的可以在下方留言。
以上两种方法均可带来一定速度提升,Use SSH可能会快那么一丢丢,不过设置复杂一些。
3.码云
这篇文章发表后,学习opencv时,发现了一个很好用的github下载工具,码云,在此补充上。试过以上方法没用的小伙伴这次一定可以帮到你。具体方法参考以下文章:《git与码云连接起来》(由于审核问题,请小伙伴们自行搜索吧)。
3.执行克隆
克隆源代码命令如下:
git clone --recurse-submodules https://github.com/gaoxiang12/slambook2.git slambook2
使用SSH请把网址改为:git@github.com: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 git@github.com: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")
五、显示运动轨迹
运行目标程序,在build目录下,执行:./plotTrajectory,程序见附件,运行结果截图为:
图:位姿可视化结果
该程序演示了如何在Panglin中画出3D的位姿。我们用红、绿、蓝三种颜色画出每个位姿的三个坐标轴,然后用黑色线将轨迹连起来。
六、显示相机的位姿
除了显示轨迹,我们也可以显示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十四讲:从理论到实践》和《Quaternions, Interpolation and Animation》编写,但相对于原文会适当精简,同时为便于全面理解,会收集其他网络好文,根据作者理解,加入一些注解和扩展知识点,如果您觉得还不错,请一键四连(点赞关注收藏评论),让更多的人看到。
参考文献:
- 《视觉SLAM十四讲:从理论到实践》,高翔、张涛等著,中国工信出版社