ps: 因为个人能力不足,本次代码 和 伪代码 框图 主要是参考 go on 大佬的,本人只是搬运工做记录
/*
* @Description: ICP 匹配模块
* @Author: KaHo
* @Date: 2020-10-17 21:46:57
*/
#ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_REGISTRATION_MANUAL_HPP_
#define LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_REGISTRATION_MANUAL_HPP_
#include
#include
#include "lidar_localization/models/registration/registration_interface.hpp" //配准接口
#include "se3.hpp" // 添加 se3
namespace lidar_localization {
class ICPRegistrationManual: public RegistrationInterface {
public:
ICPRegistrationManual(const YAML::Node &node);
ICPRegistrationManual(float max_correspond_dis, int max_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_correspond_dis,int max_iter);
void calculateTrans(const CloudData::CLOUD_PTR &input_cloud ); // 计算旋转矩阵
private:
CloudData::CLOUD_PTR target_cloud_;
pcl::KdTreeFLANN<CloudData::POINT>::Ptr kdtree_ptr_;
float max_correspond_distance_; // 阈值
int max_iterator_; //最大迭代次数
Eigen::Matrix3f rotation_matrix_; //旋转矩阵
Eigen::Vector3f translation_; //平移矩阵
Eigen::Matrix4f transformation_; // 转换矩阵
};
}
#endif
/*
* @Description: ICP 匹配模块
* @Author: Ren Qian
* @Date: 2020-02-08 21:46:45
*/
#include "lidar_localization/models/registration/icp_registration_manual.hpp"
#include "glog/logging.h"
#include
namespace lidar_localization {
ICPRegistrationManual::ICPRegistrationManual(const YAML::Node& node)
: kdtree_ptr_(new pcl::KdTreeFLANN<CloudData::POINT>) {
// 获取参数
float max_correspond_dis = node["max_correspondence_distance"].as<float>();
int max_iter = node["max_iter"].as<int>(); //最大迭代次数
SetRegistrationParam(max_correspond_dis, max_iter);
}
ICPRegistrationManual :: ICPRegistrationManual(float max_correspond_dis,
int max_iter)
: kdtree_ptr_(new pcl :: KdTreeFLANN<CloudData :: POINT> ) {
SetRegistrationParam(max_correspond_dis,max_iter);
}
bool ICPRegistrationManual::SetRegistrationParam(float max_correspond_dis,
int max_iter){
max_correspond_distance_ = max_correspond_dis;
max_iterator_ = max_iter;
LOG(INFO) << "ICP Manual 的匹配参数为 : " << std::endl
<< "max_correspond_dis: " << max_correspond_dis << ", "
<< "max_iter: " << max_iter << std::endl
<< std :: endl;
return true;
}
bool ICPRegistrationManual::SetInputTarget(
const CloudData::CLOUD_PTR& input_target) {
target_cloud_.reset(new CloudData::CLOUD);
target_cloud_ = input_target;
kdtree_ptr_ ->setInputCloud(input_target);
return true;
}
bool ICPRegistrationManual::ScanMatch(const CloudData::CLOUD_PTR &input_source,
const Eigen::Matrix4f& predict_pose,
CloudData::CLOUD_PTR& result_cloud_ptr,
Eigen::Matrix4f& result_pose) {
transformation_ = predict_pose;
rotation_matrix_ = transformation_.block<3, 3>(0, 0) ; //取旋转矩阵
translation_ = transformation_.block<3, 1>(0, 3); //取平移矩阵
calculateTrans(input_source); //计算变换矩阵
pcl::transformPointCloud(*input_source, *result_cloud_ptr, transformation_); // 对点云进行变换
result_pose = transformation_;
return true;
}
void ICPRegistrationManual::calculateTrans(const CloudData::CLOUD_PTR &input_source){
CloudData::CLOUD_PTR transformed_cloud(new CloudData::CLOUD);
int knn = 1; // 进行 1nn的搜索
int iterator_num = 0;
while(iterator_num < max_iterator_)
{
pcl::transformPointCloud(*input_source,*transformed_cloud,transformation_); // 对点云进行变换
Eigen::Matrix<float,6,6> Hessian;
Eigen::Matrix<float,6,1>B;
Hessian.setZero();
B.setZero(); // 归零
for(size_t i =0; i < transformed_cloud->size(); ++i)
{
auto ori_point = input_source->at(i);
if(!pcl::isFinite(ori_point))
continue;
auto transformed_point = transformed_cloud->at(i);
std::vector<float> distances;
std::vector<int>indexs;
kdtree_ptr_->nearestKSearch(transformed_point,knn,indexs,distances); // knn搜索
if(distances[0] > max_correspond_distance_)
{
continue;
}
Eigen::Vector3f closet_point = Eigen::Vector3f(target_cloud_->at(indexs[0]).x, target_cloud_->at(indexs[0]).y ,
target_cloud_->at(indexs[0]).z );
// 计算 原始点 与 最邻近点 的 距离
Eigen::Vector3f err_dis =
Eigen::Vector3f(transformed_point.x,transformed_point.y,transformed_point.z) - closet_point;
Eigen::Matrix<float,3,6> Jacobian(Eigen::Matrix<float,3,6>::Zero());
Jacobian.leftCols<3>() = Eigen::Matrix3f::Identity();
Jacobian.rightCols<3>() =
-rotation_matrix_* Sophus::SO3f::hat(Eigen::Vector3f(ori_point.x,ori_point.y,ori_point.z)) ;
Hessian += Jacobian.transpose()* Jacobian;
B += -Jacobian.transpose()*err_dis;
}
iterator_num++;
if(Hessian.determinant() == 0)
{
continue;
}
Eigen::Matrix<float,6,1> delta_x = Hessian.inverse()*B;
translation_ += delta_x.head<3>();
auto delta_rotation = Sophus::SO3f::exp(delta_x.tail<3>());
rotation_matrix_ *= delta_rotation.matrix();
transformation_.block<3,3>(0,0) = rotation_matrix_;
transformation_.block<3,1>(0,3) = translation_;
}
}
}
//选择配准方式
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 if (registration_method == "ICP_MANUAL") {
registration_ptr = std::make_shared<ICPRegistrationManual>(config_node[registration_method]);
}
else {
LOG(ERROR) << "没找到与 " << registration_method << " 相对应的点云匹配方式!";
return false;
}
return true;
}
data_path: ./ # 数据存放路径
# 匹配
registration_method: ICP_MANUAL # 选择点云匹配方法,目前支持:NDT ICP ICP_MANUAL
# 局部地图
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
ICP_MANUAL:
max_correspondence_distance : 1.0
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]
ps: 对比图 来自 go on 助教的讲解