多传感器融合定位（2-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 <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]