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

2018-09-17 15:32:00 阅读：2363 来源： 互联网

标签：

前言

先说一下这个节点的流程，主要做了什么；然后结合代码一步一步分析。这个节点一共接收两个话题消息并发布六个话题消息。接收的两个话题分布是激光点云消息和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;
  }
}

未完待续、、、、、

标签：
来源： https://blog.csdn.net/liuyanpeng12333/article/details/82737181

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9

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

前言

main函数

imuHandler（）函数

未完待续、、、、、