前端里程计的主要代码在apps/scan_matching_odometry_nodelet.cpp中，它的主要作用是订阅点云、匹配得到位姿，并以odom形式发布位姿。

对于订阅发布数据之类的，属于ros的基础内容，我们不在这里介绍了，我们介绍核心内容，即点云配准这部分。

点云匹配包括两方面内容，一是根据配置文件选择点云匹配方法，二是根据订阅得到的点云去匹配得到位姿。

我们把这两方面内容分别介绍。

1. 选择匹配方法

这个函数在文件src/hdl_graph_slam/registrations.cpp中，我们看下代码

boost::shared_ptr<pcl::Registration<pcl::PointXYZI, pcl::PointXYZI>> select_registration_method(ros::NodeHandle& pnh) {
  using PointT = pcl::PointXYZI;

  // select a registration method (ICP, GICP, NDT)
  std::string registration_method = pnh.param<std::string>("registration_method", "NDT_OMP");
  if(registration_method == "ICP") {
    std::cout << "registration: ICP" << std::endl;
    boost::shared_ptr<pcl::IterativeClosestPoint<PointT, PointT>> icp(new pcl::IterativeClosestPoint<PointT, PointT>());
    icp->setTransformationEpsilon(pnh.param<double>("transformation_epsilon", 0.01));
    icp->setMaximumIterations(pnh.param<int>("maximum_iterations", 64));
    icp->setUseReciprocalCorrespondences(pnh.param<bool>("use_reciprocal_correspondences", false));
    return icp;
  } else if(registration_method.find("GICP") != std::string::npos) {
    if(registration_method.find("OMP") == std::string::npos) {
      std::cout << "registration: GICP" << std::endl;
      boost::shared_ptr<pcl::GeneralizedIterativeClosestPoint<PointT, PointT>> gicp(new pcl::GeneralizedIterativeClosestPoint<PointT, PointT>());
      gicp->setTransformationEpsilon(pnh.param<double>("transformation_epsilon", 0.01));
      gicp->setMaximumIterations(pnh.param<int>("maximum_iterations", 64));
      gicp->setUseReciprocalCorrespondences(pnh.param<bool>("use_reciprocal_correspondences", false));
      gicp->setCorrespondenceRandomness(pnh.param<int>("gicp_correspondence_randomness", 20));
      gicp->setMaximumOptimizerIterations(pnh.param<int>("gicp_max_optimizer_iterations", 20));
      return gicp;
    } else {
      std::cout << "registration: GICP_OMP" << std::endl;
      boost::shared_ptr<pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>> gicp(new pclomp::GeneralizedIterativeClosestPoint<PointT, PointT>());
      gicp->setTransformationEpsilon(pnh.param<double>("transformation_epsilon", 0.01));
      gicp->setMaximumIterations(pnh.param<int>("maximum_iterations", 64));
      gicp->setUseReciprocalCorrespondences(pnh.param<bool>("use_reciprocal_correspondences", false));
      gicp->setCorrespondenceRandomness(pnh.param<int>("gicp_correspondence_randomness", 20));
      gicp->setMaximumOptimizerIterations(pnh.param<int>("gicp_max_optimizer_iterations", 20));
      return gicp;
    }
  } else {
    if(registration_method.find("NDT") == std::string::npos ) {
      std::cerr << "warning: unknown registration type(" << registration_method << ")" << std::endl;
      std::cerr << "       : use NDT" << std::endl;
    }

    double ndt_resolution = pnh.param<double>("ndt_resolution", 0.5);
    if(registration_method.find("OMP") == std::string::npos) {
      std::cout << "registration: NDT " << ndt_resolution << std::endl;
      boost::shared_ptr<pcl::NormalDistributionsTransform<PointT, PointT>> ndt(new pcl::NormalDistributionsTransform<PointT, PointT>());
      ndt->setTransformationEpsilon(pnh.param<double>("transformation_epsilon", 0.01));
      ndt->setMaximumIterations(pnh.param<int>("maximum_iterations", 64));
      ndt->setResolution(ndt_resolution);
      return ndt;
    } else {
      int num_threads = pnh.param<int>("ndt_num_threads", 0);
      std::string nn_search_method = pnh.param<std::string>("ndt_nn_search_method", "DIRECT7");
      std::cout << "registration: NDT_OMP " << nn_search_method << " " << ndt_resolution << " (" << num_threads << " threads)" << std::endl;
      boost::shared_ptr<pclomp::NormalDistributionsTransform<PointT, PointT>> ndt(new pclomp::NormalDistributionsTransform<PointT, PointT>());
      if(num_threads > 0) {
        ndt->setNumThreads(num_threads);
      }
      ndt->setTransformationEpsilon(pnh.param<double>("transformation_epsilon", 0.01));
      ndt->setMaximumIterations(pnh.param<int>("maximum_iterations", 64));
      ndt->setResolution(ndt_resolution);
      if(nn_search_method == "KDTREE") {
        ndt->setNeighborhoodSearchMethod(pclomp::KDTREE);
      } else if (nn_search_method == "DIRECT1") {
        ndt->setNeighborhoodSearchMethod(pclomp::DIRECT1);
      } else {
        ndt->setNeighborhoodSearchMethod(pclomp::DIRECT7);
      }
      return ndt;
    }
  }

  return nullptr;
}

