主要内容
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),