【多传感器融合定位】【从零开始做自动驾驶定位_任佬】【所学到的东西汇总】

0 前言

• 参考：从零开始做自动驾驶定位（文章汇总）

1 开篇

1.1 代码工程的运行

• 这里记录一下，任佬的代码是如何运行的

1. 首先git clone对应版本标签的代码工程
2. 然后创建一个ros工程

mkdir -p catkin_ws/src
cd catkin_ws
mkdir build devel
catkin_make//编译命令

3. 然后将git得到的文件里面的所有文件放在src文件下，git的tag参考：【git和github的使用杂记】【环境虚拟机ubuntu18.04.6】的3.6 git 下载指定tag版本
4. 在主工程目录下catkin_make即可
5. 如果要使用clion编译，参考【在clion中配置使用和调试ros工程】

2 数据集

• 我有这个bag包，这一节就没怎么细看
• 就是把bag播放一下就可以啦

3 软件框架

3.1 运行

1. 编译报错

/usr/local/lib/libglog.a(logging.cc.o)：在函数‘__static_initialization_and_destruction_0(int, int)’中：
logging.cc:(.text+0x6961)：对‘google::FlagRegisterer::FlagRegisterer<bool>(char const*, char const*, char const*, bool*, bool*)’未定义的引用
logging.cc:(.text+0x6a05)：对‘google::FlagRegisterer::FlagRegisterer<bool>(char const*, char const*, char const*, bool*, bool*)’未定义的引用
logging.cc:(.text+0x6aa1)：对‘google::FlagRegisterer::FlagRegisterer<bool>(char const*, char const*, char const*, bool*, bool*)’未定义的引用
logging.cc:(.text+0x6b3d)：对‘google::FlagRegisterer::FlagRegisterer<bool>(char const*, char const*, char const*, bool*, bool*)’未定义的引用
logging.cc:(.text+0x6b6f)：对‘google::FlagRegisterer::FlagRegisterer<int>(char const*, char const*, char const*, int*, int*)’未定义的引用
logging.cc:(.text+0x6c0c)：对‘google::FlagRegisterer::FlagRegisterer<std::__cxx11::basic_string<char, std::char_traits<char>, 
...
CMakeFiles/Makefile2:510: recipe for target 'lidar_localization/CMakeFiles/test_frame_node.dir/all' failed
make[1]: *** [lidar_localization/CMakeFiles/test_frame_node.dir/all] Error 2
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2
Invoking "make -j8 -l8" failed

解决方法
这是因为缺少gflags的依赖链接，我们对lidar_localization下的CMakeLists.txt修改：

set(ALL_TARGET_LIBRARIES "" gflags)

3.2 学到的

3.2.1 对消息的订阅和发布的类的封装

• 对于消息的订阅和发布，如果订阅的topic太多，就会导致node文件很长，所以可以把每一类信息的订阅和发布封装成一个类，把callback作为类内函数存在

3.2.2 传感器数据结构的封装

• 这种封装就是为了适应一开始提到的接口功能，同时也可以配合第一步封装的订阅类和发布类使用，把订阅的数据直接封装好再供主程序取，这样封闭性更强。

3.2.3 缓冲区机制的了解和改进

• ROS在每次循环时，会逐个遍历各个subscriber的缓冲区，并且把缓冲区中的数据读完，不管有多少。
• 我们在subscriber的callback中解析数据的时候，一般都是把数据赋给一个变量，然后在融合的时候使用最后更新的值作为输入。
• 当融合算法处理时间比较长，超出了传感器信息的发送周期的时候，未被接收的数据会被放在每个subscriber对应的缓冲区中，等当前融合步骤处理完之后，下次ros从缓冲区中读取数据的时候，会先把前一个传感器信息的数据读完，此时这个变量就变成最新的前一个传感器信息数据，然后再读另一个传感器信息的数据，这就导致，我们再一次进入另一个传感器信息的回调函数时，使用的前一个传感器信息已经不是和另一个传感器信息同一时刻的数据了，而是它后面时刻的数据。
• 所以不用单个变量来存储数据，而是用容器。例如这里就是放在一个deque容器里的。
• 多个传感器产生了多个容器，往算法模块里输入的时候，应该按照各容器第一个数据的时间戳，把最早的那个输入送进去，循环这个过程，直到所有容器数据送完为止。

3.2.4 CMakeLists文件的规划

• 已经写入：【Cmake】【Cmake实践】【cmake的使用学习记录】的7.1 分模块分文件-创建子文件

3.2.5 Glog的使用

• 安装参考：【多传感器融合定位】【ubuntu18.06配置环境】【ROS melodic】【g2o】【ceres】【Geographic】【gflags】【glog】【sophus】【GTSAM】【gtest】
• GLog是google开源的代码日志开源库，它把信息分为INFO、WARNING、ERROR几个等级，使用时如果想添加日志信息，只需要一行代码就可以了：

LOG(INFO) << "自定义日志信息";

日志信息会自动存储在你定义的目录中。总之使用起来还是比直接使用std::cout要方便很多

3.3 实现功能

• 基本思路就是订阅GNSS、IMU、lidar信息，然后把GNSS信息中的位置、IMU信息中的姿态信息解析出来，粗略的充当当前帧的位姿信息，G然后用odometry发布出去，再把订阅的点云信息按照解析的位姿数据转换到当前车的位置和方向上去，最后发布出去。

4 前端里程计之初试

4.2 学到的

4.2.1 Eigen和tf中位姿表达方式转换

• 参考：【Eigen和tf中位姿表达方式转换】【tf::StampedTransform】【Eigen::Matrix4f】

4.2.2 tf位姿的base和child之间的关系

• 得到的变换矩阵transform，是Tbase_child，世纪上得到的是在base坐标系下的

