SLAM系列——第三讲 三维空间刚体运动[2023.1]
2024-04-18 04:30:33
系列文章目录
SLAM系列——第一讲 预备知识[2023.1]
SLAM系列——第二讲 初识SLAM[2023.1]
SLAM系列——第三讲 三维空间刚体运动[2023.1]
SLAM系列——第四讲 李群李代数[2023.1]
SLAM系列——第五讲 相机与图像[2023.1]
SLAM系列——第六讲 非线性优化[2023.1]
SLAM系列——第七讲 视觉里程计1[2023.1]
SLAM系列——第八讲 视觉里程计2[2023.1]
SLAM系列——第九讲 后端1[2023.1]
SLAM系列——第十讲 后端2[2023.1]
SLAM系列——第十一讲 回环检测[2023.1]
SLAM系列——第十二讲 建图[2023.1]
SLAM系列——第十三讲 实践：设计SLAM系统[2023.1]
SLAM系列——第十四讲 SLAM：现在与未来[2023.1]
文章目录
- 系列文章目录
- 3.1 旋转矩阵
- 3.2 实践：Eigen
- 3.3 旋转向量和欧拉角
- 3.4 四元数
- 3.5 相似、仿射、射影变换
- 3.6 实践：Eigen几何模块
- 3.6.2 实际的坐标变换例子
- 3.7 可视化演示
- 3.7.1 显示运动轨迹
- 习题
3.1 旋转矩阵
详见。
3.2 实践：Eigen
详见。
#include <iostream>using namespace std;#include <ctime>
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>using namespace Eigen;#define MATRIX_SIZE 50/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/int main(int argc, char **argv) {// Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列// 声明一个2*3的float矩阵Matrix<float, 2, 3> matrix_23;// 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix// 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量Vector3d v_3d;// 这是一样的Matrix<float, 3, 1> vd_3d;// Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>Matrix3d matrix_33 = Matrix3d::Zero(); //初始化为零// 如果不确定矩阵大小，可以使用动态大小的矩阵Matrix<double, Dynamic, Dynamic> matrix_dynamic;// 更简单的MatrixXd matrix_x;// 这种类型还有很多，我们不一一列举// 下面是对Eigen阵的操作// 输入数据（初始化）matrix_23 << 1, 2, 3, 4, 5, 6;// 输出cout << "matrix 2x3 from 1 to 6: \n" << matrix_23 << endl;// 用()访问矩阵中的元素cout << "print matrix 2x3: " << 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里你不能混合两种不同类型的矩阵，像这样是错的// Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;// 应该显式转换Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;cout << "[1,2,3;4,5,6]*[3,2,1]=" << result.transpose() << endl;Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;cout << "[1,2,3;4,5,6]*[4,5,6]: " << result2.transpose() << endl;// 同样你不能搞错矩阵的维度// 试着取消下面的注释，看看Eigen会报什么错// Eigen::Matrix<double, 2, 3> result_wrong_dimension = matrix_23.cast<double>() * v_3d;// 一些矩阵运算// 四则运算就不演示了，直接用+-*/即可。matrix_33 = Matrix3d::Random(); // 随机数矩阵cout << "random matrix: \n" << matrix_33 << endl;cout << "transpose: \n" << matrix_33.transpose() << endl; // 转置cout << "sum: " << matrix_33.sum() << endl; // 各元素和cout << "trace: " << matrix_33.trace() << endl; // 迹cout << "times 10: \n" << 10 * matrix_33 << endl; // 数乘cout << "inverse: \n" << matrix_33.inverse() << endl; // 逆cout << "det: " << matrix_33.determinant() << endl; // 行列式// 特征值// 实对称矩阵可以保证对角化成功SelfAdjointEigenSolver<Matrix3d> eigen_solver(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 这个方程// N的大小在前边的宏里定义，它由随机数生成// 直接求逆自然是最直接的，但是求逆运算量大Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN= MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);matrix_NN = matrix_NN * matrix_NN.transpose(); // 保证半正定Matrix<double, MATRIX_SIZE, 1> v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);clock_t time_stt = clock(); // 计时// 直接求逆Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;cout << "time of normal inverse is "<< 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;cout << "x = " << x.transpose() << endl;// 通常用矩阵分解来求，例如QR分解，速度会快很多time_stt = clock();x = matrix_NN.colPivHouseholderQr().solve(v_Nd);cout << "time of Qr decomposition is "<< 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;cout << "x = " << x.transpose() << endl;// 对于正定矩阵，还可以用cholesky分解来解方程time_stt = clock();x = matrix_NN.ldlt().solve(v_Nd);cout << "time of ldlt decomposition is "<< 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;cout << "x = " << x.transpose() << endl;return 0;
}终端输出：
/slambook2/ch3/cmake-build-debug/useEigen/eigenMatrix
matrix 2x3 from 1 to 6:
1 2 3
4 5 6
print matrix 2x3:
1 2 3
4 5 6
[1,2,3;4,5,6]*[3,2,1]=10 28
[1,2,3;4,5,6]*[4,5,6]: 32 77
random matrix: 0.680375 0.59688 -0.329554
-0.211234 0.823295 0.5364590.566198 -0.604897 -0.444451
transpose: 0.680375 -0.211234 0.5661980.59688 0.823295 -0.604897
-0.329554 0.536459 -0.444451
sum: 1.61307
trace: 1.05922
times 10: 6.80375 5.9688 -3.29554
-2.11234 8.23295 5.364595.66198 -6.04897 -4.44451
inverse:
-0.198521 2.22739 2.83571.00605 -0.555135 -1.41603-1.62213 3.59308 3.28973
det: 0.208598
Eigen values =
0.02428990.9921541.80558
Eigen vectors =
-0.549013 -0.735943 0.3961980.253452 -0.598296 -0.760134
-0.796459 0.316906 -0.514998
time of normal inverse is 0.064ms
x = -55.7896 -298.793 130.113 -388.455 -159.312 160.654 -40.0416 -193.561 155.844 181.144 185.125 -62.7786 19.8333 -30.8772 -200.746 55.8385 -206.604 26.3559 -14.6789 122.719 -221.449 26.233 -318.95 -78.6931 50.1446 87.1986 -194.922 132.319 -171.78 -4.19736 11.876 -171.779 48.3047 84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237 28.9419 111.421 92.1237 -288.248 -23.3478 -275.22 -292.062 -92.698 5.96847 -93.6244 109.734
time of Qr decomposition is 0.04ms
x = -55.7896 -298.793 130.113 -388.455 -159.312 160.654 -40.0416 -193.561 155.844 181.144 185.125 -62.7786 19.8333 -30.8772 -200.746 55.8385 -206.604 26.3559 -14.6789 122.719 -221.449 26.233 -318.95 -78.6931 50.1446 87.1986 -194.922 132.319 -171.78 -4.19736 11.876 -171.779 48.3047 84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237 28.9419 111.421 92.1237 -288.248 -23.3478 -275.22 -292.062 -92.698 5.96847 -93.6244 109.734
time of ldlt decomposition is 0.017ms
x = -55.7896 -298.793 130.113 -388.455 -159.312 160.654 -40.0416 -193.561 155.844 181.144 185.125 -62.7786 19.8333 -30.8772 -200.746 55.8385 -206.604 26.3559 -14.6789 122.719 -221.449 26.233 -318.95 -78.6931 50.1446 87.1986 -194.922 132.319 -171.78 -4.19736 11.876 -171.779 48.3047 84.1812 -104.958 -47.2103 -57.4502 -48.9477 -19.4237 28.9419 111.421 92.1237 -288.248 -23.3478 -275.22 -292.062 -92.698 5.96847 -93.6244 109.734Process finished with exit code 03.3 旋转向量和欧拉角
详见。
3.4 四元数
详见。
3.5 相似、仿射、射影变换
详见。
3.6 实践：Eigen几何模块
详见。
#include <iostream>
#include <cmath>using namespace std;#include <Eigen/Core>
#include <Eigen/Geometry>using namespace Eigen;// 本程序演示了 Eigen 几何模块的使用方法int main(int argc, char **argv) {// Eigen/Geometry 模块提供了各种旋转和平移的表示// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3fMatrix3d rotation_matrix = Matrix3d::Identity();// 旋转向量使用 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 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顺序，即yaw-pitch-roll顺序cout << "yaw pitch roll = " << euler_angles.transpose() << endl;// 欧氏变换矩阵使用 Eigen::IsometryIsometry3d 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+tcout << "v tranformed = " << 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 be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;return 0;
}3.6.2 实际的坐标变换例子
#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Geometry>using namespace std;
using namespace Eigen;int main(int argc, char** argv) {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;
}3.7 可视化演示
详见。
3.7.1 显示运动轨迹
#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <unistd.h>// 本例演示了如何画出一个预先存储的轨迹using namespace std;
using namespace Eigen;// path to trajectory file
string trajectory_file = "./examples/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;// draw trajectory in pangolinDrawTrajectory(poses);return 0;
}/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {// create pangolin window and plot the trajectorypangolin::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); // sleep 5 ms}
}习题
详见。
SLAM系列——第三讲 三维空间刚体运动[2023.1]相关推荐
- 高博14讲--第三讲 三维空间刚体运动
高博14讲--第三讲 三维空间刚体运动 旋转矩阵 点和向量.坐标系 坐标系间的欧式变换 变换矩阵与齐次坐标 旋转向量和欧拉角 旋转向量 欧拉角 四元数 四元数的定义 四元数的运算 用四元数表示旋转 四 ...
- 【视觉SLAM十四讲】第三讲 三维空间刚体运动
点与坐标系 右手系, 内积(点乘) 外积(叉乘) |a||b|sin<a,b> a×b = -b×a 坐标系的变换 原点间的平移 三个轴的旋转 旋转矩阵 R为旋转矩阵 R是一个正交矩阵(R ...
- 三维空间刚体运动5：详解SLAM中显示机器人运动轨迹及相机位姿（原理流程）
三维空间刚体运动5:详解SLAM中显示机器人运动轨迹及相机位姿(原理流程) 一.显示运动轨迹原理讲解 二.前期准备 三.git管理子模块及克隆源代码 1.学习使用Git Submodule 2.克隆源 ...
- 高博SLAM十四讲书本程序学习——第3讲 三维空间刚体运动
小白高博SLAM十四讲书本程序学习_1 第3讲 三维空间刚体运动 在高博原始注释上,针对我自己不明白的部分,做额外注释 如果有错误的地方,请大家指点指点 博文目录 一.P.48 eigenMatrix ...
- SLAM概念三维空间刚体运动讲解
一.SLAM是什么?为什么要学习? 目前,GPS定位和标签定位应用比较广泛,但是这两者都有自己的缺点,GPS是一个涵盖很广的卫星信号,面对一些低信号地区,GPS会面对失灵的表现,比如室内.山区等: ...
- 视觉SLAM十四讲 第3讲 三维空间刚体运动（相关知识点汇总）
视觉SLAM十四讲 第3讲 三维空间刚体运动 1. 刚体 2. 欧氏空间(euclidean space) 2.1 欧氏距离: 2.2 欧氏变换: 3. 笛卡尔坐标系 4. 透视空间 5. 齐次坐标系 ...
- 视觉SLAM十四讲：第3讲 三维空间刚体运动
第3讲:三维空间刚体运动 三维空间中刚体运动的描述方式:旋转矩阵.变换矩阵.四元数和欧拉角 3.1 旋转矩阵 3.1.1 点和向量,坐标系 三维空间中,给定线性空间基(e1,e2,e3)(\mathb ...
- 三维空间刚体运动4-5：四元数多点离散数值解插值方法：Sping
三维空间刚体运动4-5:四元数多点离散数值解插值方法:Sping 1. 正切曲率κ(γ,t)\kappa(\gamma, t)κ(γ,t)在H1H_{1}H1上的离散数值解--Sping 1.1 离 ...
- 三维空间刚体运动4-4：四元数多点连续解析解插值方法：Spicv
三维空间刚体运动4-4:四元数多点连续解析解插值方法:Spicv 1. 总述:多点旋转插值的数学方法 2. 插值曲线及其连续性 2.1 插值曲线定义 2.2 插值曲线连续性的讨论 3. 最优插值曲线 ...
最新文章
- Scrapy框架-去重原理讲解、数据收集以及信号量知识
- 运维工作钱少、事多而且杂？年轻人，你这个思想很危险吶
- Hessian RPC示例和基于Http请求的Hessian序列化对象传输
- 34 多线程同步之Event
- C# 子窗口修改主窗口的控件
- grafana 画拓扑图 能不能_Grafana之ImageIt实现动态可感知网络拓扑(第十七篇)
- jquery ajax传值php,jquery ajax传值问题
- PHP调用新浪API 生成短链接
- linux下php可以实现哪些功能,基于Linux的远程管理系统的设计与实现(PHP)
- html 两个图片并排,HTML – 两个图像并排和响应
- mysql 配置文件my-default.cnf
- VS2015快捷键使用与常见问题
- 在C#中判断某个类是否实现了某个接口
- python批量读取图片并复制入word_提取word文档中的图片并使用Python进行批量格式转换,出,Word,里,利用,python...
- el-menu实现路由跳转及当前页的导航
- 《家财通》普及版序列号
- 计算机基础构建,构建高职《计算机网页设计》课程教学过程设计模式_计算机基础大一考试题...
- c++邻接表实现BFS算法遍历
- Springcloud微服务中多模块重复代码重构成公共模块的实现
- 网易云音乐移动端项目实战(分解下)