《多传感器融合定位》 3D激光里程计（二）
续： 《多传感器融合定位》 3D激光里程计（一）.
3D激光里程计习题
- 三、 自己实现一个激光匹配的方法,可以是 ICP, NDT 或者是 LOAM,新建一个接口类的实例,同样要求可在配置文件中对各方法进行切换。最后和作业 2 中所用的开源的 ICP,NDT 进行精度比较。
-
- 1.实现高斯牛顿的迭代方法实现ICP的求解
-
- (1)基本思路
- (2)代码实现
- (3)运行和评估
三、 自己实现一个激光匹配的方法,可以是 ICP, NDT 或者是 LOAM,新建一个接口类的实例,同样要求可在配置文件中对各方法进行切换。最后和作业 2 中所用的开源的 ICP,NDT 进行精度比较。
1.实现高斯牛顿的迭代方法实现ICP的求解
参考链接： https://blog.csdn.net/weixin_41281151/article/details/109439049.
(1)基本思路
构建最小二乘问题：
输入：
目标点云（构建kdtree，方便待匹配点云寻找最近点）
待匹配点云
初始位姿R和t
输出：
精确的R和t
关键：
在非线性优化中，迭代问题的最重要在于雅克比矩阵的求解，显然这个问题的模型是典型的李代数扰动模型（单个误差项关于位姿导数）：
求解：
单个点：
计算所有点的H和b，求解状态增量：
不断迭代直到满足条件.
(2)代码实现
添加Sophus库：
(在学习视觉slam14讲时安装了该库)
在工程的CMakeLists.txt下添加
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
target_link_libraries(front_end_node ${Sophus_LIBRARIES})
实现icp_registration_manual.hpp
在头文件中主要的不同是数据成员，增加了变换所需的T，R，t，以及一个kdtree的指针用来保存上一帧的点云数据（按照kdtree的格式）
#ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_REGISTRATION_MANUAL_HPP_
#define LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_REGISTRATION_MANUAL_HPP_
#include "sophus/se3.hpp"
#include 实现icp_registration_manual.cpp
其主要是实现ICP的迭代匹配
#include "lidar_localization/models/registration/icp_registration_manual.hpp"
#include 实现icp_registration_manual的接口
config.yaml
ICP:
max_dist : 1.0
trans_eps : 0.01
eculi_eps : 0.01
max_iter : 30
front_end.cpp
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 if (registration_method == "ICP")
{
registration_ptr = std::make_shared<ICPRegistration>(config_node[registration_method]);
}
else if (registration_method == "ICP_MANUAL")
{
registration_ptr = std::make_shared<ICPRegistrationManual>(config_node[registration_method]);
}
else {
LOG(ERROR) << "没找到与 " << registration_method << " 相对应的点云匹配方式!";
return false;
}
return true;
}
front_end.hpp
#include "lidar_localization/models/registration/icp_registration_manual.hpp"
(3)运行和评估
rivz显示：
保存地图：
分段评估：
整体评估：
结论：对比三种效果，ICP_MANIAL>NDT>ICP