深蓝学院 《多传感器融合定位》 第1章作业

对应任乾 从零开始做激光SLAM（文章汇总） 前八讲

1. 了解 KITTI 数据集,完成相关依赖库的安装,运行作业代码,提供程序运行成功的截图

预备知识 ROS学习教程：
古月 · ROS入门21讲
ROS/Tutorials

使用 kitti2bag工具包 制作rosbag：

使用的 KITTI rosbag信息如下：

jevin@jevin-GS65-Stealth-9SE:/media/jevin/File/my_file/BaiduyunDownload/转换后的bag文件/2011_10_03/bag$ rosbag info kitti_2011_10_03_drive_0027_synced.bag 
path:        kitti_2011_10_03_drive_0027_synced.bag
version:     2.0
duration:    7:50s (470s)
start:       Oct 03 2011 12:55:34.00 (1317617735.00)
end:         Oct 03 2011 13:03:25.83 (1317618205.83)
size:        24.0 GB
messages:    63616
compression: none [18184/18184 chunks]
types:       geometry_msgs/TwistStamped [98d34b0043a2093cf9d9345ab6eef12e]
             sensor_msgs/CameraInfo     [c9a58c1b0b154e0e6da7578cb991d214]
             sensor_msgs/Image          [060021388200f6f0f447d0fcd9c64743]
             sensor_msgs/Imu            [6a62c6daae103f4ff57a132d6f95cec2]
             sensor_msgs/NavSatFix      [2d3a8cd499b9b4a0249fb98fd05cfa48]
             sensor_msgs/PointCloud2    [1158d486dd51d683ce2f1be655c3c181]
             tf2_msgs/TFMessage         [94810edda583a504dfda3829e70d7eec]
topics:      /kitti/camera_color_left/camera_info    4544 msgs    : sensor_msgs/CameraInfo    
             /kitti/camera_color_left/image_raw      4544 msgs    : sensor_msgs/Image         
             /kitti/camera_color_right/camera_info   4544 msgs    : sensor_msgs/CameraInfo    
             /kitti/camera_color_right/image_raw     4544 msgs    : sensor_msgs/Image         
             /kitti/camera_gray_left/camera_info     4544 msgs    : sensor_msgs/CameraInfo    
             /kitti/camera_gray_left/image_raw       4544 msgs    : sensor_msgs/Image         
             /kitti/camera_gray_right/camera_info    4544 msgs    : sensor_msgs/CameraInfo    
             /kitti/camera_gray_right/image_raw      4544 msgs    : sensor_msgs/Image         
             /kitti/oxts/gps/fix                     4544 msgs    : sensor_msgs/NavSatFix     
             /kitti/oxts/gps/vel                     4544 msgs    : geometry_msgs/TwistStamped
             /kitti/oxts/imu                         4544 msgs    : sensor_msgs/Imu           
             /kitti/velo/pointcloud                  4544 msgs    : sensor_msgs/PointCloud2   
             /tf                                     4544 msgs    : tf2_msgs/TFMessage        
             /tf_static                              4544 msgs    : tf2_msgs/TFMessage

1.1 前端里程计节点 front_end_node 介绍

bool FrontEndFlow::Run() {
    if (!ReadData())
        return false;

    if (!InitCalibration()) 
        return false;

    if (!InitGNSS())
        return false;

    while(HasData()) {
        if (!ValidData())
            continue;

        UpdateGNSSOdometry();
        if (UpdateLaserOdometry()) {
            PublishData();
            SaveTrajectory();
        }
    }

    return true;
}

ReadData()：获取来自 Subscriber 的原始数据存储在std::deque中，同步各传感器数据
InitCalibration()：获取 lidar_to_imu_ 坐标系变换矩阵
InitGNSS()：初始化GNSS数据
UpdateGNSSOdometry()：更新GNSS位置，并通过 lidar_to_imu_ 变换到雷达坐标系下
UpdateLaserOdometry()：更新–>

bool FrontEnd::Update(const CloudData& cloud_data, Eigen::Matrix4f& cloud_pose)

• 滤波 pcl::VoxelGrid

• 局部地图容器中没有关键帧，代表是第一帧数据，把当前帧数据作为第一个关键帧，并更新局部地图容器和全局地图容器

• 使用NDT匹配 pcl::NormalDistributionsTransform::Ptr

    // 不是第一帧，就正常匹配
    registration_ptr_->ScanMatch(filtered_cloud_ptr, predict_pose, result_cloud_ptr_, current_frame_.pose);
    cloud_pose = current_frame_.pose;

    // 更新相邻两帧的相对运动
    step_pose = last_pose.inverse() * current_frame_.pose;
    predict_pose = current_frame_.pose * step_pose;
    last_pose = current_frame_.pose;

• 根据距离判断选取新的关键帧 -->

bool FrontEnd::UpdateWithNewFrame(const Frame& new_key_frame) 

