热门标签
热门文章
- 1System.InvalidCastException:“无法将类型为“Microsoft.Office.Interop.Excel.ApplicationClass”的 COM 对象强制转换为接口类
- 2HTC vive pro & unity开发者软件/SDK版本注意事项_htc vive unity配置
- 3php毕业设计 基于php+mysql的在线购物商城毕业设计开题报告功能参考_php mysql navicat hbuilder购买系统开题报告
- 4UE蓝图 函数调用(CallFunction)节点和源码
- 5【毕业设计】基于微信小程序体育场地预约系统（源码+LW+部署+讲解）_基于微信小程序体育馆预约系统
- 6mysql 安装问题：由于找不到MSVCP120.dll,无法继续执行代码.重新安装程序可能会解决此问题。_mysql找不到msvcp120
- 7sdkmanager工具安装
- 8redis持久化失败问题（MISCONF Redis is configured to save RDB snapshots, but ......）问题解决
- 9计算机系统：EEPROM详解
- 10虚拟机Ubuntu18.04为Jetson AGX Xavier使用jetpack4.4刷机_agx orin装jetpack4。4
当前位置: article > 正文
SLAM14-3讲_slam读取文件显示轨迹
作者：你好赵伟 | 2024-02-18 00:57:23
赞
踩
slam读取文件显示轨迹
SLAM14-3讲
一、实践Eigen
1.环境
# 若安装速度慢可以在网络配置中更换软件源，选择最为合适的软件源
sudo apt-get install libeigen3-dev
# 更新
sudo updatedb
# 定位Eigen3库安装位置
locate eigen3
- 1
- 2
- 3
- 4
- 5
- 6
# 定位eigen3库头文件位置
/usr/local/include/eigen3
- 1
- 2
注： Eigen库只有头文件，因此不需要在Cmake中使用target_link_libraries程序链接至库上
2.代码
#include <iostream> #include <ctime> using namespace std; // Eigen part #include <Eigen/Core> //Algebraic operation of dense matrix #include <Eigen/Dense> #define MATRIX_SIZE 60 int main(int argc,char* argv[]) { // Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列 // 声明一个2*3的float矩阵 Eigen::Matrix<float, 2, 3> matrix_23; // 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix // 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量 Eigen::Vector3d v_3d; // 这是一样的 Eigen::Matrix<float,3,1> vd_3d; // Matrix3d 实质上是 Eigen::Matrix<double, 3, 3> Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零 // 如果不确定矩阵大小，可以使用动态大小的矩阵 Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic; // 更简单的 Eigen::MatrixXd matrix_x; // 这种类型还有很多，我们不一一列举 // 下面是对Eigen阵的操作 // 输入数据（初始化） matrix_23 << 1, 2, 3, 4, 5, 6; // 输出 cout << matrix_23 << 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; vd_3d << 4,5,6; // 但是在Eigen里你不能混合两种不同类型的矩阵(矩阵中元素 的数据类型 不同)，像这样是错的 // Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d; // 应该显式转换 Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d; cout << result << endl; // 随机数矩阵 matrix_33 = Eigen::Matrix3d::Random(); cout << matrix_33 << endl; cout << matrix_33.transpose() << endl; // 转置 cout << matrix_33.sum() << endl; // 各元素和 cout << matrix_33.trace() << endl; // 迹 cout << 10 * matrix_33 << endl; // 数乘 cout << matrix_33.inverse() << endl; // 逆 cout << matrix_33.determinant() << endl; // 行列式 // 特征求解1 // 实对称矩阵可以保证对角化成功(实对此矩阵是对角化的充分条件) Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33.transpose()*matrix_33 );// 自伴随特征求解 cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl; cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl; // 特征求解2 Eigen::EigenSolver<Eigen::Matrix3d> eigen_solve_2(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 这个方程 // MATRIX_SIZE 大小在前边的宏里定义，它由随机数生成 Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > matrix_NN; matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );//随机数矩阵 Eigen::Matrix< double, MATRIX_SIZE, 1> v_Nd; v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 ); clock_t time_stt = clock(); // 计时 // 直接求逆 (最直接的，但是求逆运算量大) x = (matrix_NN)-1 * v_Nd; Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd; cout <<"time use in normal inverse is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl; // cout << x.transpose() <<endl; // 通常用矩阵分解来求，例如QR分解，速度会快很多 time_stt = clock(); x = matrix_NN.colPivHouseholderQr().solve(v_Nd); cout <<"time use in Qr decomposition is " <<1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl; return 0; }
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
3.CMakeLists.txt
# Cmake最低版本要求 cmake_minimum_required(VERSION 3.20) # 项目名称 project(ch3) # C++ 标准 set(CMAKE_CXX_STANDARD 14) # 单配置生成器CMAKE_BUILD_TYPE 控制生成构建系统使用的配置变量 是 CMAKE_BUILD_TYPE 。该变量默认为空，CMake识别的值为: # Debug：用于在没有优化的情况下，使用带有调试符号构建库或可执行文件。 # Release：用于构建的优化的库或可执行文件，不包含调试符号。 # RelWithDebInfo：用于构建较少的优化库或可执行文件，包含调试符号。 # MinSizeRel：用于不增加目标代码大小的优化方式，来构建库或可执行文件。 set( CMAKE_BUILD_TYPE "Release" ) # CMAKE_CXX_FLAGS变量针对C++编译器，后设置参数-O[n]可优化源代码 set( CMAKE_CXX_FLAGS "-O3" ) # 添加Eigen头文件 include_directories( "/usr/local/include/eigen3" ) # 添加可执行文件（程序名 源代码） add_executable(useEigen main.cpp)
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
4.运行结果
/home/zxz/my_slam14/ch3/useEigen/cmake-build-debug/useEigen 1 2 3 4 5 6 1 2 3 4 5 6 10 28 0.680375 0.59688 -0.329554 -0.211234 0.823295 0.536459 0.566198 -0.604897 -0.444451 0.680375 -0.211234 0.566198 0.59688 0.823295 -0.604897 -0.329554 0.536459 -0.444451 1.61307 1.05922 6.80375 5.9688 -3.29554 -2.11234 8.23295 5.36459 5.66198 -6.04897 -4.44451 -0.198521 2.22739 2.8357 1.00605 -0.555135 -1.41603 -1.62213 3.59308 3.28973 0.208598 Eigen values = 0.0242899 0.992154 1.80558 Eigen vectors = -0.549013 -0.735943 0.396198 0.253452 -0.598296 -0.760134 -0.796459 0.316906 -0.514998 Eigen values = 0.0242899 0.992154 1.80558 Eigen vectors = -0.549013 -0.735943 0.396198 0.253452 -0.598296 -0.760134 -0.796459 0.316906 -0.514998 time use in normal inverse is 0.127ms time use in Qr decomposition is 0.08ms 进程已结束，退出代码为 0
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
二、Eigen集合模块Geometry
1.代码
#include <iostream> #include <cmath> using namespace std; #include <Eigen/Core> // Eigen 几何模块 #include <Eigen/Geometry> int main(int argc,char* argv[]) { // Eigen/Geometry 模块提供了各种旋转和平移的表示 // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d ::Identity();//单位矩阵 // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符） // PI/4表示旋转的角度，Eigen::Vector3d(0,0,1)表示旋转轴 沿Z轴旋转45度 Eigen::AngleAxisd rotation_vector(M_PI/4,Eigen::Vector3d(0,0,1)); // 浮点数的精度值 cout输出格式 进行设置 cout.precision(3); // 旋转向量通过矩阵的形式输出 cout << "rotation_vector = \n" << rotation_vector.matrix() << endl; // 旋转向量也可以通过直接赋值的方式进行旋转矩阵的转换 rotation_matrix = rotation_vector.toRotationMatrix(); cout << "rotation_matrix = \n" << rotation_matrix << endl; // 用 AngleAxis(轴角) 可以进行坐标变换 v is 3*1 matrix Eigen::Vector3d v(1,0,0); // 向量v绕着z轴旋转45度 的坐标是什么 Eigen::Vector3d v_rotated = rotation_vector * v; cout << "(1,0,0) after roation = " << v_rotated.transpose() << endl; cout << "(1,0,0) after roation = " << (rotation_matrix * v).transpose() << endl; // 欧拉角: 可以将旋转矩阵直接转换成欧拉角 Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2,1,0); // ZYX顺序，即yaw pitch roll顺序 cout << " yaw pitch roll = " << euler_angles.transpose() << endl; // 欧氏变换矩阵使用 Eigen::Isometry 欧式变换矩阵 即 变换矩阵 书中 P43 // 向量由非齐次坐标变为齐次坐标，从三维向量变成四维向量，同理其旋转矩阵为3*3也应该增加一个维度 变为 4*4 Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); // 虽然称为3d，实质上是4＊4的矩阵 // 变换矩阵中包其中包含旋转矩阵与平移向量的信息 T.rotate(rotation_vector); // 按照旋转向量rotation_vector进行旋转 // T.rotate(rotation_matrix); // 同样可以通过旋转矩阵旋转 T.pretranslate(Eigen::Vector3d(1,3,4)); // 把平移向量设成(1,3,4) cout << "Transform matrix = \n" << T.matrix() << endl; // 用变换矩阵进行坐标变换 Eigen::Vector3d v_transformed = T * v; // 因为变换矩阵中包含 旋转矩阵R与平移向量t的信息所以 T*v 相当于 R*v+t cout << "v transformed = " << v_transformed.transpose() << endl; // 四元数 // 可以直接把AngleAxis (旋转向量)赋值给四元数 反之亦然 旋转矩阵同样可以 Eigen::Quaterniond q = Eigen::Quaterniond (rotation_vector); cout << "quaternion = \n" << q.coeffs() << endl; // 注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部 // 旋转矩阵赋值给四元数 q = Eigen::Quaterniond (rotation_matrix); cout << "quaternion = \n" << q.coeffs() << endl; // 使用四元数旋转一个向量，使用重载的乘法即可 v_rotated = q * v; // 数学上是qvq^{-1} 该处对乘法进行了重载 cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl; return 0; }
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
2.CMakeLists.txt
cmake_minimum_required(VERSION 3.16)
project(useGeometry)
set(CMAKE_CXX_STANDARD 14)
# 添加头文件
include_directories("/usr/local/include/eigen3")
add_executable(useGeometry main.cpp)
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
3.运行结果
/home/zxz/my_slam14/ch3/useGeometry/cmake-build-debug/useGeometry rotation_vector = 0.707 -0.707 0 0.707 0.707 0 0 0 1 rotation_matrix = 0.707 -0.707 0 0.707 0.707 0 0 0 1 (1,0,0) after roation = 0.707 0.707 0 (1,0,0) after roation = 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 = 0 0 0.383 0.924 quaternion = 0 0 0.383 0.924 (1,0,0) after rotation = 0.707 0.707 0 进程已结束，退出代码为 0
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
三、测试四元数的使用
注意：
四元数使用前需要进行归一化，即对四元数进行单位化，单位四元数表示三维空间中的任意旋转：
a.类似于 二维平面的复数，可表示2D的旋转与缩放，对复数进行单位化之后，则该复数缩放效果则为1倍，即没有缩放效果，只留下旋转
b.若不对四元数进行归一化，得到的旋转矩阵可能非正交
要求：
题目要求在slam14讲第二版书中P64
1.代码
#include <iostream> #include <cmath> using namespace std; #include <Eigen/Core> // Eigen 几何模块 #include <Eigen/Geometry> int main(int argc,char **argv) { // 四元数表示旋转 Eigen::Quaterniond q1 = Eigen::Quaterniond(0.35,0.2,0.3,0.1); Eigen::Quaterniond q2 = Eigen::Quaterniond(-0.5,0.4,-0.1,0.2); // 对四元数进行归一化操作 q1.normalize(); q2.normalize(); // 平移分量 Eigen::Vector3d t1 = Eigen::Vector3d(0.3,0.1,0.1); Eigen::Vector3d t2 = Eigen::Vector3d(-0.1,0.5,0.3); // 需要进行坐标系转换的坐标 Eigen::Vector3d p1 = Eigen::Vector3d(0.5,0,0.2); // 将四元数转换为旋转矩阵 Eigen::Matrix3d r_m1 = q1.toRotationMatrix(); Eigen::Matrix3d r_m2 = q2.toRotationMatrix(); // 打印r_m1 与 其转置 逆 cout << "q1 对应的旋转矩阵: \n"<< r_m1 << endl; cout << "q1 对应的旋转矩阵的转置: \n"<< r_m1.transpose() << endl; cout << "q1 对应的旋转矩阵的逆: \n"<< r_m1.inverse() << endl; /******对比结果发现，归一化的四元数转换的旋转矩阵为正交，若不进行归一化则不会正交*******/ // 打印r_m2 与 其转置 逆 cout << "q2 对应的旋转矩阵: \n"<< r_m2 << endl; cout << "q2 对应的旋转矩阵的转置: \n"<< r_m2.transpose() << endl; cout << "q2 对应的旋转矩阵的逆: \n"<< r_m2.inverse() << endl; // 欧式变换矩阵 // 1.变换矩阵通过四元数加入旋转分量 Eigen::Isometry3d T1w = Eigen::Isometry3d(q1); Eigen::Isometry3d T2w = Eigen::Isometry3d(q2); // 2.将平移分量加入旋转矩阵 T1w.pretranslate(t1); T2w.pretranslate(t2); // 则坐标p1在小萝卜二号坐标系下的坐标(通过T1w.inverse() 表示一个相反的旋转，将坐标转换之世界坐标系下) Eigen::Vector3d p2 = T2w * T1w.inverse() * p1; cout << "坐标p1在小萝卜二号坐标系下的坐标:\n" << p2.transpose() << endl; return 0; }
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
2.CMakeLists.txt
cmake_minimum_required(VERSION 3.20)
project(test)
set(CMAKE_CXX_STANDARD 14)
include_directories("/usr/local/include/eigen3")
add_executable(test main.cpp)
- 1
- 2
- 3
- 4
- 5
- 6
- 7
3.运行结果
/home/zxz/my_slam14/ch3/test/cmake-build-debug/test q1 对应的旋转矩阵: 0.238095 0.190476 0.952381 0.72381 0.619048 -0.304762 -0.647619 0.761905 0.00952381 q1 对应的旋转矩阵的转置: 0.238095 0.72381 -0.647619 0.190476 0.619048 0.761905 0.952381 -0.304762 0.00952381 q1 对应的旋转矩阵的逆: 0.238095 0.72381 -0.647619 0.190476 0.619048 0.761905 0.952381 -0.304762 0.00952381 q2 对应的旋转矩阵: 0.782609 0.26087 0.565217 -0.608696 0.130435 0.782609 0.130435 -0.956522 0.26087 q2 对应的旋转矩阵的转置: 0.782609 -0.608696 0.130435 0.26087 0.130435 -0.956522 0.565217 0.782609 0.26087 q2 对应的旋转矩阵的逆: 0.782609 -0.608696 0.130435 0.26087 0.130435 -0.956522 0.565217 0.782609 0.26087 坐标p1在小萝卜二号坐标系下的坐标: -0.0309731 0.73499 0.296108
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
四、显示运动轨迹
1.环境配置
// 将slambook下3rdparty下的Pangolin.tar.gz进行解压
// 进入解压的文件
cd [path-to-pangolin]
mkdir build
cd build
cmake ..
make
- 1
- 2
- 3
- 4
- 5
- 6
- 7
报错：
home/zxz/Downloads/Pangolin/include/pangolin/video/drivers/ffmpeg.h:94:5: error: ‘PixelFormat’ does not name a type; did you mean ‘AVPixelFormat’?
94 | PixelFormat fmtout;
| ^~~~~~~~~~~
| AVPixelFormat
/home/zxz/Downloads/Pangolin/include/pangolin/video/drivers/ffmpeg.h:142:5: error: ‘PixelFormat’ does not name a type; did you mean ‘AVPixelFormat’?
142 | PixelFormat fmtsrc;
| ^~~~~~~~~~~
| AVPixelFormat
/home/zxz/Downloads/Pangolin/include/pangolin/video/drivers/ffmpeg.h:143:5: error: ‘PixelFormat’ does not name a type; did you mean ‘AVPixelFormat’?
143 | PixelFormat fmtdst;
| ^~~~~~~~~~~
| AVPixelFormat
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
解决报错：
// 在pangolin/src文件夹下，找到CMakeLists.txt文件，打开，并在266行将以下代码注释。
find_package(FFMPEG QUIET)
if(BUILD_PANGOLIN_VIDEO AND FFMPEG_FOUND)
set(HAVE_FFMPEG 1)
list(APPEND INTERNAL_INC ${FFMPEG_INCLUDE_DIRS} )
list(APPEND LINK_LIBS ${FFMPEG_LIBRARIES} )
list(APPEND HEADERS ${INCDIR}/video/drivers/ffmpeg.h)
list(APPEND SOURCES video/drivers/ffmpeg.cpp)
message(STATUS "ffmpeg Found and Enabled")
endif()
// 删除之前的build目录，重新创建，重新cmake
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
注意：下载高博的slambook2源码：
git clone https://github.com/gaoxiang12/slambook2
- 1
2.代码
参考文章：
https://blog.csdn.net/Ai_July/article/details/104929300
https://blog.csdn.net/weixin_43991178/article/details/105119610
- 1
- 2
// main.cpp #include <pangolin/pangolin.h> #include <Eigen/Core> #include <unistd.h> // 本例演示了如何画出一个预先存储的轨迹 using namespace std; using namespace Eigen; //trajectory文件的路径（可以使用相对路径，也可以使用绝对路径） 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;//容器的方式存储 //因为类型是Eigen的Isometry3d和cpp的类内存分配不一样，所以要指定内存的分配方式 //即:Eigen::aligned_allocator< Isometry3d > ifstream fin(trajectory_file); //读文件 //未成功读取 if (!fin) { cout << "cannot find trajectory file at " << trajectory_file << endl; return 1; } //一行一行读取，直到文件尾标志（efo标志） 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; // 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);//启用深度渲染，当需要显示3D模型时需要打开，根据目标的远近自动隐藏被遮挡的模型 glEnable(GL_BLEND);//表示窗口使用颜色混合模式，让物体显示半透明效 glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); /*** 设置颜色RGBA混合因子,前面参数表示源因子，后面参数表示目标因子 GL_ZERO：表示使用0.0作为权重因子 GL_ONE ： 表示使用1.0作为权重因子 GL_SRC_ALPHA ：表示使用源颜色的alpha值来作为权重因子 GL_DST_ALPHA ：表示使用目标颜色的alpha值来作为权重因子 GL_ONE_MINUS_SRC_ALPHA： 表示用1.0-源颜色的alpha的值来作为权重因子 GL_ONE_MINUS_DST_ALPHA： 表示用1.0-目标颜色的alpha的值来作为权重因子 GL_SRC_COLOR>：表示用源颜色的四个分量作为权重因子 在画图的过程中如果程序glClearColor()；glColor3f()则后者为源颜色，前者的颜色为目标颜色以上的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) ); //ProjectionMatrix()设置相机内参,参数分别表示相机分辨率(2)，焦距(1)，相机光心(3)，最远最大距离(2) //ModelViewLookAt()设置观看视角,上文对应的意思是在世界坐标(0，-0.1，-1.8)处观看坐标原点（0,0,0）并设置Y轴向上 /** 另一种解释 定义观测方位向量：观测点位置：(mViewpointX mViewpointY mViewpointZ) 观测目标位置：(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)); /**创建交互视图view,用于显示上一步摄像机所“拍摄”到的内容，,setBounds()函数前四个参数依次表示视图在视窗中的范围（下、上、左、右）,显示界面长宽比*/ 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));//0.1是为了让图看起来舒服，不会太大 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); // sleep 5 ms } }
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
3.CMakeLists.txt
cmake_minimum_required(VERSION 3.20)
project(examples)
set(CMAKE_CXX_STANDARD 14)
include_directories("/usr/local/include/eigen3")
# 添加pangolin依赖
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
add_executable(plotTrajectory main.cpp)
target_link_libraries(plotTrajectory ${Pangolin_LIBRARIES})
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
4.运行效果
声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/你好赵伟/article/detail/103053
推荐阅读
相关标签
