apollo 定位localization代码NDT算法模块解析(一)

---

---

前言

入行已经5年多，一直想分享点东西，希望能帮助到大家。
apollo 中对定位模块的代码及算法介绍实在是太少了，不知道是因为用的人少，还是大家觉得太简单了。
目前apollo 的定位代码中，主要提供了三种定位算法：RTK，NDT，MSF
RTK：直接使用组合导航输出的定位结果
NDT：使用组合导航+NDT点云匹配算法
MSF：RTK + NDT 算法融合（未开源）
本文重点介绍NDT算法.
apollo 将slam 分成了localization 和mapping 倆个独立模块，即建图和定位分开，先使用其它手段建立高精度地图，然后在已经生成的地图上进行定位。
废话少说，直接橹代码。

---

算法总体流程

1，将激光雷达的实时点云数据通过体素滤波进行下采样作为输入点云；
2，将当前位置周边的高精度地图点云数据进行网格化处理，计算每个网格的正太分布模型（期望，方差，网格内的点云数量，质心坐标等）作为匹配目标；
3，利用组合导航的原始运动估计作为匹配算法的初始值；
4，采用NDT(正太分布变换)配准算法+牛顿法迭代求解，通过线搜索方法求步长，最终输出定位结果

代码入口

ndt_localization_component.cc 仅是一个组件函数，提供输入输出接口

一、Init()

初始化参数及通道，message 输入，输出

tf2_broadcaster_.reset(new apollo::transform::TransformBroadcaster(node_)); //初始化发布的tf
if (!InitConfig()) {//参数初始化，加载配置参数
    AERROR << "Init Config false.";
    return false;
  }

  if (!InitIO()) {// 初始化通道名，订阅，发布的message
    AERROR << "Init Interval false.";
    return false;
  }

LidarCallback(const std::shared_ptrdrivers::PointCloud& lidar_msg) //接收雷达点云数据，传入ndt_localization

OdometryStatusCallback(const std::shared_ptrdrivers::gnss::InsStat& status_msg) //接收组合导航定位状态，传入ndt_localization

二、Proc( const std::shared_ptrlocalization::Gps& gps_msg)

代码循环入口，每收到一次组合导航数据就循环一次

localization_->OdometryCallback(gps_msg); //传入ndt_localization 模块

  if (localization_->IsServiceStarted()) {
    LocalizationEstimate localization;
    localization_->GetLocalization(&localization); //获取融合定位结果

    LocalizationStatus localization_status;
    localization_->GetLocalizationStatus(&localization_status); //获取融合定位状态及标识

    // publish localization messages
    PublishPoseBroadcastTopic(localization);  //发布定位结果
    PublishPoseBroadcastTF(localization); //发布tf
    PublishLocalizationStatusTopic(localization_status); //发布定位状态
    ADEBUG << "[OnTimer]: Localization message publish success!";
  }

总结

component 组件一般不进行复杂的逻辑处理，仅提供算法的输入，输出接口，这种设计模式便于代码移植，同时也能让人一眼看到入口函数，就像main(）函数一样。
NDT算法原理可以参考：
https://www.cnblogs.com/21207-iHome/p/8039741.html

版权申明：转载请注明出处，严禁用于商业用途。