LOAM-Noted源码分析

LOAM的源码主要分为预处理（scanRegistration提取特征点）、laserOdometry10HZ的激光里程计用于位姿估计、1HZ的laserMapping用于地图构建、以及transformMaintenance的位姿优化四个模块组成。

本部分的源码来自于loam noted 这里的版本是loam的velodyne三维激光版本，主要考虑到KITTI上有Velodyne16线的数据，可以用来测试。

下面线看一下点云匹配部分的代码，scanRegistration.cpp；其工作如下：对点云和IMU数据预处理、对接收的点云数据划分到不同线中存储、特征点（边缘点+平面点）提取等

scanRegistration.cpp

/******************************读前须知*****************************************/
/*imu为x轴向前,y轴向左,z轴向上的右手坐标系，
  velodyne lidar被安装为x轴向前,y轴向左,z轴向上的右手坐标系，
  scanRegistration会把两者通过交换坐标轴，都统一到z轴向前,x轴向左,y轴向上的右手坐标系
  ，这是J. Zhang的论文里面使用的坐标系
  交换后：R = Ry(yaw)*Rx(pitch)*Rz(roll)
*******************************************************************************/

main函数：

int main(int argc, char** argv)
{
  ros::init(argc, argv, "scanRegistration");
  ros::NodeHandle nh;
　//两个订阅话题：点云、IMU
 //两个主要的回调函数：laserCloudHandler、imuHandler
  ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2> 
                                  ("/velodyne_points", 2, laserCloudHandler);
  ros::Subscriber subImu = nh.subscribe<sensor_msgs::Imu> ("/imu/data", 50, imuHandler);
  // 6个发布话题
  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()进入循环，一直调用回调函数，用户输入Ctrl+C退出
  ros::spin();

  return 0;
}

先看一下IMU的回调函数，首先为什么要用IMU?在cartographer中imu用于提供位姿的先验信息。但LOAM中并没有使用IMU提供下一时刻位姿的先验信息，而是利用IMU去除激光传感器在运动过程中非匀速（加减速）部分造成的误差（运动畸变）。为什么这样做呢？因为LOAM是基于匀速运动的假设，但实际激光雷达的运动不是匀速的，因此使用IMU来去除非匀速运动部分造成的误差，以满足匀速运动的假设。

那么IMU的回调函数具体做了什么呢？我们知道IMU的数据可以提供给我们IMU坐标系三个轴相对于世界坐标系的欧拉角和三个轴上的加速度。但是由于加速度受到重力的影响，所以需要去除重力的影响。在去除重力影响后，我们想要获得IMU在世界坐标系下的运动，因此根据欧拉角就可以将IMU三轴上的加速度转换到世界坐标系下的加速度。然后根据加速度利用公式s1 = s2+ vt + 1/2at*t来计算位移。因此我们可以求出每一帧IMU数据在世界坐标系下对应的位移和速度；至此IMU就完成了它的任务了。

//接收imu消息，imu坐标系为x轴向前，y轴向右，z轴向上的右手坐标系
//减去重力对imu的影响，求出xyz轴的加速度实际值，将加速度转换到世界坐标系下
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuIn)
{
  double roll, pitch, yaw;
  tf::Quaternion orientation;
  //convert Quaternion msg to Quaternion，将imu中的四元素转换到定义的四元素中
  tf::quaternionMsgToTF(imuIn->orientation, orientation);
  //This will get the roll pitch and yaw from the matrix about fixed axes X, Y, Z respectively. That's R = Rz(yaw)*Ry(pitch)*Rx(roll).
  //Here roll pitch yaw is in the global frame
 //为世界坐标系下的角度信息
  tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

  //减去重力的影响,求出xyz方向在IMU坐标系下的加速度实际值，并进行坐标轴交换，统一到z轴向前,x轴向左的右手坐标系, 交换过后RPY对应fixed axes ZXY(RPY---ZXY)。Now R = Ry(yaw)*Rx(pitch)*Rz(roll).
  //先将重力加速度的坐标系从世界坐标系转换到IMU坐标系，然后用imu测量值减去重力的影响，最后得到真实的加速度值
  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();
}

激光点云的回调函数laserCloudHandler

主要包括以下模块：

1、点云预处理：过滤无效点、计算起始和终止点的方位角。

2、根据每个点的仰角将点划入不同线中，共16线，记录线束号和获取的相对时间（也就是代码中的intensity，这个不是强度的意思）

3、使用imu数据插值计算点云中点的位置，消除由于非匀速运动造成的运动畸变。

4、计算所有点的曲率，剔除两类点

5、提取边缘点和平面点

6、最后使用ROS发布消息。

先看1）点云预处理：首先过滤无效点、计算起始和终止点的方位角。计算起始、终止点的角度是为了计算点云数据中每个点在本次扫描点云数据中的相对位置，从而得到每个点获取的时间relTime，为后面IMU插值做准备。

后面会用上的变量为：

1、每次扫描点开始和结束的索引：scanStartInd、scanEndInd；

2、pcl点云数据：laserCloudIn；

3、起始和终止点的方位角：startOri、endOri；

LOAM-Noted源码分析

推荐阅读