hdl_graph_slam （2）前端里程计 点云配准分析

hdl_graph_slam scan matching odometry 前端里程计

该节点将当前帧的激光点云（滤波后）与关键帧的激光点云进行匹配得到相对位姿，再将历史的相对位姿进行拼接融合得到全局的里程计信息发布出去。涉及到的关键点

1. 关键帧相关 （matching 函数）

   matching函数主要进行关键帧的判断，更新等。具体的配准过程在registration对象中实现。同时初始化中还对registration method进行了参数读取

   registration = select_registration_method(pnh);  //pnh:private nodehandle
   

   参考registration对应源码的私有句柄参数读取设置，可以在launch时赋值registration_method参数，默认为ndt_omp，另可选icp，gicp，ndt

   std::string registration_method = pnh.param<std::string>("registration_method", "NDT_OMP");
   

   如果是第一帧则进行记录保存，非第一帧则判断。

    if(!keyframe) {
         prev_trans.setIdentity();
         keyframe_pose.setIdentity();
         keyframe_stamp = stamp;
         keyframe = downsample(cloud);
         registration->setInputTarget(keyframe);
         return Eigen::Matrix4f::Identity();
       }
   

   接下来进行匹配，用registration中align方法进行点云配准

   pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
       registration->align(*aligned, prev_trans * msf_delta.matrix());
   

   更新关键帧

   默认关键帧的选取方法有以下三个参数可以设置：delta_trans>0.25, delta_angle>0.15, delta_time>1.0 。即判断当前的位移旋转和时间差，满足其一条件即可设置更新帧为关键帧。

   if(delta_trans > keyframe_delta_trans || delta_angle > keyframe_delta_angle || delta_time > keyframe_delta_time) {
         keyframe = filtered;
         registration->setInputTarget(keyframe);
   
         keyframe_pose = odom;
         keyframe_stamp = stamp;
         prev_trans.setIdentity();
       }
   

   较大转移的异常判断

   同时还有异常判断，两帧匹配点云的转移矩阵相差较大将抛弃改组匹配。

   if(transform_thresholding) {
         Eigen::Matrix4f delta = prev_trans.inverse() * trans;
         double dx = delta.block<3, 1>(0, 3).norm();
         double da = std::acos(Eigen::Quaternionf(delta.block<3, 3>(0, 0)).w());
   
         if(dx > max_acceptable_trans || da > max_acceptable_angle) {
           NODELET_INFO_STREAM("too large transform!!  " << dx << "[m] " << da << "[rad]");
           NODELET_INFO_STREAM("ignore this frame(" << stamp << ")");
           return keyframe_pose * prev_trans;
         }
       }
   

   思考
   匹配为两帧对比 没有滑动窗口 匹配准确度相比没有带滑动窗口的高
   初始化中有点云滤波相关的参数初始化，该部分在前一点云滤波节点中已经实现，这里再次实现一遍。

2. ndt匹配方法（icp, gicp,ndt,ndt_omp等)

• ndt
  • 协方差矩阵:两个随机变量联合分布现行相关程度的度量。对于多维随机变量 $X=[X_1,X_2,X_3,...,X_n{]}^T$,需要计算两两各自之间的协方差，如此对于多维变量的协方差矩阵为一个$n\times n$ 的矩阵。
    • 对称矩阵
    • 半正定
  • ndt 大致流程
    • 依据参考点云生成体素结构
    • 将参考点云中的点分配到生成的体素结构里
    • 计算每个体素内点的分布，均值和协方差
      • While 配准未收敛时
        • 遍历所有待配准点云
          • 查找位姿变换（初始位姿变换 不准确）前对应的体素栅格
          • 依据对应体素的概率分布计算匹配得分
          • 更新梯度向量
          • 更新海森矩阵
        • 求解位姿变换的增量，用于优化位姿变换
        • 更新位姿变换
      • end while

参考内容： hdl_graph_slam 源码解析（二）