SLAM 基础学习记录（3） 关于SLAM 基础学习记录1和2的改进与部分遗留问题解决

主要内容

1. 解决之前点云转换后得到二维平面点云的问题；

2. 对点云每帧的每个点进行旋转；

3. 解决之前的点云保存问题。

具体内容

1. 解决之前点云转换后得到二维平面点云的问题

因为之前是用的EIGEN库自带的转换矩阵函数，所以点云转换后的结果一直都没有问题。

将自己写的转换矩阵依次与EIGEN库自带转换函数进行对比，找到出错原因，绕Y轴的旋转矩阵书写错误，修改后的转换矩阵代码如下。

      MatrixXf Z_AxisRotMat(4,4);
      MatrixXf Y_AxisRotMat(4,4);
      MatrixXf X_AxisRotMat(4,4);
      MatrixXf TransformLIDARandIMU(4,4);
      MatrixXf TranslationMat(4,4);
      MatrixXf TransformMat(4,4);

      Z_AxisRotMat<< cos(_imuCur.yaw), -sin(_imuCur.yaw),  0, 0,
          sin(_imuCur.yaw), cos(_imuCur.yaw ),  0, 0,
          0,                0,  1, 0,
          0,                0,  0, 1;

      Y_AxisRotMat<< cos(_imuCur.pitch),               0, sin(_imuCur.pitch), 0,
          0,                1,                  0, 0,
          -sin(_imuCur.pitch),