LOAM源代码分析附公式推导之LaserOdometry

更新日志

同步更新地址知乎：LOAM SLAM原理之源代码分析附公式推导
有细节性出入！！主要是细节部分的解疑，问题讨论部分，建议前往知乎阅读，此博客只是原始版本（还不是因为大家讨论得太少了，修改都在知乎上进行了 ） 。

• 2020-12-07
  整理完成LaserOdometry节点内容
  最近有点忙，因此这个时间跨度有点大，两个月了，我自己都没有想到！害，很多公式自己有重新推导了一次，希望能够帮助大家！
• 2020-10-11
  整理完成MultiScanRegistration节点内容，地址请转到LOAM源代码分析附公式推导之MultiScanRegistration
• 2020-10-09
  第一次编辑，初版

6. 之LaserOdometry

6.1 参数配置

默认设置如下

• 每次激光扫描周期 scanPeriod = 0.1s
• 输入与输出激光帧的比值 ioRatio = 2
• 最大迭代次数 maxIterations = 25

explicit LaserOdometry(float scanPeriod = 0.1, uint16_t ioRatio = 2, size_t maxIterations = 25);

还有其他可配置参数及默认值

• 迭代优化时平移量阈值 deltaTAbort = 0.1
• 迭代优化时旋转量阈值 deltaRAbort = 0.1

6.2 话题订阅

发布的话题如下，平面点、边缘点、全部点云以及里程计信息

    // advertise laser odometry topics
    _pubLaserCloudCornerLast = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);
    _pubLaserCloudSurfLast = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
    _pubLaserCloudFullRes = node.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 2);
    _pubLaserOdometry = node.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 5);

对于发布的里程计信息/laser_odom_to_init，坐标系定义如下

  LaserOdometry::LaserOdometry(float scanPeriod, uint16_t ioRatio, size_t maxIterations):
    BasicLaserOdometry(scanPeriod, maxIterations),
    _ioRatio(ioRatio)
  {
   
    // initialize odometry and odometry tf messages
    _laserOdometryMsg.header.frame_id = "/camera_init";
    _laserOdometryMsg.child_frame_id  = "/laser_odom";

    _laserOdometryTrans.frame_id_       = "/camera_init";
    _laserOdometryTrans.child_frame_id_ = "/laser_odom";
  }

订阅的话题，主要是MultiScanRegistration中发布的数据。

    // subscribe to scan registration topics
    _subCornerPointsSharp = node.subscribe<sensor_msgs::PointCloud2>
      ("/laser_cloud_sharp", 2, &LaserOdometry::laserCloudSharpHandler, this);

    _subCornerPointsLessSharp = node.subscribe<sensor_msgs::PointCloud2>
      ("/laser_cloud_less_sharp", 2, &LaserOdometry::laserCloudLessSharpHandler, this);

    _subSurfPointsFlat = node.subscribe<sensor_msgs::PointCloud2>
      ("/laser_cloud_flat", 2, &LaserOdometry::laserCloudFlatHandler, this);

    _subSurfPointsLessFlat = node.subscribe<sensor_msgs::PointCloud2>
      ("/laser_cloud_less_flat", 2, &LaserOdometry::laserCloudLessFlatHandler, this);

    _subLaserCloudFullRes = node.subscribe<sensor_msgs::PointCloud2>
      ("/velodyne_cloud_2", 2, &LaserOdometry::laserCloudFullResHandler, this);

    _subImuTrans = node.subscribe<sensor_msgs::PointCloud2>
      ("/imu_trans", 5, &LaserOdometry::imuTransHandler, this);

6.3 IMU数据处理

首先是IMU数据的回调函数，转换数据格式，使用了pcl::PointXYZ，与前面一致。

  void LaserOdometry::imuTransHandler(const sensor_msgs::PointCloud2ConstPtr& imuTransMsg)
  {
   
    _timeImuTrans = imuTransMsg->header.stamp;

    pcl::PointCloud<pcl::PointXYZ> imuTrans;
    pcl::fromROSMsg(*imuTransMsg, imuTrans);
    updateIMU(imuTrans);
    _newImuTrans = true;
  }

进一步更新IMU数据信息，参考之前的分析5.4.4 打包IMU数据，有

获取点云中第一个点时对应的激光姿态角_imuStart，获取点云中最后一个点时对应的激光姿态角_imuCur
获取点云中最后一个点时激光相对于获取开始点时的激光的 非匀速运动 位置偏移 开始点对应的IMU局部坐标系imuShiftFromStart
获取点云中最后一个点时激光相对于获取开始点时的激光的 非匀速运动 速度偏移 开始点对应的IMU局部坐标系imuVelocityFromStart

void BasicLaserOdometry::updateIMU(pcl::PointCloud<pcl::PointXYZ> const& imuTrans)
{
   
   assert(4 == imuTrans.size());
   _imuPitchStart = imuTrans.points[0].x;
   _imuYawStart = imuTrans.points[0].y;
   _imuRollStart = imuTrans.points[0].z;

   _imuPitchEnd = imuTrans.points[1].x;
   _imuYawEnd = imuTrans.points[1].y;
   _imuRollEnd = imuTrans.points[1].z;

   _imuShiftFromStart = imuTrans.points[2];
   _imuVeloFromStart = imuTrans.points[3];
}

这样就OK了，接下来就是考虑如何使用它了，在6.4 点云数据处理中再详细解释吧。

6.4 点云数据处理

6.4.1 点云数据

