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

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); //最大欧式距离差值

调用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