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

多传感器融合定位(2-3D激光里程计)4-实现调用pcl-icp-1

程序员文章站 2022-03-04 18:41:58
...

多传感器融合定位(2-3D激光里程计)4-实现调用pcl-icp-1

本次使用的是任乾老师 tag4.0的代码,修改原来NDT的接口代码
参考博客:
从零开始做自动驾驶定位(四): 前端里程计之初试
无人驾驶算法学习(五):激光里程计之帧间匹配算法

NDT vs ICP

红色为经ICP处理后的里程计运动轨迹,黄色为GNSS的轨迹(可视为groundTruth),地图上白色是当前帧,彩色是地图

NDT带参数处理 里程计

    设置NDT匹配参数
    ndt_ptr_->setResolution(1.0);
    ndt_ptr_->setStepSize(0.1);
    ndt_ptr_->setTransformationEpsilon(0.01);
    ndt_ptr_->setMaximumIterations(30);

多传感器融合定位(2-3D激光里程计)4-实现调用pcl-icp-1

ICP 里程计

参数设置:

    //设置ICP参数配置
    //icp_ptr_->setMaxCorrespondenceDistance(1e-10) ;      //设置最大对应点的欧式距离,只有对应点之间的距离小于该设置值的对应点才作为ICP计算的点对。默认值为:1.7976931348623157e+308,基本上对所有点都计算了匹配点。 
//    icp_ptr_->setTransformationEpsilon(1e-7);
//    icp_ptr_->setMaxCorrespondenceDistance(0.1);
    icp_ptr_->setEuclideanFitnessEpsilon(1e-6);    //设置前后两次迭代的点对的欧式距离均值的最大容差,迭代终止条件之三,默认值为:-std::numeric_limits::max ()
    icp_ptr_->setMaximumIterations(50);     //最大迭代次数
    icp_ptr_->setTransformationEpsilon(1e-6);  //设置前后两次迭代的转换矩阵的最大容差(epsilion),一旦两次迭代小于这个最大容差,则认为已经收敛到最优解,迭代停止。迭代停止条件之二,默认值为:0 
    icp_ptr_->setMaxCorrespondenceDistance(1); //最大欧式距离差值

多传感器融合定位(2-3D激光里程计)4-实现调用pcl-icp-1
多传感器融合定位(2-3D激光里程计)4-实现调用pcl-icp-1

调用PCL-ICP的主要修改部分

源代码 为任乾老师 从零开始自动驾驶tag4.0的代码 Little-Potato-1990 /localization_in_auto_driving
使用vscode 进行ros下的编程
ros项目调试:vscode下配置开发ROS项目

主要稍微修改的工程部分为 front_end.hpp front_end.cpp

//front_end.cpp 添加 PCL-ICP的实例化
icp_ptr_(new pcl::IterativeClosestPoint<CloudData::POINT, CloudData::POINT>()),                          //初始化ICP
//front_end.hpp 添加 PCL-ICP的头文件
#include <pcl/registration/icp.h>                        
//front_end.hpp 添加 PCL-ICP的声明
pcl::IterativeClosestPoint<CloudData::POINT, CloudData::POINT>::Ptr icp_ptr_;                         

全部修改代码

front_end.cpp

/*
 * @Description: 前端里程计算法
 * @Author: Ren Qian
 * @Date: 2020-02-04 18:53:06
 */  
#include "lidar_localization/front_end/front_end.hpp"  
#include <cmath>
#include <pcl/common/transforms.h>   
#include "glog/logging.h"

