高博SLAM十四讲书本程序学习——第3讲三维空间刚体运动

小白高博SLAM十四讲书本程序学习_1
第3讲三维空间刚体运动

在高博原始注释上，针对我自己不明白的部分，做额外注释
如果有错误的地方，请大家指点指点

博文目录

一、P.48 eigenMatrix.cpp
二、P.62 useGeometry.cpp
三、P.65 coordinateTransform.cpp
四、P.67 plotTrajectory.cpp

一、P.48 eigenMatrix.cpp

#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;  //3d d--double  3f f--float// 这两者生成结果是一样的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 自伴随特征求解器 Adjoint 伴随矩阵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();// colPivHouseholderQr().solve()函数，求解的就是Ax=b中的x// x = A.colPivHouseholderQr().solve(b);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();// ldlt().solve()函数，求解的就是Ax=b中的x// x = A.ldlt().solve(b);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;
}

二、P.62 useGeometry.cpp

旋转矩阵(3x3):Eigen::Matrix3d
旋转向量(3x1):Eigen::AngleAxisd
欧拉角(3x1):Eigen::Vector3d
四元数(4x1):Eigen::Quaterniond
欧式变换矩阵:Eigen::Isomotery3d

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>using namespace std;
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 度  PI  pi/2 = 90°  pi/4 = 45°  // cout.precision()其实是输出流cout的一个格式控制函数，也就是在iostream中的一个成员函数。// precision()返回当前的浮点数的精度值，而cout.precision(val)其实就是在输出的时候设定输出值以新的浮点数精度值显示，即小数点后保留val位。cout.precision(3);// 旋转向量->矩阵形式 // .matrix()cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用matrix()转换成矩阵// 也可以直接赋值// 旋转向量->旋转矩阵// .toRotationMatrix()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;// 欧拉角: 可以将旋转矩阵直接转换成欧拉角// 旋转矩阵->欧拉角 // .eulerAngles(2, 1, 0); // ZYX顺序，即roll pitch yaw顺序  eulerAngles(0, 1, 2) //XYZ顺序Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); 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}  q 乘 v 再乘以 q的逆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;
}

三、P.65 coordinateTransform.cpp

#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);// normalize() 归一化函数// 四元数使用前如果没有进行归一化的话，生成的旋转矩阵无法满足正交性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);// 以下为初始化（赋值）变换矩阵T 的一种方法，其他见参考文献[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;
}

四、P.67 plotTrajectory.cpp