对于不同话题下的点云数据处理大同小异，这里只简单说一个就行

• 1.获取点云数据的时间_timeSurfPointsFlat
• 2.转换点云格式fromROSMsg
• 3.去除无效点removeNaNFromPointCloud
• 4.设置标志位_newSurfPointsFlat

  void LaserOdometry::laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr& surfPointsFlatMsg)
  {
   
    _timeSurfPointsFlat = surfPointsFlatMsg->header.stamp;

    surfPointsFlat()->clear();
    pcl::fromROSMsg(*surfPointsFlatMsg, *surfPointsFlat());
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*surfPointsFlat(), *surfPointsFlat(), indices);
    _newSurfPointsFlat = true;
  }

得到的点云数据分别为

    pcl::PointCloud<pcl::PointXYZI>::Ptr _cornerPointsSharp;      ///< sharp corner points cloud
    pcl::PointCloud<pcl::PointXYZI>::Ptr _cornerPointsLessSharp;  ///< less sharp corner points cloud
    pcl::PointCloud<pcl::PointXYZI>::Ptr _surfPointsFlat;         ///< flat surface points cloud
    pcl::PointCloud<pcl::PointXYZI>::Ptr _surfPointsLessFlat;     ///< less flat surface points cloud
    pcl::PointCloud<pcl::PointXYZI>::Ptr _laserCloud;             ///< full resolution cloud

每次对点云数据处理时需要保证同时接受到最新的数据

  bool LaserOdometry::hasNewData()
  {
   
    return _newCornerPointsSharp && _newCornerPointsLessSharp && _newSurfPointsFlat &&
      _newSurfPointsLessFlat && _newLaserCloudFullRes && _newImuTrans &&
      fabs((_timeCornerPointsSharp - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
      fabs((_timeCornerPointsLessSharp - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
      fabs((_timeSurfPointsFlat - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
      fabs((_timeLaserCloudFullRes - _timeSurfPointsLessFlat).toSec()) < 0.005 &&
      fabs((_timeImuTrans - _timeSurfPointsLessFlat).toSec()) < 0.005;
  }

对最新的点云数据进行处理BasicLaserOdometry::process()，如何处理的呢？我们下一个小节展开，这也是激光里程计最核心的部分。

  void LaserOdometry::process()
  {
   
    if (!hasNewData())
      return;// waiting for new data to arrive...

    reset();// reset flags, etc.
    BasicLaserOdometry::process();
    publishResult();
  }

6.4.2 公式推导

6.4.2.1 帧间运动

使用3.3 坐标变换的定义，继续推导。
对于一帧点云数据，有第一个点$p_s$和最后一个点$p_e$，以及任意时刻的点$p_t$，
首先将任意时刻$t$的点重新投影到统一时刻$t_s$得到对应的点${\tilde{p}}_{st}$，因此有关系

$$p_t=R_{s:t}{\tilde{p}}_{st}+T_{s:t}$$

进一步得到

$${\tilde{p}}_{st}=R_{s:t}^{-1}(p_t-T_{s:t})$$

可以看到，我们需要求解的就是每个时刻$t$对应的变换关系$R_{s:t}$和$T_{s:t}$，根据匀速运动假设，实际只需要求解第一个点和最后一个点的变换关系$R_{s:e}$和$T_{s:e}$，并且在迭代优化中使用的是矩阵$R_{s:t}^{-1}$，考虑将该矩阵元素使用欧拉角表示。

这里考虑清楚为什么使用$R_{s:t}^{-1}$矩阵，而不是$R_{s:t}$矩阵。

那么，根据坐标系定义，使用左乘、旋转坐标轴、先Z再X后Y得到旋转矩阵$R_{ZXY}$，有
$$R_{s:t}^{-1}=R_{ZXY}=R_{ry}{R}_{rx}{R}_{rz}=\left[\begin{array}{ccc}cycz-sxsysz& cysz+sxsycz& -sycx\\ -cxsz& cxcz& sx\\ sycz+sxcysz& sysz-sxcycz& cycx\end{array}\right]$$
那么，对于矩阵$R_{s:t}$，有

$$R_{s:t}=(R_{ZXY}{)}^{-1}=R_{-rz}{R}_{-rx}{R}_{-ry}$$

那么对于优化的目标可以简述为

寻找最优的旋转矩阵$R_{s:t}^{-1}$和平移量$T_{s:t}$，使得所有点云特征点到特征区域的距离最小。

那么，对于边缘点、平面点均有下面等式

$$f({\tilde{p}}_{st})=f(R_{s:t}^{-1}(p_t-T_{s:t}))=f(R_{s:t}^{-1}，p_t，T_{s:t})=f(R_{s:t}^{-1}，T_{s:t})=d$$

定义$R_{s:t}^{-1}$对应的欧拉角分别为$rx,ry,rz$即是

$$R_{s:t}^{-1}=R_{ZXY}=R_{ry}{R}_{rx}{R}_{rz}$$

$T_{s:t}$分量分别为$t_x,t_y,t_z$，即是

$$T_{s:t}=[t_x;t_y;t_z]$$

进一步有
$$\frac{\partial f}{\partial (rx,ry,rz,t_x,t_y,t_z)}=[\frac{\partial f}{\partial rx},\frac{\partial f}{\partial ry},\frac{\partial f}{\partial rz},\frac{\partial f}{\partial t_x},\frac{\partial f}{\partial t_y},\frac{\partial f}{\partial t_z}]$$
现在以$rx$,$t_x$举例说明࿰