从零开始做自动驾驶定位(五): 前端里程计之代码优化

本文纯属转载，并认真学习一遍，感谢大佬分享！

本文章配套源代码地址：https://github.com/Little-Potato-1990/localization_in_auto_driving

测试数据：https://pan.baidu.com/s/1TyXbifoTHubu3zt4jZ90Wg提取码: n9ys

本篇文章对应的代码Tag为 5.0

代码在后续可能会有调整，如和文章有出入，以实际代码为准

================================================================================

一、概述

看到本篇文章题目，可能就会有人问，为什么仅仅在已有的框架上加了个前端就要再次优化，而且专门写一篇文章来介绍？为什么不在上一篇就直接使用优化后的代码，而要放出一个不完善的中间版本？

其实这个问题在本系列文章的开篇中就提前解释了一下，就是怕引起误会，这里再详细说一下我的想法，可能有些啰嗦，不过我觉得咱们尽早达成这种共识是有必要的。

1. 本系列文章是面向过程，而非面向结果

所谓面向过程，是指我们要还原整个系统的开发过程，包括它所有重要的中间环节，而不只是给出开发结果。不然，就直接在github上放出一套完整的定位融合系统的源代码就好了，不必花这么多精力去写系列文章了，但那样就违背了我们的初衷，可能也违背了各位读者的期望。

2. 代码优化可不可以提前进行

所谓提前进行，是指我们能不能先设计一个好的结构，然后按照这个结构编写代码，而不是像现在这样分两步走，先用最简单的方式让它跑起来，再做优化。

其实这倒是一个开放性的问题。

在早期的软件开发行业，大家确实更倾向于前者，不仅要设计好，还要文档化，代码编写按文档严格执行，写完之后按文档去验收。

不过最近很多年，这类做法正在逐渐被抛弃，取而代之，是敏捷开发。先写最简单的框架，跑起来，再叠加功能和优化。

另外，各位根据自身经历回顾一下，在自己的体会里，如果这两种方式你都尝试过，哪种效率更高。各位可能很多人都读过《重构》这本书，整本书其实都是在讲一个核心问题：先把脑子里的简单想法落实在代码上，跑通它，然后调整它。现在这类思想越来越受推崇，我觉得可能跟人的特性有关，人在有序和简单的思维下，行动会更高效，当我们面对的是一个空白的屏幕，而脑子里是整个软件的复杂架构的时候，手指头会悬在键盘上方迟迟不知道怎么敲。

基于不断重构的做法可能会导致工作量会有所增加，但是任务进度的推进却更快了。所以，我个人现在就是这种习惯，我按照我的习惯把开发过程复现出来，如果各位有不一样的做法，欢迎探讨。

天也不早了，人也不少了，啰嗦这么多，该干点正事了。

二、优化事项

1. 功能模块降耦合

按照我们上一篇文章的介绍，前端按照模块可以分为匹配、滤波、局部地图滑窗、全局地图等，其中前两者是可以独立成为通用模块的。比如匹配，不仅我们前端里程计要用，以后闭环修正也要用，地图建完以后，基于地图做定位还要用，所以它应该具有共用性。滤波也一样，上面提到的各个环节它也参与。

明确了要独立的模块以后，我们要思考的就是怎样设计这个模块。首先就是接口，对于匹配，输入的是点云，输出的是位姿。对于滤波，输入输出都是点云。

除了接口以外，我们还需要考虑一件事情，就是如果我们要换不同的匹配方式怎么办，将来从ndt变成icp的时候，那么所有调用ndt模块进行匹配的代码都要改动吗？这显然是不划算的。解决这个问题的办法就是多态。

多态在程序设计中是一种常用的方法，它的实现方式是先定义一个基类，然后不同的具体实现分别作为它的不同子类存在。在程序运行时执行哪个实现，取决于我们在定义类的对象时用哪个子类做的实例化。以我们匹配模块的具体例子来说，我们定义了一个基类RegistrationInterface，它执行匹配的函数是ScanMatch()，NDTRegistration和ICPRegistration都是RegistrationInterface的子类，定义registration_ptr作为类对象的指针，那么registration_ptr->ScanMatch()执行的到底是ndt匹配还是icp匹配，取决于定义指针时用哪个子类做的实例化，具体来讲就是下面的指令。如果使用第一行初始化，则执行的是NDT匹配，如果是用第二行初始化，则执行的是ICP匹配。

// 使用ndt匹配
std::shared_ptr<RegistrationInterface> registration_ptr = std::make_shared<NDTRegistration>();
// 使用icp匹配
std::shared_ptr<RegistrationInterface> registration_ptr = std::make_shared<ICPRegistration>();

以上就是多态的实现原理。这样做的好处是，我们如果想更换匹配方式，只需要改变初始化就可以了。反之，如果不这样做，那么就得ndt和icp分别定义对象ndt_registration和icp_registration，更换匹配方式时所有调用的地方都要更换变量名字。

同样的，滤波模块我们也采用这样的方式设计，因为用固定尺寸方格滤波的方式有点太粗暴了，后续可能会尝试更好的滤波方式。

新建一个文件夹，名为models，存储匹配和滤波这两个类，以及以后可能新增的通用模块。

其实类里面的内容倒是很简单。

匹配类NDTRegistration内部主要函数为SetInputTarget和ScanMatch，作用分别是输入目标点云和执行点云匹配，并输出匹配后位姿。

bool NDTRegistration::SetInputTarget(const CloudData::CLOUD_PTR& input_target) {
    ndt_ptr_->setInputTarget(input_target);

    return true;
}

