【Apollo源码分析】系列的第五部分【localization】

【Apollo源码分析】系列的第五部分【localization】

localization 是车辆 定位模块，是自动驾驶的核心模块之一。 
目前Apollo的定位模块大概有1000行代码。

localization的输入:

• GPS全球信号
• IMU惯性测量单元信号

localization的输出:

• 由localization/proto/localization.proto文件描述的LocalizationEstimate定位信息

---

目前Apollo 1.0采用的是 RTK-GPS定位法。

RTK-GPS定位法：

• RTK（Real - time kinematic）载波相位差分技术，是实时处理两个测量站载波相位观测量的差分方法，将基准站采集的载波相位发给用户接收机，进行求差解算坐标。这是一种新的常用的GPS测量方法，以前的静态、快速静态、动态测量都需要事后进行解算才能获得厘米级的精度，而RTK是能够在野外实时得到厘米级定位精度的测量方法，它采用了载波相位动态实时差分方法，是GPS应用的重大里程碑，它的出现为工程放样、地形测图，各种控制测量带来了新曙光，极大地提高了外业作业效率。
• 高精度的GPS测量必须采用载波相位观测值，RTK定位技术就是基于载波相位观测值的实时动态定位技术，它能够实时地提供测站点在指定坐标系中的三维定位结果，并达到厘米级精度。在RTK作业模式下，基准站通过数据链将其观测值和测站坐标信息一起传送给流动站。流动站不仅通过数据链接收来自基准站的数据，还要采集GPS观测数据，并在系统内组成差分观测值进行实时处理，同时给出厘米级定位结果，历时不足一秒钟。流动站可处于静止状态，也可处于运动状态；可在固定点上先进行初始化后再进入动态作业，也可在动态条件下直接开机，并在动态环境下完成整周模糊度的搜索求解。在整周未知数解固定后，即可进行每个历元的实时处理，只要能保持四颗以上卫星相位观测值的跟踪和必要的几何图形，则流动站可随时给出厘米级定位结果。

GPS-RTK 至少有一个基准站接收机，一个流动站接收机。

基准站的接收机有两个任务：

• 1）接收卫星信号
• 2）把自己的观测量通过发送给流动站

流动站有三个任务：

• 1）接收卫星信号
• 2）接收基准站所发送的基准站观测量
• 3）对卫星信号与基准站观测量进行对比处理，从而对自己的观测量进行修正

关于RTK-GPS可以参阅： 
[1]https://en.wikipedia.org/wiki/Real_Time_Kinematic 
[2]https://baike.baidu.com/item/RTK 
[3]https://www.zhihu.com/question/24471733/answer/28676022 
[4]http://www.sbsm.gov.cn/article/zszygx/chzs/chkp/ddcl/200912/20091200059052.shtml

---

localization/main.cc

照例先看看 localization文件夹下面最重要的文件main.cc 写了什么：

   
   
   
   

    
    
    
    
#include "modules/common/apollo_app.h"
    
    
    
    
#include "modules/localization/localization.h"
    
    
    
    
APOLLO_MAIN(::apollo::localization::Localization)
   
   
   
   

结合common/apollo_app.h文件，上述代码一看便知是用于注册localization模块，并运行本模块。而本文件没有重写ApolloApp类就不能实例化，那么只能是localization.h文件重写了ApolloApp类。

localization/localization.h

   
   
   
   

    
    
    
    
class Localization : public apollo::common::ApolloApp
    
    
    
    
果然，Localization类继承了ApolloApp类，并重写了父类的纯虚函数接口。
   
   
   
   

重写的接口有:

   
   
   
   

    
    
    
    
std::string Name() const override;
    
    
    
    
模块名称
   
   
   
   

   
   
   
   

    
    
    
    
apollo::common::Status Init() override;
    
    
    
    
初始化步骤
   
   
   
   

   
   
   
   

    
    
    
    
apollo::common::Status Start() override;
    
    
    
    
模块执行内容 
   
   
   
   

   
   
   
   

    
    
    
    
void Stop() override;
    
    
    
    
模块清理内容
   
   
   
   

以上全都是public访问权限。

本模块独有的 private函数有:

   
   
   
   

    
    
    
    
void RegisterLocalizationMethods();
    
    
    
    
工厂函数。注册RTK定位实例和CAMERA定位实例。
   
   
   
   