别看代码这么长，其实流程很简单，就是根据从配置文件读取的registration_method这个配置项来选择相应的匹配算法，选择匹配算法以后再把这个算法对应的参数从配置文件中读取出来即可。

可选择的匹配算法一共包括：ICP、GICP、GICP_OMP、NDT、NDT_OMP，这里ICP、GICP和NDT都是经典的点云配准方法，感兴趣的可以自己去网上查资料，而带OMP字样的两种方法，指的是对应方法的多线程版本，可以缩短匹配算法的执行时间。

最后说以下返回值，这个函数的返回类型是boost::shared_ptr<pcl::Registration<pcl::PointXYZI, pcl::PointXYZI>>，以上提到的所有方法都是pcl库下面的，而pcl::Registration就是所有这些方法的基类，都继承自它，所以不管选择了什么方法，都可以返回该方法的指针。

2. 点云匹配

点云匹配的代码在apps/scan_matching_odometry_nodelet.cpp中，其实就是对应一个matching函数，它的代码如下

Eigen::Matrix4f matching(const ros::Time& stamp, const pcl::PointCloud<PointT>::ConstPtr& cloud) {
    //还没有关键帧，说明它是第一帧，则直接把它设置成关键帧，并初始化位姿为单位阵，直接返回
    if(!keyframe) {
      prev_trans.setIdentity();
      keyframe_pose.setIdentity();
      keyframe_stamp = stamp;
      keyframe = downsample(cloud);
      registration->setInputTarget(keyframe);
      return Eigen::Matrix4f::Identity();
    }
    //当前点云给registration
    auto filtered = downsample(cloud);
    registration->setInputSource(filtered);
    //执行匹配
    pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
    registration->align(*aligned, prev_trans);
    //匹配结束，则返回它相对于关键帧额位姿
    if(!registration->hasConverged()) {
      NODELET_INFO_STREAM("scan matching has not converged!!");
      NODELET_INFO_STREAM("ignore this frame(" << stamp << ")");
      return keyframe_pose * prev_trans;
    }

    Eigen::Matrix4f trans = registration->getFinalTransformation();
    Eigen::Matrix4f odom = keyframe_pose * trans;
    
    //这段代码的主要功能是判断当前匹配的相对位姿是否过大，载体不可能短时间内有这么大的运动
    //所以如果出现了，就说明这一帧匹配可能不正常,则直接舍弃
    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;
      }
    }

    prev_trans = trans;


    auto keyframe_trans = matrix2transform(stamp, keyframe_pose, odom_frame_id, "keyframe");
    keyframe_broadcaster.sendTransform(keyframe_trans);
    
    //判断这一帧相对于关键帧的位置或角度是否比较大了
    //如果比较大，则需要更新关键帧，并把新的关键帧的点云设置成Target
    double delta_trans = trans.block<3, 1>(0, 3).norm();
    double delta_angle = std::acos(Eigen::Quaternionf(trans.block<3, 3>(0, 0)).w());
    double delta_time = (stamp - keyframe_stamp).toSec();
    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();
    }

    return odom;
  }

点云匹配的流程比较清晰，我已经在代码里加了注释了，直接看注释应该就能看明白。

3. 一点思考

它的这个匹配流程里面有优点也有缺点，优点是对当前的位姿做了异常判断，如果不合理，则舍弃这一帧，缺点是target只是当前关键帧，一帧点云作为target还是略显稀疏了，一般都有map的概念，这个map可以是一个滑窗，即相邻的几个关键帧共同组成一个target，这是常见的做法。它的前端里程计的效果并不好，这是一个重要的原因。

另一点就是在前端里程计中，包含了点云滤波和关键帧计算这两部分代码，滤波部分和之前介绍点云滤波的那一节里提到的prefiltering_nodelet.cpp文件中的代码完全一样，而关键帧计算部分的代码和之前在关键帧那一节里所提到的keyframe_updater.hpp的代码功能也一样，不清楚作者为啥把同样的东西写两遍，这明显是重复代码，至少从代码规范上不合理。