• • 关键帧点云存储到硬盘里，节省内存
  • 更新局部地图 使用pcl::transformPointCloud，根据NDT匹配获得的pose将局部地图中每个关键帧位姿统一
  • 更新ndt匹配的目标点云 SetInputTarget(local_map_ptr_)

1.2 运行程序

运行代码执行步骤如下：

• 将下下载的压缩包合成一个再解压获得kitti_2011_10_03_drive_0027_synced.bag

cat bag_file*>bag.tar.gz

• 创建工作空间，将提供的lidar_localization包加入workspace，编译，配置环境变量

mkdir -p ros_workspace/src
cd ros_workspace
catkin_make
source ./devel/setup.bash

• （可选）若要在clion执行则在配置过环境变量的同一终端中启动clion，Clion配置ROS环境参考链接：
  Jetbrains ros-setup-tutorial
  ROS wiki IDE

sh /opt/clion-2020.1.1/bin/clion.sh

• 新建终端启动ros

roscore

• 启动test_frame.launch

roslaunch lidar_localization test_frame.launch

• （可选）启动rviz直接播放bag
  打开终端，cd到“kitti_test”文件夹下，输入指令

rviz -d display_bag.rviz

• 播放bag
  打开终端，cd到bag所在目录，输入指令

rosbag play kitti_2011_10_03_drive_0027_synced.bag

... logging to /home/jevin/.ros/log/83be515c-1801-11eb-9b4a-803253b3bbcc/roslaunch-jevin-GS65-Stealth-9SE-9962.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://jevin-GS65-Stealth-9SE:37849/

SUMMARY
========

PARAMETERS
 * /rosdistro: melodic
 * /rosversion: 1.14.9

NODES
  /
    front_end_node (lidar_localization/front_end_node)
    rviz (rviz/rviz)

auto-starting new master
process[master]: started with pid [9972]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 83be515c-1801-11eb-9b4a-803253b3bbcc
process[rosout-1]: started with pid [9983]
started core service [/rosout]
process[rviz-2]: started with pid [9987]
process[front_end_node-3]: started with pid [9991]
Could not create logging file: No such file or directory
COULD NOT CREATE A LOGGINGFILE 20201027-110715.9991!I20201027 11:07:15.661072  9991 front_end.cpp:61] 地图点云存放地址：/home/jevin/code/multi-sensors-homework/hw1/catkin-ws/src/lidar_localization/slam_data
I20201027 11:07:15.661288  9991 front_end.cpp:70] 关键帧点云存放地址：/home/jevin/code/multi-sensors-homework/hw1/catkin-ws/src/lidar_localization/slam_data/key_frames

I20201027 11:07:15.661334  9991 front_end.cpp:78] 点云匹配方式为：NDT
I20201027 11:07:15.661494  9991 ndt_registration.cpp:35] NDT 的匹配参数为：
res: 1, step_size: 0.1, trans_eps: 0.01, max_iter: 30

I20201027 11:07:15.661507  9991 front_end.cpp:92] local_map选择的滤波方法为：voxel_filter
I20201027 11:07:15.661557  9991 voxel_filter.cpp:27] Voxel Filter 的参数为：
0.6, 0.6, 0.6

I20201027 11:07:15.661564  9991 front_end.cpp:92] frame选择的滤波方法为：voxel_filter
I20201027 11:07:15.661571  9991 voxel_filter.cpp:27] Voxel Filter 的参数为：
1.3, 1.3, 1.3

I20201027 11:07:15.661576  9991 front_end.cpp:92] display选择的滤波方法为：voxel_filter
I20201027 11:07:15.661583  9991 voxel_filter.cpp:27] Voxel Filter 的参数为：
0.5, 0.5, 0.5

用ROS的service实现生成地图：

rosservice call /save_map

地图默认路径是在工程目录下的slam_data文件夹下，使用

pcl_viewer map.pcd 

查看点云地图，但地图太大没加载出来，查看了一个关键帧：

2. 在作业代码中,仿照写好的基于 NDT 的匹配,重新建立一个基于 ICP 的里程计(可以使用 PCL),要求可在配置文件中切换两种方法。之后使用 evo 评估两种算法得到的轨迹精度

学习PCL：
pointclouds.org
Tutorials

ICP documentation
The algorithm has several termination criteria:
1. Number of iterations has reached the maximum user imposed number of iterations (via setMaximumIterations)
2. The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via setTransformationEpsilon)
3. The sum of Euclidean squared errors is smaller than a user defined threshold (via setEuclideanFitnessEpsilon)

Usage example:

IterativeClosestPoint<PointXYZ, PointXYZ> icp;
// Set the input source and target
icp.setInputCloud (cloud_source);
icp.setInputTarget (cloud_target);
 
// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
icp.setMaxCorrespondenceDistance (0.05);
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (50);
// Set the transformation epsilon (criterion 2)
icp.setTransformationEpsilon (1e-8);
// Set the euclidean distance difference epsilon (criterion 3)
icp.setEuclideanFitnessEpsilon (1);
 
