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

小白高博SLAM十四讲书本程序学习_1

第3讲 三维空间刚体运动

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

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 或 Matrix3f
  Matrix3d 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::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 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 pangolin
  DrawTrajectory(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×768
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST); // 启动深度测试，根据坐标的远近自动隐藏被遮住的图形（材料）。值为2929
  
  /*
  glEnable( GL_BLEND );   // 启用混合
  glDisable( GL_BLEND );  // 禁用关闭混合
 */
  glEnable(GL_BLEND); // 启用颜色混合。例如实现半透明效果。值为3042
  glBlendFunc(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库要点
还查阅了其他许多博客主的文章，在此致谢。本文仅供学习