视觉SLAM十四讲笔记-3-03
文章目录
参考链接： link，
高翔,张涛,等. 视觉 SLAM 十四讲:从理论到实践[M]. 电子工业出版社, 2019.
3.6 实践：Eigen几何模块
3.6.1 Eigen几何模块的数据演示
新建 useGeometry 文件夹，在该文件夹下打开 VS Code
mkdir useGeometry
cd useGeometry
code .
然后在该文件夹下新建 launch.json 文件和 tasks.json 文件。
直接参考这篇博客 link 中的步骤，进行创建。
//launch.json
{
// Use IntelliSense to learn about possible attributes.
// Hover to view descriptions of existing attributes.
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
"version": "0.2.0",
"configurations": [
{
"name": "g++ - 生成和调试活动文件",
"type": "cppdbg",
"request":"launch",
"program":"${workspaceFolder}/build/useGeometry",
"args": [],
"stopAtEntry": false,
"cwd": "${workspaceFolder}",
"environment": [],
"externalConsole": false,
"MIMode": "gdb",
"setupCommands": [
{
"description": "为 gdb 启动整齐打印",
"text": "-enable-pretty-printing",
"ignoreFailures": true
}
],
"preLaunchTask": "Build",
"miDebuggerPath": "/usr/bin/gdb"
}
]
}
//tasks.json
{
"version": "2.0.0",
"options":{
"cwd": "${workspaceFolder}/build" //指明在哪个文件夹下做下面这些指令
},
"tasks": [
{
"type": "shell",
"label": "cmake", //label就是这个task的名字,这个task的名字叫cmake
"command": "cmake", //command就是要执行什么命令,这个task要执行的任务是cmake
"args":[
".."
]
},
{
"label": "make", //这个task的名字叫make
"group": {
"kind": "build",
"isDefault": true
},
"command": "make", //这个task要执行的任务是make
"args": [
]
},
{
"label": "Build",
"dependsOrder": "sequence", //按列出的顺序执行任务依赖项
"dependsOn":[ //这个label依赖于上面两个label
"cmake",
"make"
]
}
]
}
由于要使用 eigen 库，因此需要在 CMakeLists.txt 中将 eigen 的头文件添加进去，这里直接使用绝对路径,添加完的 CMakeLists.txt 如下:
cmake_minimum_required(VERSION 3.0)
project(EIGENMATRIX)
#在g++编译时，添加编译参数，比如-Wall可以输出一些警告信息
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
#一定要加上这句话，加上这个生成的可执行文件才是可以Debug的，不然不加或者是Release的话生成的可执行文件是无法进行调试的
set(CMAKE_BUILD_TYPE Debug)
#添加头文件
include_directories(include)
include_directories("/usr/include/eigen3")
#或者通过绝对路径来进行添加，include_directories(${CMAKE_SOURCE_DIR}/include)
#其中${CMAKE_SOURCE_DIR}就是VSCODE_C_LEARNING_3这个文件夹的绝对路径
add_executable(useGeometry useGeometry.cpp)
然后写个简单的 main 函数测试下，保存，按下 F5 发现可以正常编译。
开始学习本节代码。
#include <iostream>
#include <cmath>
using namespace std;
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace Eigen;
//本程序演示了 Eigen 几何模块的使用方法
int main()
{
//Eigen/Geometry模块提供了各种旋转和平移的表示
//3D旋转矩阵直接使用Matrix3d和Matrix3f,赋值为单位阵
Matrix3d rotation_matrix = Matrix3d::Identity();
//cout << "rotation_matrix:" << rotation_matrix << endl;
//旋转向量使用AngleAxis,它底层不直接是Matrix,但运算可以当做矩阵(因为重载了运算符)
AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1)); //沿z轴旋转了45度
cout.precision(3);
cout << "rotation matrix = \n" << rotation_vector.matrix() << endl; //用matrix()转换为矩阵
//阵也可以直接赋值
rotation_matrix = rotation_vector.toRotationMatrix();
//用AngleAxis可以进行坐标变换,Vector3d为Eigen::Matrix<double, 3, 1>
Vector3d v(1, 0, 0);
Vector3d v_rotated = rotation_vector * v;
cout << "(1,0,0) after rotation(by angle axis) = " << v_rotated.transpose() << endl;
//或者使用旋转矩阵
v_rotated = rotation_matrix * v;
cout << "(1,0,0) after rotation(by matrix) = " << v_rotated.transpose() << endl;
//欧拉角：可以将旋转矩阵直接转换成欧拉角
Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); //ZYX顺序， 即roll pitch yaw顺序
cout << "yaw pitch roll = " << euler_angles.transpose() << endl;
//欧氏变换矩阵使用Eigen::Isometry
Isometry3d T = Isometry3d::Identity(); //虽然称为3D，实质上是4*4的矩阵
T.rotate(rotation_vector); //按照rotation_vector进行旋转
T.pretranslate(Vector3d(1,3,4)); //把平移向量设为(1,3,4)
cout << "Transform matrix = \n" << T.matrix() << endl;
//用变换矩阵进行坐标变换
Vector3d v_transformed = T * v; //相当于 R * v + t
cout << "v transformed = " << v_transformed.transpose() << endl;
//对于仿射变换和射影变换，使用Eigen::Affine3d和Eigen::Projective3d即可，略
//四元数
//可以直接将AngleAxis赋值给四元数，反之亦然
Quaterniond q = Quaterniond(rotation_vector);
cout << "quaternion from rotation vector = " << q.coeffs().transpose() << endl; //请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
//也可以将旋转矩阵赋值给它
q = Quaterniond(rotation_matrix);
cout << "quaternion from rotation matrix= " << q.coeffs().transpose() << endl;
//使用四元数旋转一个向量，使用重载的乘法即可
v_rotated = q * v; //注意数学上是 qvq^{-1}
cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
//用常规向量乘法表示，则计算如下啊：
cout << "should ba equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;
return 0;
}
按下 F5 输出：
rotation matrix =
0.707 -0.707 0
0.707 0.707 0
0 0 1
(1,0,0) after rotation(by angle axis) = 0.707 0.707 0
(1,0,0) after rotation(by matrix) = 0.707 0.707 0
yaw pitch roll = 0.785 -0 0
Transform matrix =
0.707 -0.707 0 1
0.707 0.707 0 3
0 0 1 4
0 0 0 1
v transformed = 1.71 3.71 4
quaternion from rotation vector = 0 0 0.383 0.924
quaternion from rotation matrix= 0 0 0.383 0.924
(1,0,0) after rotation = 0.707 0.707 0
should ba equal to 0.707 0.707 0 0
想进一步了解 Eigen 的几何模块的读者可以参考 link
3.6.2实际的坐标变换例子
根上述例子一样创建工程，然后编写下面程序，进行调试。
/*
小萝卜一号和小萝卜二号都位于世界坐标系下。记世界坐标系为 W，小萝卜们的坐标系分别为 R1 和 R2.
小萝卜一号的位姿为 q1 = [0.35, 0.2, 0.3, 0.1]^T, t1 = [0.3, 0.1, 0.1]^T
小萝卜二号的位姿为 q1 = [-0.5, 0.4, -0.1, 0.2]^T, t1 = [-0.1, 0.5, 0.3]^T
这里的q 和 t 表达的是 T_{R_[k}], W},k = 1,2,即世界坐标系到相机坐标系的变换关系
现在小萝卜一号看到某个点在自身坐标系下的坐标为 p_{R1} = [0.5, 0, 0.2]^T，求该向量在小萝卜二号坐标系下的坐标
*/
#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
using namespace Eigen;
int main()
{
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);
Isometry3d T1w(q1), T2w(q2);
T1w.pretranslate(t1);
T2w.pretranslate(t2);
Vector3d p2 = T2w * T1w.inverse() * p1;
cout << endl << p2.transpose() << endl;
return 0;
}
输出：
-0.0309731 0.73499 0.296108
3.7 可视化演示
3.7.1 显示运动轨迹
首先，通过某种方式记录了一个机器人的运动轨迹，现在想把它画在一个窗口中。假设轨迹文件存储在 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
其中， time 指该位姿的记录时间， t 为平移， q 为旋转四元数，均为世界坐标系到机器人坐标系记录。下面从文件中读取这些轨迹，并显示在一个窗口中。绘制轨迹：其实是机器人(相机)坐标系的原点在世界坐标系中的坐标。
考虑机器人坐标系的原点
O
R
O_{R}
OR，此时的
O
W
O_{W}
OW 就是这个原点在世界坐标系下的坐标。
O
W
=
T
W
R
∗
O
R
O_{W} = T_{WR} * O_{R}
OW=TWR∗OR
这里需要安装基于 OpenGL 的 Pangolin 库。
slam 14 讲中是从 link 处下载的 Pangolin，我也用以下命令进行安装：
//安装依赖库
sudo apt install libgl1-mesa-dev
sudo apt install libglew-dev
sudo apt install cmake
//建议安装的库
sudo apt install libpython2.7-dev
sudo apt install pkg-config
sudo apt install libegl1-mesa-dev libwayland-dev libxkbcommon-dev wayland-protocols
//安装源文件
git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
mkdir build
cd build
cmake ..
make
make install
但是在运行到
c
m
a
k
e
.
.
cmake ..
cmake.. 时会报错误：
在网上找了解决的办法，参考链接： link 但还是没有解决。
重新找思路，在其他群中看到一个 Pangolin 的版本，下载下来编译安装，成功安装上。
安装过程见 link
新建文件夹 plotTrajectory， 并在文件夹中打开 VS Code
mkdir plotTrajectory
cd plotTrajectory
code .
按照以前的流程建立工程，并建立文件：
创建 launch.json，tasks.json
//launch.json
{
// Use IntelliSense to learn about possible attributes.
// Hover to view descriptions of existing attributes.
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
"version": "0.2.0",
"configurations": [
{
"name": "g++ - 生成和调试活动文件",
"type": "cppdbg",
"request":"launch",
"program":"${workspaceFolder}/build/plotTrajectory",
"args": [],
"stopAtEntry": false,
"cwd": "${workspaceFolder}",
"environment": [],
"externalConsole": false,
"MIMode": "gdb",
"setupCommands": [
{
"description": "为 gdb 启动整齐打印",
"text": "-enable-pretty-printing",
"ignoreFailures": true
}
],
"preLaunchTask": "Build",
"miDebuggerPath": "/usr/bin/gdb"
}
]
}
//tasks.json
{
"version": "2.0.0",
"options":{
"cwd": "${workspaceFolder}/build" //指明在哪个文件夹下做下面这些指令
},
"tasks": [
{
"type": "shell",
"label": "cmake", //label就是这个task的名字,这个task的名字叫cmake
"command": "cmake", //command就是要执行什么命令,这个task要执行的任务是cmake
"args":[
".."
]
},
{
"label": "make", //这个task的名字叫make
"group": {
"kind": "build",
"isDefault": true
},
"command": "make", //这个task要执行的任务是make
"args": [
]
},
{
"label": "Build",
"dependsOrder": "sequence", //按列出的顺序执行任务依赖项
"dependsOn":[ //这个label依赖于上面两个label
"cmake",
"make"
]
}
]
}
编写 CMakeLists.txt，这里除了要添加 Eigen 的头文件外，还要添加 Panlogin 的头文件和库文件：
cmake_minimum_required(VERSION 3.0)
project(EIGENMATRIX)
#在g++编译时，添加编译参数，比如-Wall可以输出一些警告信息
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11")
#一定要加上这句话，加上这个生成的可执行文件才是可以Debug的，不然不加或者是Release的话生成的可执行文件是无法进行调试的
set(CMAKE_BUILD_TYPE Debug)
#添加头文件
include_directories(include)
#Eigen库只需要添加头文件
include_directories("/usr/include/eigen3")
#或者通过绝对路径来进行添加，include_directories(${CMAKE_SOURCE_DIR}/include)
#其中${CMAKE_SOURCE_DIR}就是VSCODE_C_LEARNING_3这个文件夹的绝对路径
#添加Pangolin依赖
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
add_executable(plotTrajectory plotTrajectory.cpp)
target_link_libraries(plotTrajectory ${Pangolin_LIBRARIES})
然后开始编写 plotTrajectory.cpp:
#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <unistd.h>
#include <iostream>
using namespace std;
using namespace Eigen;
//本程序演示了如何画出一个预先存储的轨迹
// path to trajectory file
string trajectory_file = "./trajectory.txt";
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);
int main()
{
vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
//虽然会爆红，但是不影响编译
ifstream fin(trajectory_file);
cout << "trajectory_file:" << trajectory_file << endl;
if(!fin)
{
cout << "cannot find trajectory file at " << trajectory_file << endl;
return 1;
}
//把每一个点的坐标及变换矩阵数据读进来，将数据弄成变换矩阵的形式存到vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses中
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 << "real total" << poses.size() << "pose entries" << endl;
//draw trajectory in pangolin
DrawTrajectory(poses);
return 0;
}
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses)
{
//create pangolin window and plot the trajectory
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);
}
}
运行结果：
3.7.2 显示相机的位姿
创建文件夹 visualizeGeometry ，并在该文件夹下打开 VS Code。
mkdir visualizeGeometry
cd visualizeGeometry/
code .
和 3.7.1 一样创建工程，并编写 launch.json 和 tasks.json, CMakeLists.txt，只不过在 launch.json 和 CMakeLists.txt 中将可执行文件的名字改为 visualizeGeometry。
然后编写 visualizeGeometry.cpp：
#include <iostream>
using namespace std;
#include <iomanip>
#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();
}
}
运行结果：
1718
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
