欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

多传感器融合定位(2-3D激光里程计)5-实现调用pcl-icp-2 通过 config.yaml 实现接口的多态性

程序员文章站 2022-03-04 18:40:46
...

多传感器融合定位(2-3D激光里程计)5-实现调用pcl-icp-2 通过 config.yaml 实现接口的多态性

本章实现icp ndt的多态性,通过修改config 里的 yaml文件,实现icp ndt 里程计算法的方便替换
参考博客:
从零开始做自动驾驶定位(五): 前端里程计之代码优化
代码:任乾 从零开始自动驾驶(五)

NDT VS ICP

NDT

效果图

多传感器融合定位(2-3D激光里程计)5-实现调用pcl-icp-2 通过 config.yaml 实现接口的多态性
多传感器融合定位(2-3D激光里程计)5-实现调用pcl-icp-2 通过 config.yaml 实现接口的多态性多传感器融合定位(2-3D激光里程计)5-实现调用pcl-icp-2 通过 config.yaml 实现接口的多态性

参数配置

NDT:
    res : 1.0
    step_size : 0.1
    trans_eps : 0.01
    max_iter : 30

ICP

效果图

可以明显看出 红色(ICP) 已经跑飞~
多传感器融合定位(2-3D激光里程计)5-实现调用pcl-icp-2 通过 config.yaml 实现接口的多态性

多传感器融合定位(2-3D激光里程计)5-实现调用pcl-icp-2 通过 config.yaml 实现接口的多态性

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
(记得在对应编译器中添加新增的头文件路径)

多传感器融合定位(2-3D激光里程计)5-实现调用pcl-icp-2 通过 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]