深蓝学院-多传感器融合定位学习第二章

第一次课程作业

1. 跑通提供的工程框架
2. 使用基于NDT的匹配，之后使用evo评估得到的轨迹精度
3. 实现ICP-SVD点云匹配方法，并使用evo评估得到的轨迹精度

1.跑通提供的工程框架
编译环境

catkin_make
source ./devel/setup.bash

启动front_end.launch

roslaunch lidar_localization hello_kitti.launch 

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

rosbag play kitti_2011_10_03_drive_0027_synced.bag

2.使用基于NDT的匹配，之后使用evo评估两种算法得到的轨迹精度
在代码框架/lidar_localization/config/front_end/config.yaml修改registration_method为NDT

# 匹配
# TODO: implement your custom registration method and add it here
registration_method: NDT   # 选择点云匹配方法，目前支持：ICP, ICP_SVD, NDT, SICP

# 局部地图
key_frame_distance: 2.0 # 关键帧距离
local_frame_num: 20
local_map_filter: voxel_filter # 选择滑窗地图点云滤波方法，目前支持：voxel_filter

# rviz显示
display_filter: voxel_filter # rviz 实时显示点云时滤波方法，目前支持：voxel_filter

# 当前帧
frame_filter: voxel_filter # 选择当前帧点云滤波方法，目前支持：voxel_filter

## 滤波相关参数
voxel_filter:
    local_map:
        leaf_size: [0.6, 0.6, 0.6]
    frame:
        leaf_size: [1.3, 1.3, 1.3]
    display:
        leaf_size: [0.5, 0.5, 0.5]

# 各配置选项对应参数
## 匹配相关参数
ICP:
    max_corr_dist : 1.2
    trans_eps : 0.01
    euc_fitness_eps : 0.36
    max_iter : 30
ICP_SVD:
    max_corr_dist : 1.2
    trans_eps : 0.01
    euc_fitness_eps : 0.36
    max_iter : 10
NDT:
    res : 1.0
    step_size : 0.1
    trans_eps : 0.01
    max_iter : 30
SICP:
    p : 1.0
    mu : 10.0
    alpha : 1.2
    max_mu : 1e5
    max_icp : 100
    max_outer : 100
    max_inner : 1
    stop : 1e-5

然后编译环境运行节点

roslaunch lidar_localization front_end.launch 
rosbag play kitti_2011_10_03_drive_0027_synced.bag

使用evo工具进行评估

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

3.实现ICP-SVD点云匹配方法，并使用evo计算出指标
ScanMatch 函数改写

bool ICPSVDRegistration::ScanMatch(
    const CloudData::CLOUD_PTR& input_source, 
    const Eigen::Matrix4f& predict_pose, 
    CloudData::CLOUD_PTR& result_cloud_ptr,