tf::StampedTransform transform;
listener_.lookupTransform(base_frame_id_, child_frame_id_, ros::Time(0), transform);

4.2.3 NDT算法使用pcl的实现

• 参考：【点云配准算法】【NDT】

4.3 实现功能

5 前端里程计之代码优化

5.1 运行

1. 首先按照前面说好的存放文件
2. 然后catkin_make
3. 然后运行launch文件

source devel/setup.bash
roslaunch lidar_localization front_end.launch

4. 播放bag包
5. call保存地图服务

source devel/setup.bash
rosservice call /save_map

5.2 学到的

5.2.1 .yaml配置文件的使用

• 参考：【YAML】【YAML的实践】【YAML的使用学习记录】

5.2.2 代码中指定路径文件夹的检查与创建

• 参考：【多传感器融合】【使用代码检查和创建指定路径下文件夹】

5.2.3 使用多态的思想编写代码

• 就是如果我们要换不同的匹配方式怎么办，将来从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，更换匹配方式时所有调用的地方都要更换变量名字，显然不好。

5.2.4 善用类进行封装

6 传感器时间同步

7 里程计精度评价

7.2 学到的

7.2.1 数据存储，将里程计信息保存为文件

7.2.1.1 创建文件夹

bool FileManager::CreateDirectory(std::string directory_path) {
    
    
    if (!boost::filesystem::is_directory(directory_path)) {
    
    
        boost::filesystem::create_directory(directory_path);
    }
    if (!boost::filesystem::is_directory(directory_path)) {
    
    
        LOG(WARNING) << "无法建立文件夹: " << directory_path;
        return false;
    }
    return true;
}

7.2.1.2 创建文件

bool FileManager::CreateFile(std::ofstream& ofs, std::string file_path) {
    
    
    ofs.open(file_path.c_str(), std::ios::app);
    if (!ofs) {
    
    
        LOG(WARNING) << "无法生成文件: " << file_path;
        return false;
    }

    return true;
}

7.2.2 EVO评价

• 参考：【多传感器融合定位】【2】【python版本的切换和evo的安装】【evo的使用记录】

8 点云畸变补偿

8.2 学到的

8.2.1 关于轮速计和激光雷达关于不同安装位置对速度的补偿计算

• 数据中提供的速度是IMU所处位置的速度，而我们要的是激光雷达所处位置的速度，由于这两者并不重合，即存在杆臂，所以在车旋转时他们的速度并不一致，需要按照这两者之间的相对坐标，把速度转到雷达对应的位置上去。
• 这里首先是将IMU坐标系下的角/线速度变换到和激光雷达坐标下的角/线速度，使用相对位姿的旋转部分进行变换，所得到的变换后的角速度就是补偿后的激光雷达角速度。
• 然后计算线速度，要计算出两者之间的相对线速度，使用变换后的角速度和相对位置来补偿计算。
• 这个功能我们放在了sensor_data的velocity_data.cpp，把它作为VelocityData类的成员函数，只要给他一个相对坐标，它就自动把类内部成员变量转换了，调用时就一行程序

current_velocity_data_.TransformCoordinate(lidar_to_imu_);

8.2.2 看不懂看这个角速度与线速度变换

https://www.guyuehome.com/19879

9 建图系统结构优化

9.2 学到的

9.2.1 分模块写代码

10 后端优化

10.1 实现

• 这个g2o的安装煞费苦心

10.2 学到的

10.2.1 前段

• 前段匹配的位姿都是基于第一个关键帧坐标系下的，也可以说为里程计坐标系下的

11 闭环修正

11.2 学到的

11.2.1 仅保存关键帧位姿

• 在这之前都是把所有的位姿都保存
• 这节是只保存关键帧位姿

12 前端里程计扩展

12.2 学到的

12.2.1 模块的替换

• 以下直接摘自原文
  我们在这个程序框架设计之初，就把扩展性作为一个重要特性来指导我们的设计。

所谓扩展性，就是每一个功能模块都可以随意更换。

这个更换不仅是模块内部可以增加选项，比如后端优化中，在g2o的基础上增加gtsam选项，前端匹配中，在ndt的基础上增加icp选项，等等。而且还包括可以整个替换掉一个模块。

在之前提到的四大模块（前端、后端、闭环、显示）里，可替换选项最多的就是前端了，因为我们随处可见各种各样的激光里程计。

其实我们的程序架构已经具备了这样的扩展性。这次就以A-LOAM作为例子，把它加入我们的建图系统中来。

其实没啥可总结的，就是想说，用A-LOAM代替前端只是举个例子，各位可以把任何自己想用的前端按照同样的方法加入到这个系统中来。

需要注意的是，我们所有的数据都是按时间戳对齐的，所以如果您使用了别的里程计，那么应该保证输出的odom里的时间戳要和接收的点云里的时间戳保持一致。

13 关于建图的讨论

13.2 学到的

13.2.1 互补性

• 其实要不要建图的问题在不同场景的定位需求下确实有不同的答案，也即在不同场景的任务中要适应性地改变方案。
• 作为融合定位工程师，为了能够准确给出匹配的方案，我们首先要明白定位方案设计的核心思想是什么，各种不同的方案其实只是核心思想在不同场景下的应用而已。

我总结的核心其实就三个字：“互补性”。

• “互补性”这三个字怎么强调都不为过，定位方案中传感器选择的过程，就是“寻找互补性”的过程，而融合的过程，就是“执行互补性”的过程。

14 基于地图的定位

14.1 运行

1. 首先运行

roslaunch lidar_localization mapping.launch

播放完bag包之后，记得执行：

rosservice call /save_map

生成地图文件filtered_map.pcd

2. 记得在配置文件中设置好地图路径，按如下指令启动程序

roslaunch lidar_localization matching.launch

14.2 学到的