Posicionamiento de fusión multisensor (odómetro láser 1-3D) 4- realizar la llamada pcl-icp-1

Posicionamiento de fusión multisensor (odómetro láser 1-3D) 4- realizar la llamada pcl-icp-1

Esta vez usé el código de la etiqueta 4.0 del Sr. Ren Qian y modifiqué el código de la interfaz NDT original.
Blog de referencia:
Posicionamiento del piloto automático desde cero (4): La primera prueba del odómetro frontal.
Aprendizaje del algoritmo de conducción no tripulado (5): Kilometraje láser Algoritmo de coincidencia entre cuadros

NDT frente a ICP

El rojo es la trayectoria del odómetro después del procesamiento de ICP, el amarillo es la trayectoria del GNSS (se puede considerar como GroundTruth), el blanco es el marco actual en el mapa y el color es el mapa

Odómetro NDT con procesamiento de parámetros

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

Odómetro ICP

configuración de parámetros:

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

Llame a la parte principal de modificación de PCL-ICP

El código fuente es el código para la etiqueta de auto-conducción 4.0 del maestro Ren Qian desde cero. Little-Potato-1990 / localization_in_auto_driving
usa vscode para programar bajo
ros. Depuración del proyecto ros: configurar y desarrollar el proyecto ROS bajo vscode

La parte principal del proyecto ligeramente modificada es 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_;                         

Modificar el código todo

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