namespace lidar_localization {
FrontEnd::FrontEnd():
    icp_ptr_(new pcl::IterativeClosestPoint<CloudData::POINT, CloudData::POINT>()),                          //初始化ICP
    //ndt_ptr_(new pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>()),     //初始化 NDT      
     local_map_ptr_(new CloudData::CLOUD()),
     global_map_ptr_(new CloudData::CLOUD()),
     result_cloud_ptr_(new CloudData::CLOUD()) {

    // 给个默认参数,以免类的使用者在匹配之前忘了设置参数
    cloud_filter_.setLeafSize(1.3, 1.3, 1.3);
    local_map_filter_.setLeafSize(0.6, 0.6, 0.6);
    display_filter_.setLeafSize(0.5, 0.5, 0.5);
    // 设置NDT匹配参数
    // ndt_ptr_->setResolution(1.0);
    // ndt_ptr_->setStepSize(0.1);
    // ndt_ptr_->setTransformationEpsilon(0.01);
    // ndt_ptr_->setMaximumIterations(30);

    //设置ICP参数配置
    //icp_ptr_->setMaxCorrespondenceDistance(1e-10) ;      //设置最大对应点的欧式距离,只有对应点之间的距离小于该设置值的对应点才作为ICP计算的点对。默认值为:1.7976931348623157e+308,基本上对所有点都计算了匹配点。 
//    icp_ptr_->setTransformationEpsilon(1e-7);
//    icp_ptr_->setMaxCorrespondenceDistance(0.1);
    icp_ptr_->setEuclideanFitnessEpsilon(1e-6);    //设置前后两次迭代的点对的欧式距离均值的最大容差,迭代终止条件之三,默认值为:-std::numeric_limits::max ()
    icp_ptr_->setMaximumIterations(50);     //最大迭代次数
    icp_ptr_->setTransformationEpsilon(1e-6);  //设置前后两次迭代的转换矩阵的最大容差(epsilion),一旦两次迭代小于这个最大容差,则认为已经收敛到最优解,迭代停止。迭代停止条件之二,默认值为:0 
    icp_ptr_->setMaxCorrespondenceDistance(1); //最大欧式距离差值
}

Eigen::Matrix4f FrontEnd::Update(const CloudData& cloud_data) {
    current_frame_.cloud_data.time = cloud_data.time;
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*cloud_data.cloud_ptr, *current_frame_.cloud_data.cloud_ptr, indices);

    CloudData::CLOUD_PTR filtered_cloud_ptr(new CloudData::CLOUD());
    cloud_filter_.setInputCloud(current_frame_.cloud_data.cloud_ptr);
    cloud_filter_.filter(*filtered_cloud_ptr);

    static Eigen::Matrix4f step_pose = Eigen::Matrix4f::Identity();
    static Eigen::Matrix4f last_pose = init_pose_;
    static Eigen::Matrix4f predict_pose = init_pose_;
    static Eigen::Matrix4f last_key_frame_pose = init_pose_;

    // 局部地图容器中没有关键帧,代表是第一帧数据
    // 此时把当前帧数据作为第一个关键帧,并更新局部地图容器和全局地图容器
    if (local_map_frames_.size() == 0) {
        current_frame_.pose = init_pose_;
        UpdateNewFrame(current_frame_);
        return current_frame_.pose;
    }

    // 不是第一帧,就正常匹配
    icp_ptr_->setInputSource(filtered_cloud_ptr);      // 输入当前帧
    icp_ptr_->align(*result_cloud_ptr_, predict_pose);
    current_frame_.pose = icp_ptr_->getFinalTransformation();

    // 更新相邻两帧的相对运动
    step_pose = last_pose.inverse() * current_frame_.pose;
    predict_pose = current_frame_.pose * step_pose;
    last_pose = current_frame_.pose;

    // 匹配之后根据距离判断是否需要生成新的关键帧,如果需要,则做相应更新
    if (fabs(last_key_frame_pose(0,3) - current_frame_.pose(0,3)) + 
        fabs(last_key_frame_pose(1,3) - current_frame_.pose(1,3)) +
        fabs(last_key_frame_pose(2,3) - current_frame_.pose(2,3)) > 2.0) {
        UpdateNewFrame(current_frame_);
        last_key_frame_pose = current_frame_.pose;
    }

    return current_frame_.pose;
}

bool FrontEnd::SetInitPose(const Eigen::Matrix4f& init_pose) {
    init_pose_ = init_pose;
    return true;
}

bool FrontEnd::SetPredictPose(const Eigen::Matrix4f& predict_pose) {
    predict_pose_ = predict_pose;
    return true;
}

