マルチセンサーフュージョンポジショニング（1-3Dレーザーオドメーター）5-pcl-icp-2の呼び出しを実現し、config.yamlを介してインターフェイスの多態性を実現します
この章では、icp ndtの多態性を実現し、構成内のyamlファイルを変更することでicp ndtマイレージ計算方法の便利な置き換えを実現します。
リファレンスブログ：
ゼロからの自動パイロット測位（5）：フロントエンド走行距離計の
コード最適化コード：Renganゼロからの自律運転（5）
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 <pcl/registration/icp.h>
#include "lidar_localization/models/registration/registration_interface.hpp" //配准接口
namespace lidar_localization {
class ICPRegistration: public RegistrationInterface {
public:
ICPRegistration(const YAML::Node& node);
ICPRegistration(float max_correspondence_distance,int max_iter, float trans_eps, float euclidean_fitness_eps,int ransac_iter);
bool SetInputTarget(const CloudData::CLOUD_PTR& input_target) override;
bool ScanMatch(const CloudData::CLOUD_PTR& input_source,
const Eigen::Matrix4f& predict_pose,
CloudData::CLOUD_PTR& result_cloud_ptr,
Eigen::Matrix4f& result_pose) override;
private:
bool SetRegistrationParam(float max_correspondence_distance,int max_iter, float trans_eps, float euclidean_fitness_eps,int ransac_iter);
private:
pcl::IterativeClosestPoint<CloudData::POINT, CloudData::POINT>::Ptr icp_ptr_; //声明
};
}
#endif
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]