// Perform the alignment
icp.align (cloud_source_registered);
 
// Obtain the transformation that aligned cloud_source to cloud_source_registered
Eigen::Matrix4f transformation = icp.getFinalTransformation ();

从以上介绍中得知使用PCL中的ICP方法需要设置4个参数

记录一个很傻逼的bug：
我创建icp_registration.hpp为了方便修改直接从ndt_registration.hpp复制过来修改了一下文件名，但下面预编译指令忘记修改
#ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_REGISTRATION_HPP_
#define LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_REGISTRATION_HPP_
导致一直编译报错找不到ICPRegistration类，检查了好久才看出来，这么低级的错误，自闭

使用evo计算轨迹误差参考:
evo/wiki
理解具体参数
比较好的博客

evo_rpe kitti ground_truth.txt laser_odom.txt -r trans_part --delta 100 --plot --plot_mode xyz

evo_rpe 相对姿态误差是用于调查SLAM轨迹的局部一致性的度量。时间戳对齐之后， 真实位姿和估计位姿均每隔一段相同时间计算位姿的变化量， 然后对该变化量做差， 以获得相对位姿误差
-r trans_part 只计算位置误差
–delta 100 相隔固定差100帧位姿差的精度
–plot 画图
–plot_mode xyz 画图模式

NDT结果评估:

（运行结果展示在上一节）

RPE w.r.t. translation part (m)
for delta = 100 (frames) using consecutive pairs
(not aligned)

       max	1.856809
      mean	0.816793
    median	0.721714
       min	0.185244
      rmse	0.906731
       sse	36.997272
       std	0.393714

ICP结果评估:

运行结果：

jevin@jevin-GS65-Stealth-9SE:~/code/multi-sensors-homework/hw1/catkin-ws/src/lidar_localization/launch$ roslaunch front_end.launch 
... logging to /home/jevin/.ros/log/8674a6ce-190c-11eb-85f1-803253b3bbcc/roslaunch-jevin-GS65-Stealth-9SE-11001.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://jevin-GS65-Stealth-9SE:40079/

SUMMARY
========

PARAMETERS
 * /rosdistro: melodic
 * /rosversion: 1.14.9

NODES
  /
    front_end_node (lidar_localization/front_end_node)
    rviz (rviz/rviz)

auto-starting new master
process[master]: started with pid [11011]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 8674a6ce-190c-11eb-85f1-803253b3bbcc
process[rosout-1]: started with pid [11022]
started core service [/rosout]
process[rviz-2]: started with pid [11025]
process[front_end_node-3]: started with pid [11030]
I20201028 18:58:35.906061 11030 front_end.cpp:63] 地图点云存放地址：/home/jevin/code/multi-sensors-homework/hw1/catkin-ws/src/lidar_localization/slam_data
I20201028 18:58:35.906211 11030 front_end.cpp:72] 关键帧点云存放地址：/home/jevin/code/multi-sensors-homework/hw1/catkin-ws/src/lidar_localization/slam_data/key_frames

I20201028 18:58:35.906220 11030 front_end.cpp:80] 点云匹配方式为：ICP
I20201028 18:58:35.906759 11030 icp_registration.cpp:45] ICP params:
max_corr_dist: 1, trans_eps: 0.01, euc_fitness_eps: 0.01, max_iter: 30

I20201028 18:58:35.906774 11030 front_end.cpp:98] local_map选择的滤波方法为：voxel_filter
I20201028 18:58:35.906781 11030 voxel_filter.cpp:27] Voxel Filter 的参数为：
0.6, 0.6, 0.6

I20201028 18:58:35.906787 11030 front_end.cpp:98] frame选择的滤波方法为：voxel_filter
I20201028 18:58:35.906793 11030 voxel_filter.cpp:27] Voxel Filter 的参数为：
1.3, 1.3, 1.3

I20201028 18:58:35.906798 11030 front_end.cpp:98] display选择的滤波方法为：voxel_filter
I20201028 18:58:35.906805 11030 voxel_filter.cpp:27] Voxel Filter 的参数为：
0.5, 0.5, 0.5

evo精度评估：

RPE w.r.t. translation part (m)
for delta = 100 (frames) using consecutive pairs
(not aligned)

       max	2222.513729
      mean	291.544767
    median	83.780087
       min	0.660096
      rmse	555.821329
       sse	13902180.756229
       std	473.221934

参数按照讲评中设置，调用PCL库的ICP方法效果不是很好，一开始轨迹基本相差不大，之后尺度飘移激光前端里程计的地图逐渐变大，最终发散

3. 自己实现一个激光匹配的方法,可以是 ICP, NDT 或者是 LOAM,新建一个接口类的实例,同样要求可在配置文件中对各方法进行切换。最后和作业 2 中所用的开源的 ICP,NDT 进行精度比较