再看看localization.cc.cc文件源码：

   
   
   
   

    
    
    
    
std::string Localization::Name() const {
    
    
    
    
  return FLAGS_localization_module_name;//模块名称
    
    
    
    
}
    
    
    
    

    
    
    
    
void Localization::RegisterLocalizationMethods() {
    
    
    
    
  localization_factory_.Register(//工厂函数。注册RTK定位方法的实例。
    
    
    
    
      LocalizationConfig::RTK,
    
    
    
    
      []() -> LocalizationBase* { return new RTKLocalization(); });
    
    
    
    

    
    
    
    
  localization_factory_.Register(
    
    
    
    
      LocalizationConfig::CAMERA,//工厂函数。注册CAMERA定位方法实例。
    
    
    
    
      []() -> LocalizationBase* { return new CameraLocalization(); });
    
    
    
    
}
    
    
    
    

    
    
    
    
Status Localization::Init() { //初始化操作，包括获取配置文件信息+注册定位方法实例
    
    
    
    
  RegisterLocalizationMethods();
    
    
    
    
  if (!apollo::common::util::GetProtoFromFile(FLAGS_localization_config_file,
    
    
    
    
                                              &config_)) {
    
    
    
    
    AERROR << "failed to load localization config file "
    
    
    
    
           << FLAGS_localization_config_file;
    
    
    
    
    return Status(ErrorCode::LOCALIZATION_ERROR,
    
    
    
    
                  "failed to load localization config file: " +
    
    
    
    
                      FLAGS_localization_config_file);
    
    
    
    
  }
    
    
    
    

    
    
    
    
  return Status::OK();
    
    
    
    
}
    
    
    
    

    
    
    
    
Status Localization::Start() { //开始本模块的工作内容
    
    
    
    
  localization_ =
    
    
    
    
      localization_factory_.CreateObject(config_.localization_type());//注册定位方法工厂信息
    
    
    
    
  if (!localization_) {
    
    
    
    
    return Status(ErrorCode::LOCALIZATION_ERROR,
    
    
    
    
                  "localization is not initialized with config : " +
    
    
    
    
                      config_.DebugString());
    
    
    
    
  }
    
    
    
    
  localization_->Start(); //开始定位
    
    
    
    

    
    
    
    
  return Status::OK();
    
    
    
    
}
    
    
    
    

    
    
    
    
void Localization::Stop() {}
   
   
   
   

localization/localization.h先分析到这里。先分析其他然后再回过头来总结整理。

localization/common/localization_gflags.h

.h文件和.cc文件声明和定义了一系列localization模块会用到的string/int32/bool/double变量。概览如下：

localization_gflags.h：

   
   
   
   

    
    
    
    
#include "gflags/gflags.h"
    
    
    
    
DECLARE_string(localization_module_name); //声明变量
    
    
    
    
DECLARE_double(localization_publish_freq);//声明变量
    
    
    
    
DECLARE_string(localization_config_file); //声明变量
    
    
    
    
以下略...
   
   
   
   

localization_gflags.cc:

   
   
   
   

    
    
    
    

    
    
    
    
DEFINE_string(localization_module_name, "localization",
    
    
    
    
              "localization module name"); 
    
    
    
    
              //定义变量
    
    
    
    
DEFINE_double(localization_publish_freq, 100,
    
    
    
    
              "localization publishing frequency.");
    
    
    
    
              //定义变量
    
    
    
    
DEFINE_string(rtk_adapter_config_file,
    
    
    
    
              "modules/localization/conf/rtk_adapter.conf",
    
    
    
    
              "rtk adapter configuration");
    
    
    
    
              //定义变量
    
    
    
    
以下略...
   
   
   
   

整个模块需要用到的预定义变量还是很多的。上面只是概览。

下面分析具体的定位算法，包括RTK-GPS定位算法，基于图像的定位算法，IMU惯性测量单元定位算法。

首先分析定位算法基类class LocalizationBase 。

modules/localization/localization_base.h

LocalizationBase是定位算法的基类。其他的任何定位算法都继承自这个类。 
包括 RTKLocalization和CameraLocalization都继承自这个类。

该类提供定位算法的公有抽象接口。 
2个public成员函数，纯虚函数，子类必须重写这2个接口才能实例化。

   
   
   
   

    
    
    
    
