多传感器融合定位（1-3D激光里程计）5-实现调用pcl-icp-2 通过 config.yaml 实现接口的多态性
多传感器融合定位（1-3D激光里程计）5-实现调用pcl-icp-2 通过 config.yaml 实现接口的多态性
本章实现icp ndt的多态性，通过修改config 里的 yaml文件，实现icp ndt 里程计算法的方便替换
参考博客：
从零开始做自动驾驶定位(五): 前端里程计之代码优化
代码：任乾 从零开始自动驾驶(五)
NDT VS ICP
NDT
效果图
参数配置
NDT:
res : 1.0
step_size : 0.1
trans_eps : 0.01
max_iter : 30
ICP
效果图
可以明显看出 红色(ICP) 已经跑飞～
ICP:
max_correspondence_distance : 1.0
max_iter : 30
trans_eps : 1e-6
euclidean_fitness_eps : 1e-6
ransac_iter: 0
fitness_score: 0.3
主要修改过程
仿照 ndt_registration.cpp、ndt_registration.hpp 添加 icp_registration.cpp、icp_registration.hpp,修改front_end.cpp,修改 src/lidar_localization/config/config.yaml
(记得在对应编译器中添加新增的头文件路径)
添加icp_registration.hpp
/*
* @Description: ICP 匹配模块
* @Author: KaHo
* @Date: 2020-10-17 21:46:57
*/
#ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_REGISTRATION_HPP_
#define LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_REGISTRATION_HPP_
#include 添加icp_registration.cpp
/*
* @Description: ICP 匹配模块
* @Author: Ren Qian
* @Date: 2020-02-08 21:46:45
*/
#include "lidar_localization/models/registration/icp_registration.hpp"
#include "glog/logging.h"
namespace lidar_localization {
ICPRegistration::ICPRegistration(const YAML::Node& node)
:icp_ptr_(new pcl::IterativeClosestPoint<CloudData::POINT, CloudData::POINT>()) {
//获取参数
float max_correspondence_distance = node["max_correspondence_distance"].as<float>();
float max_iter = node["max_iter"].as<int>();
float trans_eps = node["trans_eps"].as<float>();
int euclidean_fitness_eps = node["euclidean_fitness_eps"].as<float>();
int ransac_iter = node["ransac_iter"].as<int>();
SetRegistrationParam(max_correspondence_distance, max_iter, trans_eps,euclidean_fitness_eps,ransac_iter);
}
ICPRegistration::ICPRegistration(float max_correspondence_distance,int max_iter, float trans_eps, float euclidean_fitness_eps,int ransac_iter)
:icp_ptr_(new pcl::IterativeClosestPoint<CloudData::POINT, CloudData::POINT>()) {
SetRegistrationParam(max_correspondence_distance, max_iter, trans_eps,euclidean_fitness_eps,ransac_iter);
}
bool ICPRegistration:: SetRegistrationParam(float max_correspondence_distance,int max_iter, float trans_eps, float euclidean_fitness_eps,int ransac_iter) {
//设置参数
icp_ptr_->setMaxCorrespondenceDistance(max_correspondence_distance);
icp_ptr_->setMaximumIterations(max_iter);
icp_ptr_->setTransformationEpsilon(trans_eps);
icp_ptr_->setEuclideanFitnessEpsilon(euclidean_fitness_eps);
icp_ptr_->setRANSACIterations(ransac_iter);
LOG(INFO) << "ICP 的匹配参数为：" << std::endl
<< "max_correspondence_distance: " << max_correspondence_distance << ", "
<< "max_iter: " << max_iter << ", "
<< "trans_eps: " << trans_eps << ", "
<< "euclidean_fitness_eps: " << euclidean_fitness_eps << ","
<< "ransac_iter: " << ransac_iter << ","
<< std::endl << std::endl;
return true;
}
bool ICPRegistration::SetInputTarget(const CloudData::CLOUD_PTR& input_target) {
icp_ptr_->setInputTarget(input_target);
return true;
}
bool ICPRegistration::ScanMatch(const CloudData::CLOUD_PTR& input_source,
const Eigen::Matrix4f& predict_pose,
CloudData::CLOUD_PTR& result_cloud_ptr,
Eigen::Matrix4f& result_pose) {
icp_ptr_->setInputSource(input_source);
icp_ptr_->align(*result_cloud_ptr, predict_pose);
result_pose = icp_ptr_->getFinalTransformation();
return true;
}
}
修改 front_end.cpp
主要修改部分，添加icp的选择
//选择配准方式
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;
//NDT配准
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 {
LOG(ERROR) << "没找到与 " << registration_method << " 相对应的点云匹配方式!";
return false;
}
return true;
}
修改config.yaml
data_path: ./ # 数据存放路径
# 匹配
registration_method: ICP # 选择点云匹配方法，目前支持：NDT ICP
# 局部地图
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
# 各配置选项对应参数
## 匹配相关参数
ICP:
max_correspondence_distance : 1.0
max_iter : 30
trans_eps : 1e-6
euclidean_fitness_eps : 1e-6
ransac_iter: 0
fitness_score: 0.3
NDT:
res : 1.0
step_size : 0.1
trans_eps : 0.01
max_iter : 30
## 滤波相关参数
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]