void FrontEnd::UpdateNewFrame(const Frame& new_key_frame) {
    Frame key_frame = new_key_frame;
    // 这一步的目的是为了把关键帧的点云保存下来
    // 由于用的是共享指针,所以直接复制只是复制了一个指针而已
    // 此时无论你放多少个关键帧在容器里,这些关键帧点云指针都是指向的同一个点云
    key_frame.cloud_data.cloud_ptr.reset(new CloudData::CLOUD(*new_key_frame.cloud_data.cloud_ptr));
    CloudData::CLOUD_PTR transformed_cloud_ptr(new CloudData::CLOUD());
    
    // 更新局部地图,实现滑窗,把时间靠前的关键帧踢出去
    local_map_frames_.push_back(key_frame);
    while (local_map_frames_.size() > 20) {
        local_map_frames_.pop_front();
    }
    local_map_ptr_.reset(new CloudData::CLOUD());
    for (size_t i = 0; i < local_map_frames_.size(); ++i) {
        pcl::transformPointCloud(*local_map_frames_.at(i).cloud_data.cloud_ptr, 
                                 *transformed_cloud_ptr, 
                                 local_map_frames_.at(i).pose);
        *local_map_ptr_ += *transformed_cloud_ptr;
    }
    has_new_local_map_ = true;

    // 更新ndt匹配的目标点云
    if (local_map_frames_.size() < 10) {
        icp_ptr_->setInputTarget(local_map_ptr_);         //更新小地图,滑窗
    } else {
        CloudData::CLOUD_PTR filtered_local_map_ptr(new CloudData::CLOUD());
        local_map_filter_.setInputCloud(local_map_ptr_);
        local_map_filter_.filter(*filtered_local_map_ptr);
        icp_ptr_->setInputTarget(filtered_local_map_ptr);
    }

    // 更新全局地图
    global_map_frames_.push_back(key_frame);
    if (global_map_frames_.size() % 100 != 0) {
        return;
    } else {
        global_map_ptr_.reset(new CloudData::CLOUD());
        for (size_t i = 0; i < global_map_frames_.size(); ++i) {
            pcl::transformPointCloud(*global_map_frames_.at(i).cloud_data.cloud_ptr, 
                                    *transformed_cloud_ptr, 
                                    global_map_frames_.at(i).pose);
            *global_map_ptr_ += *transformed_cloud_ptr;
        }
        has_new_global_map_ = true;
    }
}

bool FrontEnd::GetNewLocalMap(CloudData::CLOUD_PTR& local_map_ptr) {
    if (has_new_local_map_) {
        display_filter_.setInputCloud(local_map_ptr_);
        display_filter_.filter(*local_map_ptr);
        return true;
    }
    return false;
}

bool FrontEnd::GetNewGlobalMap(CloudData::CLOUD_PTR& global_map_ptr) {
    if (has_new_global_map_) {
        display_filter_.setInputCloud(global_map_ptr_);
        display_filter_.filter(*global_map_ptr);
        return true;
    }
    return false;
}

bool FrontEnd::GetCurrentScan(CloudData::CLOUD_PTR& current_scan_ptr) {
    display_filter_.setInputCloud(result_cloud_ptr_);
    display_filter_.filter(*current_scan_ptr);
    return true;
}
}

front_end.hpp

/*
 * @Description: 前端里程计算法
 * @Author: Ren Qian
 * @Date: 2020-02-04 18:52:45
 */
#ifndef LIDAR_LOCALIZATION_FRONT_END_FRONT_END_HPP_
#define LIDAR_LOCALIZATION_FRONT_END_FRONT_END_HPP_

#include <deque>

#include <Eigen/Dense>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/icp.h>

#include "lidar_localization/sensor_data/cloud_data.hpp"


namespace lidar_localization {
class FrontEnd {
  public:
    class Frame {
      public:  
        Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
        CloudData cloud_data;
    };

  public:
    FrontEnd();

    Eigen::Matrix4f Update(const CloudData& cloud_data);
    bool SetInitPose(const Eigen::Matrix4f& init_pose);
    bool SetPredictPose(const Eigen::Matrix4f& predict_pose);

    bool GetNewLocalMap(CloudData::CLOUD_PTR& local_map_ptr);
    bool GetNewGlobalMap(CloudData::CLOUD_PTR& global_map_ptr);
    bool GetCurrentScan(CloudData::CLOUD_PTR& current_scan_ptr);
  
  private:
    void UpdateNewFrame(const Frame& new_key_frame);

//声明
  private:     
    pcl::VoxelGrid<CloudData::POINT> cloud_filter_;
    pcl::VoxelGrid<CloudData::POINT> local_map_filter_;
    pcl::VoxelGrid<CloudData::POINT> display_filter_;
    pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>::Ptr ndt_ptr_;     //ndt
    pcl::IterativeClosestPoint<CloudData::POINT, CloudData::POINT>::Ptr icp_ptr_;                          //icp

    std::deque<Frame> local_map_frames_;
    std::deque<Frame> global_map_frames_;

    bool has_new_local_map_ = false;
    bool has_new_global_map_ = false;
    CloudData::CLOUD_PTR local_map_ptr_;
    CloudData::CLOUD_PTR global_map_ptr_;
    CloudData::CLOUD_PTR result_cloud_ptr_;
    Frame current_frame_;

    Eigen::Matrix4f init_pose_ = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f predict_pose_ = Eigen::Matrix4f::Identity();
};
}

#endif