bool NDTRegistration::ScanMatch(const CloudData::CLOUD_PTR& input_source, 
                                const Eigen::Matrix4f& predict_pose, 
                                CloudData::CLOUD_PTR& result_cloud_ptr,
                                Eigen::Matrix4f& result_pose) {
    ndt_ptr_->setInputSource(input_source);
    ndt_ptr_->align(*result_cloud_ptr, predict_pose);
    result_pose = ndt_ptr_->getFinalTransformation();

    return true;
}

滤波类VoxelFilter内部主要函数就是Filter，这个函数参数同时包含输入和输出。

bool VoxelFilter::Filter(const CloudData::CLOUD_PTR& input_cloud_ptr, CloudData::CLOUD_PTR& filtered_cloud_ptr) {
    voxel_filter_.setInputCloud(input_cloud_ptr);
    voxel_filter_.filter(*filtered_cloud_ptr);

    return true;
}

2. 配置文件

为了方便调试，常用参数写在配置文件里是必须的，本工程采用yaml格式作为配置文件格式，在程序中，它可以把参数内容对应的放到YAML::Node格式的变量中，前端里程计的配置参数放在config/front_end文件夹下，为了把配置文件内容传入刚才所提到的匹配和滤波两个模块，每个模块均增加一个构造函数，函数参数就是YAML::Node类型，同时基类指针用哪个子类实例化也可以由配置参数决定。

这样，在front_end.cpp中就对应有两个函数InitRegistration和InitFilter，分别匹配和滤波模块的子类选择与参数配置功能。

bool FrontEnd::InitRegistration(std::shared_ptr<RegistrationInterface>& registration_ptr, const YAML::Node& config_node) {
    std::string registration_method = config_node["registration_method"].as<std::string>();
    LOG(INFO) << "点云匹配方式为：" << registration_method;

    if (registration_method == "NDT") {
        registration_ptr = std::make_shared<NDTRegistration>(config_node[registration_method]);
    } else {
        LOG(ERROR) << "没找到与 " << registration_method << " 相对应的点云匹配方式!";
        return false;
    }

    return true;
}

bool FrontEnd::InitFilter(std::string filter_user, std::shared_ptr<CloudFilterInterface>& filter_ptr, const YAML::Node& config_node) {
    std::string filter_mothod = config_node[filter_user + "_filter"].as<std::string>();
    LOG(INFO) << filter_user << "选择的滤波方法为：" << filter_mothod;

    if (filter_mothod == "voxel_filter") {
        filter_ptr = std::make_shared<VoxelFilter>(config_node[filter_mothod][filter_user]);
    } else {
        LOG(ERROR) << "没有为 " << filter_user << " 找到与 " << filter_mothod << " 相对应的滤波方法!";
        return false;
    }

    return true;
}

3. 关键帧点云和地图保存功能

上一篇里，我们的关键帧点云和全局地图都是放在内存里，这样是不利于大场景建图的，内存爆掉都是有可能的。所以我们的做法是没产生一个关键帧就把它存放在硬盘里，然后把点云释放掉，全局地图默认不生成，必须主动发送指令才会生成，生成之后会把地图保存成pcd文件，并在rviz上显示，最后再重新把地图释放掉，清理内存。

生成地图的指令是用ROS的service实现的，ROS制作service的方法如果不清楚还要麻烦各位在网上查一查。本工程对应的生成地图的指令是

rosservice call /save_map

地图默认路径是在工程目录下的slam_data文件夹下，您也可以在刚才提到的配置文件中自己定义路径，第一行的data_path变量就是它了。

注意，这样修改之后，运行程序时只显示局部地图，只有在主动发送地图生成命令时才生成并显示全局地图，所以数据处理结束输入一次看一下完整地图就行。

4. ROS流程封装

按照上一篇的做法，node文件的main函数中实现的功能有

读数据
判断是否有数据
初始化标定文件
初始化gnss
使用里程计模块计算数据
发送数据

可见，步骤还是有一些的，按照一个合理的变成策略，每个功能应该用一个函数封装，不然看着也乱。而如果直接在node文件中做这一步，文件中代码并不会减少，还要定义很多全局变量，并和流程无关的代码搅在一起，这样显然是不合理的。

解决这样的问题，借助类同样也是一个好的方法，我们把对应的流程封装在一个类里，所有通用变量放在头文件里作为类成员变量，各个步骤作为一个函数封装好，最后只留一个Run()函数作为接口给node文件去调用，这样就变得很简洁，我们看修改之后的Run函数就知道了。

bool FrontEndFlow::Run() {
    ReadData();

    if (!InitCalibration()) 
        return false;

    if (!InitGNSS())
        return false;
    
    while(HasData()) {
        if (!ValidData())
            continue;
        UpdateGNSSOdometry();
        if (UpdateLaserOdometry())
            PublishData();
    }

    return true;
}

此时node文件main函数就剩下类对象定义和调用了

_front_end_flow_ptr = std::make_shared<FrontEndFlow>(nh);

ros::Rate rate(100);
while (ros::ok()) {
    ros::spinOnce();

    _front_end_flow_ptr->Run();

    rate.sleep();
}

封装好的类和FrontEnd类同样放在front_end文件夹下。

最后，图就不贴了，和上一篇是一样的，本篇只是修改代码布局。

上一篇：从零开始做自动驾驶定位(四): 前端里程计之初试

下一篇：从零开始做自动驾驶定位(六): 传感器时间同步