class LocalizationBase {
    
    
    
    
 public:
    
    
    
    
  virtual ~LocalizationBase() = default;//虚基类应该有虚析构函数
    
    
    
    

    
    
    
    
  /**
    
    
    
    
   * @brief module start function
    
    
    
    
   * @return start status
    
    
    
    

    
    
    
    
   本模块执行任务，返回状态码
    
    
    
    
   */
    
    
    
    
  virtual apollo::common::Status Start() = 0;
    
    
    
    

    
    
    
    
  /**
    
    
    
    
   * @brief module stop function
    
    
    
    
   * @return stop status
    
    
    
    

    
    
    
    
   清理工作
    
    
    
    
   */
    
    
    
    
  virtual apollo::common::Status Stop() = 0;
    
    
    
    
};
   
   
   
   

基类比较简单。下面分析具体的定位算法。

camera.proto文件定义了camera相机image。

   
   
   
   

    
    
    
    

    
    
    
    
message Camera {
    
    
    
    
  optional apollo.common.Header header = 1;
    
    
    
    

    
    
    
    
  // The image from camera
    
    
    
    
  optional bytes image = 2;
    
    
    
    
  // The image width
    
    
    
    
  optional uint32 width = 3;
    
    
    
    
  // The image height
    
    
    
    
  optional uint32 height = 4;
    
    
    
    
}
   
   
   
   

camera_parameter.proto定义了相机内参和外参

   
   
   
   

    
    
    
    
//内参。
    
    
    
    
message CameraIntrinsicParameter {
    
    
    
    
  // focus x pixels
    
    
    
    
  optional double fx = 1; // pixels
    
    
    
    
  // focus y pixels
    
    
    
    
  optional double fy = 2; // pixels
    
    
    
    
  // center x pixels
    
    
    
    
  optional double cx = 3; // pixels
    
    
    
    
  // center y pixels
    
    
    
    
  optional double cy = 4; // pixels
    
    
    
    
}
    
    
    
    
//外参
    
    
    
    
message CameraExtrinsicParameter {
    
    
    
    
  // camera rotation parameters [-pi - pi]
    
    
    
    
  optional double roll = 1;
    
    
    
    
  optional double pitch = 2;
    
    
    
    
  optional double yaw = 3;
    
    
    
    
  // coordinates in the vehicle coordinates [meters]
    
    
    
    
  optional double tx = 4; // meters
    
    
    
    
  optional double ty = 5; // meters
    
    
    
    
  optional double tz = 6; // meters
    
    
    
    
}
    
    
    
    

    
    
    
    
message CameraParameter {
    
    
    
    
  optional CameraIntrinsicParameter intrisic_parameter = 1;
    
    
    
    
  optional CameraExtrinsicParameter extrisic_parameter = 2;
    
    
    
    
}
   
   
   
   

localization/camera/camera_localization.h

   
   
   
   

    
    
    
    
class RTKLocalization : public LocalizationBase 
   
   
   
   

CameraLocalization类继承自LocalizationBase，重写了Start()和Stop() 2个函数。 
该类主要是利用摄像机image进行定位算法。 
补充】基于相机的定位算法主要是基于视觉传感器摄像机(简称VO)和惯性测量单元的 visual inertial odometry（简称VIO）由于现在Apollo也没有具体的算法，暂时不表。更多相关知识可以搜索vslam。

---

   
   
   
   

    
    
    
    
【CameraLocalization】类 继承自【LocalizationBase】，重写了Start()和Stop() 2个函数。
    
    
    
    
该类主要是利用摄像机进行定位算法。但是目前Apollo的1.0版本还没有用具体的相机定位算法。可能后续会补充。
    
    
    
    
目前只是添加了这个功能原型，搭建了消息收发的接口。但是目前没有实用。
    
    
    
    
 */
    
    
    
    
class CameraLocalization : public LocalizationBase {
    
    
    
    
 public:
    
    
    
    
  CameraLocalization();
    
    
    
    
  virtual ~CameraLocalization() = default;
    
    
    
    

    
    
    
    
  /**
    
    
    
    
   * @brief module start function
    
    
    
    
   * @return start status
    
    
    
    

    
    
    
    
   初始化配置，并执行本类的定位算法
    
    
    
    
   */
    
    
    
    
  apollo::common::Status Start() override;
    
    
    
    

    
    
    
    
