LOAM SLAM代码解析：scanRegistration.cpp 点云及IMU数据处理节点

参考文章：LOAM SLAM代码解析之一：scanRegistration.cpp 点云及IMU数据处理节点：https://blog.csdn.net/liuyanpeng12333/article/details/82737181?utm_medium=distribute.pc_relevant.none-task-blog-OPENSEARCH-1.nonecase&depth_1-utm_source=distribute.pc_relevant.none-task-blog-OPENSEARCH-1.nonecase
前言
先说一下这个节点的流程，主要做了什么；然后结合代码一步一步分析。这个节点一共接收两个话题消息并发布六个话题消息。接收的两个话题分布是激光点云消息和IMU数据消息。建议读者先从imuHandler（）函数看起，因为laserCloudHandler（）函数会用到这个函数的结果。
main函数

这个main函数很简单，都是一些ros里面定义话题订阅器和发布器。这个节点一共订阅了两个话题的消息，发布六个话题的消息，没有什么难点，如果看不懂的读者需要首先复习一下ros的基本知识了。

int main(int argc, char** argv)
{
ros::init(argc, argv, “scanRegistration”);
ros::NodeHandle nh;

ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>
("/velodyne_points", 2, laserCloudHandler);

ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);

pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>
("/velodyne_cloud_2", 2);

pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_sharp", 2);

pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_less_sharp", 2);

pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_flat", 2);

pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_less_flat", 2);

pubImuTrans = nh.advertise<sensor_msgs::PointCloud2> ("/imu_trans", 5);

ros::spin();

return 0;
}

imuHandler（）函数
我们先总体说一下imuHandler（）函数做了些什么。首先为什么要用IMU？可能很多人认为IMU的作用类似于cartographer中提供位姿的先验信息。但LOAM中并没有使用IMU提供下一时刻位姿的先验信息，而是利用IMU去除激光传感器在运动过程中非匀速（加减速）部分造成的误差（运动畸变）。为什么这样做呢？因为LOAM是基于匀速运动的假设，但实际中激光传感器的运动肯定不是匀速的，因此使用IMU来去除非匀速运动部分造成的误差，以满足匀速运动的假设。
那么imuHandler（）函数具体做了些什么呢？我们知道IMU的数据可以提供给我们IMU坐标系三个轴相对于世界坐标系的欧拉角和三个轴上的加速度。但由于加速度 受到重力的影响所以首先得去除重力影响。在去除重力影响后我们想要获得IMU在世界坐标系下的运动，因此根据欧拉角就可以将IMU三轴上的加速度转换到世界坐标系下的加速度。 然后根据加速度利用 公式s1 = s2+ vt + 1/2at*t来计算位移。因此我们可以求出每一帧IMU数据在世界坐标系下对应的位移和速度。 至此imuHandler（）函数就完成了它的使命。

//接收imu消息，imu坐标系为x轴向前，y轴向右，z轴向上的右手坐标系
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
double roll, pitch, yaw;
//定义四元数
tf::Quaternion orientation;
//将IMU数据中的四元数转换到定义的四元数中
tf::quaternionMsgToTF(imuIn->orientation, orientation);
//由四元数获得欧拉角
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

//减去重力的影响,求出xyz方向的加速度实际值，并进行坐标轴交换，统一到z轴向前,
//x轴向左的右手坐标系, 交换过后RPY对应fixed axes ZXY(RPY—ZXY)。
//Now R = Ry(yaw)*Rx(pitch)*Rz(roll).
float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;

//循环移位效果，形成环形数组
imuPointerLast = (imuPointerLast + 1) % imuQueLength;

imuTime[imuPointerLast] = imuIn->header.stamp.toSec();
imuRoll[imuPointerLast] = roll;
imuPitch[imuPointerLast] = pitch;
imuYaw[imuPointerLast] = yaw;
imuAccX[imuPointerLast] = accX;
imuAccY[imuPointerLast] = accY;
imuAccZ[imuPointerLast] = accZ;

AccumulateIMUShift();
}

AccumulateIMUShift()函数主要用于获取每一帧IMU数据对应IMU在全局坐标系下的位移和速度。

void AccumulateIMUShift()
{
//获得由IMUHandler函数得到该帧IMU数据的欧拉角和三轴角加速度
float roll = imuRoll[imuPointerLast];
float pitch = imuPitch[imuPointerLast];
float yaw = imuYaw[imuPointerLast];
float accX = imuAccX[imuPointerLast];
float accY = imuAccY[imuPointerLast];
float accZ = imuAccZ[imuPointerLast];

//将当前时刻的加速度值绕交换过的ZXY固定轴（原XYZ）分别旋转(roll, pitch, yaw)角，转换得到世界
//坐标系下的加速度值(右手法则)
//绕z轴旋转(roll)
float x1 = cos(roll) * accX - sin(roll) * accY;
float y1 = sin(roll) * accX + cos(roll) * accY;
float z1 = accZ;
//绕x轴旋转(pitch)
float x2 = x1;
float y2 = cos(pitch) * y1 - sin(pitch) * z1;
float z2 = sin(pitch) * y1 + cos(pitch) * z1;
//绕y轴旋转(yaw)
accX = cos(yaw) * x2 + sin(yaw) * z2;
accY = y2;
accZ = -sin(yaw) * x2 + cos(yaw) * z2;

//上一个imu点
int imuPointerBack = (imuPointerLast + imuQueLength - 1) % imuQueLength;
//上一个点到当前点所经历的时间，即计算imu测量周期
double timeDiff = imuTime[imuPointerLast] - imuTime[imuPointerBack];
//要求imu的频率至少比lidar高，这样的imu信息才使用，后面校正也才有意义
if (timeDiff < scanPeriod) {//（隐含从静止开始运动）
//求每个imu时间点的位移与速度,两点之间视为匀加速直线运动
imuShiftX[imuPointerLast] = imuShiftX[imuPointerBack] + imuVeloX[imuPointerBack] * timeDiff
+ accX * timeDiff * timeDiff / 2;
imuShiftY[imuPointerLast] = imuShiftY[imuPointerBack] + imuVeloY[imuPointerBack] * timeDiff
+ accY * timeDiff * timeDiff / 2;
imuShiftZ[imuPointerLast] = imuShiftZ[imuPointerBack] + imuVeloZ[imuPointerBack] * timeDiff
+ accZ * timeDiff * timeDiff / 2;
imuVeloX[imuPointerLast] = imuVeloX[imuPointerBack] + accX * timeDiff;
imuVeloY[imuPointerLast] = imuVeloY[imuPointerBack] + accY * timeDiff;
imuVeloZ[imuPointerLast] = imuVeloZ[imuPointerBack] + accZ * timeDiff;
}
}