#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";// 这是因为在C++11标准中，aligned_allocator管理C++中的各种数据类型的内存方法是一样的，可以不需要着重写出来。
// 但是在Eigen管理内存和C++11中的方法是不一样的，所以需要单独强调元素的内存分配和管理。
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;}// .eof 相当于 end
// eof()函数可以帮助我们用来判断文件是否为空，抑或是判断其是否读到文件结尾while (!fin.eof()) {// time 为时间，tx,ty,tz 为平移部分，qx,qy,qz,qw 是四元数表示的旋转部分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)); // 注意归一化问题 这里可能是已经自动normalize()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;
}/*******************************************************************************************/
// 这是因为在C++11标准中，aligned_allocator管理C++中的各种数据类型的内存方法是一样的，可以不需要着重写出来。
// 但是在Eigen管理内存和C++11中的方法是不一样的，所以需要单独强调元素的内存分配和管理。
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {// create pangolin window and plot the trajectory// 新建一个窗口 // 创建名称为“Trajectory Viewer”的GUI窗口，尺寸为1024×768pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);glEnable(GL_DEPTH_TEST); // 启动深度测试，根据坐标的远近自动隐藏被遮住的图形（材料）。值为2929/*glEnable( GL_BLEND );   // 启用混合glDisable( GL_BLEND );  // 禁用关闭混合*/glEnable(GL_BLEND); // 启用颜色混合。例如实现半透明效果。值为3042glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); // glBlendFunc 混合函数 混合因子见为知笔记// 定义投影和初始模型视图矩阵 pangolin::OpenGlRenderState s_cam(// 创建一个观察相机视图。// ProjectMatrix(int h, int w, int fu, int fv, int cu, int cv, int znear, int zfar)// 参数依次为观察相机的图像高度、宽度、4个内参(fx, fy, cx, cy)以及最近和最远视距pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),// ModelViewLookAt(double ex, double ey, double ez,double lx, double ly, double lz, double ux, double uy, double uz)// 参数依次为相机在世界坐标的位置（相机位置），以及相机镜头对准的物体在世界坐标的位置(一般会设置在原点)（参考点位置）。// 假设人站在eye处，眼睛看向( lx, ly, lz ) 这个点，u为相机向上的方向在世界坐标中的方向// up vector(上向量)pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0));/*创建一个窗口，也就是打开相机后相机有一个成像平面，即视口viewport.SetBounds（pangolin::Attach bottom, pangolin::Attach top, pangolin::Attach left, pangolin::Attach right ）定义面板的位置，其中的参数可以设定成如上0.0，1.0 等（0~1）的相对位置数值，也可以设定成绝对位置数值例如：SetBounds（0，1，0，pangolin::Attach::Pix(100)）前两个参数（0.0, 1.0）表明面板纵向宽度和窗口大小相同 后两个参数（0 ，pangolin::Attach::Pix(100)）表明右边横向100个像素所有部分用于显示按钮面板.SetBound最后一个参数（-1024.0f/768.0f）为显示长宽比*/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));/*形状可以设为：  GL_POINTS：点            GL_LINES：线          GL_LINE_STRIP：折线GL_LINE_LOOP：封闭折线    GL_TRIANGLES：三角形   GL_POLYGON：多边形glBegin()和glEnd()之间可以调用的函数有： glVertex()设置顶点坐标     glColor()设置当前颜色       glIndex()设置当前颜色表    glNormal()设置法向坐标     glEvalCoord()产生坐标      glCallList(),glCallLists()执行显示列表    glTexCoord()设置纹理坐标   glEdgeFlag()控制边界绘制    glMaterial()设置材质*/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]: 高翔，张涛. 视觉SLAM十四讲（第二版） [M]. 电子工业出版社, 2019.
[2]: Eigen库要点
还查阅了其他许多博客主的文章，在此致谢。本文仅供学习

高博SLAM十四讲书本程序学习——第3讲三维空间刚体运动相关推荐

视觉SLAM14讲笔记分享——第三章【三维空间刚体运动】
这一章开始有一些数学公式上的推导和定义的理解,但是还不算太难
高博视觉SLAM十四讲思维导图分享
最近学习高博士的视觉slam十四讲,学到后面忘了前面(害~学渣一枚罢了),并且到后期所有的知识都需要交叉使用.融会贯通,所有自己根据高博的课程做了个思维导图,分享链接就放在这里啦!希望可以帮助到大家. ...
高博-《视觉SLAM十四讲》
0 讲座 (1)SLAM定义对比雷达传感器和视觉传感器的优缺点(主要介绍视觉SLAM) 单目:不知道尺度信息双目:知道尺度信息,但测量范围根据预定的基线相关 RGBD:知道深度信息,但是深度信息对 ...
视觉SLAM十四讲学习笔记-第三讲-相似、仿射、射影变换和eigen程序、可视化演示
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
《视觉SLAM十四讲》（第二版）程序
目录 clone github下代码 CH2 IDE的使用 CMake CH3 Eigen CH4 Sophus CH5 OpenCV clone github下代码由于网络等问题,会导致git c ...
视觉SLAM十四讲slambook2/ch3/examples/plotTrajectory.cpp程序报错解决
视觉SLAM十四讲slambook2/ch3/examples/plotTrajectory.cpp程序报错解决原程序编译后出现如下报错: /usr/local/include/pangolin/v ...
视觉SLAM十四讲报错ch13 问题汇总 /usr/bin/ld: /usr/local/lib/libfmt.a(format.cc.o)
视觉SLAM十四讲报错ch13 /usr/bin/ld: /usr/local/lib/libfmt.a 1. 问题1:未链接fmt库 1.1 解决方案 2. 问题2:-fPIC 2.1 解决方案 ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-特征点法和特征提取和匹配实践
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...

高博SLAM十四讲书本程序学习——第3讲三维空间刚体运动

博文目录

一、P.48 eigenMatrix.cpp

二、P.62 useGeometry.cpp

三、P.65 coordinateTransform.cpp

四、P.67 plotTrajectory.cpp

高博SLAM十四讲书本程序学习——第3讲三维空间刚体运动相关推荐

最新文章

热门文章

高博SLAM十四讲书本程序学习——第3讲 三维空间刚体运动

博文目录

一、P.48 eigenMatrix.cpp

二、P.62 useGeometry.cpp

三、P.65 coordinateTransform.cpp

四、P.67 plotTrajectory.cpp

高博SLAM十四讲书本程序学习——第3讲 三维空间刚体运动相关推荐

最新文章

热门文章

高博SLAM十四讲书本程序学习——第3讲三维空间刚体运动

高博SLAM十四讲书本程序学习——第3讲三维空间刚体运动相关推荐