  /**
    
    
    
    
   * @brief module stop function
    
    
    
    
   * @return stop status
    
    
    
    

    
    
    
    
  清理工作
    
    
    
    
   */
    
    
    
    
  apollo::common::Status Stop() override;
    
    
    
    

    
    
    
    
 private:
    
    
    
    
  void OnTimer(const ros::TimerEvent &event);//数据到达，进行一次定位算法
    
    
    
    
  bool PublishLocalization();//发布gps和camera二者的综合定位结果
    
    
    
    

    
    
    
    
  void RunWatchDog();//检查时间合理性，是否延迟过大。
    
    
    
    

    
    
    
    
  bool PrepareLocalizationMsg(LocalizationEstimate *localization);
    
    
    
    
  //该函数尚未实现。应该是利用camera产生定位结果
    
    
    
    

    
    
    
    
  //根据gps_msg和camera_msg，得到综合的定位结果
    
    
    
    
  bool CreateLocalizationMsg(const ::apollo::localization::Gps &gps_msg,
    
    
    
    
                             const ::apollo::localization::Camera &camera_msg,
    
    
    
    
                             LocalizationEstimate *localization);
    
    
    
    

    
    
    
    
 private:
    
    
    
    
  ros::Timer timer_;  //ros系统的时间
    
    
    
    

    
    
    
    
  //监视本模块工作状态，以便于发送给ros topic。
    
    
    
    
  apollo::common::monitor::Monitor monitor_; 
    
    
    
    

    
    
    
    
  CameraParameter camera_parameter_; //相机参数，包括内参和外参。啥是内参,啥是外参？google一下 ^_^。
    
    
    
    
  double last_received_timestamp_sec_ = 0.0;//最近接收data的时间
    
    
    
    
  double last_reported_timestamp_sec_ = 0.0;//最近发布定位的时间
    
    
    
    
  bool use_imu_ = false;                    //融合imu
    
    
    
    
  bool service_started_ = false;            //开始工作
    
    
    
    
};
   
   
   
   

localization/rtk/rtk_localization.h

Real_Time_Kinematic，即差分GPS技术：将一台GPS接收机安置在基准站上进行观测。根据基准站已知精密坐标，计算出基准站到卫星的距离改正数，并由基准站实时将这一数据发送出去。用户接收机在进行GPS观测的同时，也接收到基准站发出的改正数，并对其定位结果进行改正，从而提高定位精度。

目前Apollo的1.0主要依靠RTK-GPS定位算法。其他算法目前尚为实现。 
关于RTK-GPS可以参阅https://en.wikipedia.org/wiki/Real_Time_Kinematic

优点:精度能达到厘米级。 
缺点：要安装基站，成本高，在没有基站的户外就不适用了。

RTKLocalization类继承自LocalizationBase，重写了Start()和Stop() 2个函数。

   
   
   
   

    
    
    
    
class RTKLocalization : public LocalizationBase {
    
    
    
    
 public:
    
    
    
    
  RTKLocalization();
    
    
    
    
  virtual ~RTKLocalization() = default;
    
    
    
    

    
    
    
    
  /**
    
    
    
    
   * @brief module start function
    
    
    
    
   * @return start status
    
    
    
    
   */
    
    
    
    
  apollo::common::Status Start() override;
    
    
    
    

    
    
    
    
  /**
    
    
    
    
   * @brief module stop function
    
    
    
    
   * @return stop status
    
    
    
    
   */
    
    
    
    
  apollo::common::Status Stop() override;
    
    
    
    

    
    
    
    
 private:
    
    
    
    
  void OnTimer(const ros::TimerEvent &event);//数据到达，进行一次定位算法
    
    
    
    
  void PublishLocalization();//发布 定位结果
    
    
    
    
  void RunWatchDog();//检查时间合理性，是否延迟过大。
    
    
    
    

    
    
    
    
  void PrepareLocalizationMsg(LocalizationEstimate *localization);
    
    
    
    
  ///计算gps的定位结果
    
    
    
    

    
    
    
    
  void ComposeLocalizationMsg(const ::apollo::localization::Gps &gps,
    
    
    
    
                              const ::apollo::localization::Imu &imu,
    
    
    
    
                              LocalizationEstimate *localization);
    
    
    
    
  ///将gps和imu做数据融合。
    
    
    
    

    
    
    
    
  ///根据时间戳找到大于该时间戳的第一个imu数据
    
    
    
    
  bool FindMatchingIMU(const double gps_timestamp_sec, Imu *imu_msg);
    
    
    
    

    
    
    
    
  ///对imu数据进行插值计算，得到给定timestamp_sec的值
    
    
    
    
  void InterpolateIMU(const Imu &imu1, const Imu &imu2,
    
    
    
    
                      const double timestamp_sec, Imu *msgbuf);
    
    
    
    

    
    
    
    
  template <class T>
    
    
    
    
  T InterpolateXYZ(const T &p1, const T &p2, const double &frac1);
    
    
    
    

    
    
    
    
 private:
    
    
    
    
  ros::Timer timer_;//ros系统的时间
    
    
    
    
  //监视本模块工作状态，以便于发送给ros topic。
    
    
    
    
  apollo::common::monitor::Monitor monitor_;
    
    
    
    

    
    
    
    
  const std::vector map_offset_;//在地图上的偏移量
    
    
    
    
  double last_received_timestamp_sec_ = 0.0;//最近接收data的时间
    
    
    
    
  double last_reported_timestamp_sec_ = 0.0;//最近发布定位的时间
    
    
    
    
  bool service_started_ = false; //开始工作
   
   
   
   

rtk_localization_test.cc

理解RTK-GPS的定位算法最好还是通过test测量代码理解。并且还有定位实例：

   
   
   
   

    
    
    
    
TEST_F(RTKLocalizationTest, InterpolateIMU) {
    
    
    
    
  // timestamp inbetween + time_diff is big enough(>0.001), interpolate
    
    
    
    
  {
    
    
    
    
    //加载imu1测量结果
    
    
    
    
    apollo::localization::Imu imu1;
    
    
    
    
    load_data("modules/localization/testdata/1_imu_1.pb.txt", &imu1);
    
    
    
    

    
    
    
    
    //加载imu2测量结果
    
    
    
    
    apollo::localization::Imu imu2;
    
    
    
    
    load_data("modules/localization/testdata/1_imu_2.pb.txt", &imu2);
    
    
    
    

    
    
    
    
    //期望结果
    
    
    
    
    apollo::localization::Imu expected_result;
    
    
    
    
    load_data("modules/localization/testdata/1_imu_result.pb.txt",
    
    
    
    
              &expected_result);
    
    
    
    

    
    
    
    
    //2和exp完全一样。
    
    
    
    

    
    
    
    
    apollo::localization::Imu imu;
    
    
    
    
    double timestamp = 1173545122.69;
    
    
    
    

    
    
    
    
    ///imu返回的是timestamp处的插值结果。
    
    
    
    
    rtk_localizatoin_->InterpolateIMU(imu1, imu2, timestamp, &imu);
    
    
    
    
    ///插值结果就是2
    
    
    
    
    EXPECT_EQ(expected_result.DebugString(), imu.DebugString());
    
    
    
    
  }
    
    
    
    

    
    
    
    
  // timestamp inbetween + time_diff is too small(<0.001), no interpolate
    
    
    
    
  {
    
    
    
    
    ///1和exp在时间上不一致，其余一致。
    
    
    
    
    apollo::localization::Imu imu1;
    
    
    
    
    load_data("modules/localization/testdata/2_imu_1.pb.txt", &imu1);
    
    
    
    

    
    
    
    
    ///imu2与imu1时间戳很小，则直接使用imu1的数据。
    
    
    
    
    apollo::localization::Imu imu2;
    
    
    
    
    load_data("modules/localization/testdata/2_imu_2.pb.txt", &imu2);
    
    
    
    

    
    
    
    
    apollo::localization::Imu expected_result;
    
    
    
    
    load_data("modules/localization/testdata/2_imu_result.pb.txt",
    
    
    
    
              &expected_result);
    
    
    
    

    
    
    
    
    apollo::localization::Imu imu;
    
    
    
    
    double timestamp = 1173545122.2001;
    
    
    
    
    //返回在timestamp处的插值结果。
    
    
    
    
    rtk_localizatoin_->InterpolateIMU(imu1, imu2, timestamp, &imu);
    
    
    
    

    
    
    
    
    ///不插值，直接使用imu1的数据
    
    
    
    
    EXPECT_EQ(expected_result.DebugString(), imu.DebugString());
    
    
    
    
  }
    
    
    
    

    
    
    
    
  // timestamp < imu1.timestamp
    
    
    
    
  {
    
    
    
    
    apollo::localization::Imu imu1;
    
    
    
    
    load_data("modules/localization/testdata/1_imu_1.pb.txt", &imu1);
    
    
    
    

    
    
    
    
    apollo::localization::Imu imu2;
    
    
    
    
    load_data("modules/localization/testdata/1_imu_2.pb.txt", &imu2);
    
    
    
    

    
    
    
    
    apollo::localization::Imu expected_result;
    
    
    
    
    load_data("modules/localization/testdata/1_imu_1.pb.txt", &expected_result);
    
    
    
    

    
    
    
    
    apollo::localization::Imu imu;
    
    
    
    
    double timestamp = 1173545122;
    
    
    
    
    //timestamp小于imu1的时间戳，则直接使用imu1的数据，不插值
    
    
    
    
    rtk_localizatoin_->InterpolateIMU(imu1, imu2, timestamp, &imu);
    
    
    
    

    
    
    
    
    EXPECT_EQ(expected_result.DebugString(), imu.DebugString());
    
    
    
    
  }
    
    
    
    

    
    
    
    
  // timestamp > imu2.timestamp
    
    
    
    
  {
    
    
    
    
    apollo::localization::Imu imu1;
    
    
    
    
    load_data("modules/localization/testdata/1_imu_1.pb.txt", &imu1);
    
    
    
    

    
    
    
    
    apollo::localization::Imu imu2;
    
    
    
    
    load_data("modules/localization/testdata/1_imu_2.pb.txt", &imu2);
    
    
    
    

    
    
    
    
    apollo::localization::Imu expected_result;
    
    
    
    
    load_data("modules/localization/testdata/1_imu_1.pb.txt", &expected_result);
    
    
    
    

    
    
    
    
    apollo::localization::Imu imu;
    
    
    
    
    double timestamp = 1173545122.70;
    
    
    
    
    //timestamp大于imu2的时间戳，则直接使用imu1的数据。
    
    
    
    
    rtk_localizatoin_->InterpolateIMU(imu1, imu2, timestamp, &imu);
    
    
    
    

    
    
    
    
    EXPECT_EQ(expected_result.DebugString(), imu.DebugString());
    
    
    
    
  }
    
    
    
    
}
   
   
   
   

   
   
   
   

    
    
    
    

    
    
    
    
TEST_F(RTKLocalizationTest, ComposeLocalizationMsg) {
    
    
    
    
  // FLAGS_enable_map_reference_unify: false
    
    
    
    
  {
    
    
    
    
    FLAGS_enable_map_reference_unify = false;
    
    
    
    

    
    
    
    
    apollo::localization::Gps gps;
    
    
    
    
    load_data("modules/localization/testdata/3_gps_1.pb.txt", &gps);
    
    
    
    

    
    
    
    
    apollo::localization::Imu imu;
    
    
    
    
    load_data("modules/localization/testdata/3_imu_1.pb.txt", &imu);
    
    
    
    

    
    
    
    
    apollo::localization::LocalizationEstimate expected_result;
    
    
    
    
    load_data("modules/localization/testdata/3_localization_result_1.pb.txt",
    
    
    
    
              &expected_result);
    
    
    
    

    
    
    
    
    apollo::localization::LocalizationEstimate localization;
    
    
    
    

    
    
    
    
    //使用GPS和imu对定位结果进行数据融合。
    
    
    
    
    rtk_localizatoin_->ComposeLocalizationMsg(gps, imu, &localization);
    
    
    
    

    
    
    
    
    EXPECT_EQ(1, localization.header().sequence_num());
    
    
    
    
    EXPECT_STREQ("localization", localization.header().module_name().c_str());
    
    
    
    
    EXPECT_STREQ(expected_result.pose().DebugString().c_str(),
    
    
    
    
                 localization.pose().DebugString().c_str());
    
    
    
    
  }
   
   
   
   

总结

localization模块主要定义了使用RTK-GPS和camera相结合的定位方法，并且融合了imu传感器。但是基于图像的定位方法尚未发布。目前Apollo主要采用RKT-GPS并辅助imu用于车辆定位。总的来说，目前还有很大的改进空间，但是Apollo的定位框架目前已搭建完毕，能run起来，精度+普适廉价性+可靠性+多传感器融合算法应该是未来改进的地方。每一个方向都值得深究。

---

本文首发于微信公众号slamcode。

 注释版源码：源码