多传感器融合定位（1-3D激光里程计）6-实现手写icp

ps: 因为个人能力不足，本次代码 和 伪代码 框图 主要是参考 go on 大佬的，本人只是搬运工做记录

ps：本文 来自 go on 助教的讲解ppt

伪代码框图 ps：图来自 go on 助教的讲解

添加sophus 库

sophus库代码地址

代码实现

添加icp_registration_manual.hpp

/*
 * @Description: ICP 匹配模块
 * @Author: KaHo
 * @Date: 2020-10-17 21:46:57
 */
#ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_REGISTRATION_MANUAL_HPP_
#define LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_REGISTRATION_MANUAL_HPP_

#include <pcl/registration/icp.h>    
#include <pcl/kdtree/kdtree_flann.h>
#include "lidar_localization/models/registration/registration_interface.hpp"         //配准接口
#include "se3.hpp"                // 添加 se3

namespace lidar_localization {
class ICPRegistrationManual: public RegistrationInterface {
  public:
    ICPRegistrationManual(const YAML::Node  &node);
    ICPRegistrationManual(float max_correspond_dis,  int max_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_correspond_dis,int  max_iter);
      void calculateTrans(const CloudData::CLOUD_PTR   &input_cloud );       // 计算旋转矩阵

  private:
      CloudData::CLOUD_PTR target_cloud_;        
      pcl::KdTreeFLANN<CloudData::POINT>::Ptr  kdtree_ptr_;
      float max_correspond_distance_;     // 阈值
      int max_iterator_;                                     //最大迭代次数

      Eigen::Matrix3f  rotation_matrix_;       //旋转矩阵
      Eigen::Vector3f  translation_;                 //平移矩阵
      Eigen::Matrix4f  transformation_;       // 转换矩阵     
      
};
}

#endif

添加icp_registration_manual.cpp

/*
 * @Description: ICP 匹配模块
 * @Author: Ren Qian
 * @Date: 2020-02-08 21:46:45
 */
#include "lidar_localization/models/registration/icp_registration_manual.hpp"
#include "glog/logging.h"
#include <Eigen/Dense>

namespace lidar_localization {

ICPRegistrationManual::ICPRegistrationManual(const YAML::Node& node)
            :  kdtree_ptr_(new pcl::KdTreeFLANN<CloudData::POINT>)  {
    // 获取参数
     float max_correspond_dis  =  node["max_correspondence_distance"].as<float>();      
     int  max_iter  =  node["max_iter"].as<int>();      //最大迭代次数
     SetRegistrationParam(max_correspond_dis,  max_iter); 
}

ICPRegistrationManual :: ICPRegistrationManual(float max_correspond_dis,
                                                                                                    int max_iter)
            :  kdtree_ptr_(new pcl :: KdTreeFLANN<CloudData :: POINT> )   {
                SetRegistrationParam(max_correspond_dis,max_iter);
            }

bool  ICPRegistrationManual::SetRegistrationParam(float max_correspond_dis,
                                                                                                            int  max_iter){
    max_correspond_distance_  = max_correspond_dis;
    max_iterator_ =  max_iter;
    LOG(INFO)     <<  "ICP Manual  的匹配参数为 :   "   << std::endl
                               << "max_correspond_dis:  "       <<  max_correspond_dis  << ", " 
                               << "max_iter:  "      <<  max_iter   <<  std::endl
                               << std :: endl;
    return true;
}

bool ICPRegistrationManual::SetInputTarget(
            const CloudData::CLOUD_PTR& input_target)   {
        target_cloud_.reset(new CloudData::CLOUD);
        target_cloud_ = input_target;
        kdtree_ptr_ ->setInputCloud(input_target);
        return   true;
}

bool ICPRegistrationManual::ScanMatch(const CloudData::CLOUD_PTR   &input_source, 
                                                                                    const Eigen::Matrix4f& predict_pose, 
                                                                                    CloudData::CLOUD_PTR& result_cloud_ptr,
                                                                                    Eigen::Matrix4f& result_pose) {
    transformation_  = predict_pose;  
    rotation_matrix_ = transformation_.block<3,  3>(0,  0) ;    //取旋转矩阵
    translation_  = transformation_.block<3,  1>(0,  3);                      //取平移矩阵

    calculateTrans(input_source);       //计算变换矩阵


    pcl::transformPointCloud(*input_source,   *result_cloud_ptr,  transformation_);   // 对点云进行变换
    result_pose = transformation_;
                                                                            
    return true;
}

void ICPRegistrationManual::calculateTrans(const CloudData::CLOUD_PTR   &input_source){
    CloudData::CLOUD_PTR  transformed_cloud(new CloudData::CLOUD);
    int knn = 1;     // 进行 1nn的搜索
    int iterator_num = 0;
    while(iterator_num < max_iterator_)
    {
        pcl::transformPointCloud(*input_source,*transformed_cloud,transformation_);    // 对点云进行变换
        Eigen::Matrix<float,6,6> Hessian;
        Eigen::Matrix<float,6,1>B;
        Hessian.setZero();
        B.setZero();     // 归零

        for(size_t i =0; i < transformed_cloud->size();  ++i)
        {
            auto ori_point = input_source->at(i);
            if(!pcl::isFinite(ori_point))
                continue;
            auto transformed_point = transformed_cloud->at(i);
            std::vector<float> distances;
            std::vector<int>indexs;     
            kdtree_ptr_->nearestKSearch(transformed_point,knn,indexs,distances);      // knn搜索
            if(distances[0] > max_correspond_distance_)
            {
                continue;
            }
            Eigen::Vector3f closet_point  = Eigen::Vector3f(target_cloud_->at(indexs[0]).x,   target_cloud_->at(indexs[0]).y ,
                                                                target_cloud_->at(indexs[0]).z );
            // 计算 原始点 与  最邻近点 的 距离
            Eigen::Vector3f err_dis = 
                Eigen::Vector3f(transformed_point.x,transformed_point.y,transformed_point.z) - closet_point;

            Eigen::Matrix<float,3,6> Jacobian(Eigen::Matrix<float,3,6>::Zero());
            Jacobian.leftCols<3>() = Eigen::Matrix3f::Identity();
            Jacobian.rightCols<3>() = 
                    -rotation_matrix_* Sophus::SO3f::hat(Eigen::Vector3f(ori_point.x,ori_point.y,ori_point.z)) ;
                    Hessian  +=  Jacobian.transpose()* Jacobian; 
                    B += -Jacobian.transpose()*err_dis;
        }
        iterator_num++;
        if(Hessian.determinant() == 0)
        {
                continue;
        }
        Eigen::Matrix<float,6,1> delta_x =  Hessian.inverse()*B;

        translation_ += delta_x.head<3>();
        auto  delta_rotation = Sophus::SO3f::exp(delta_x.tail<3>());
        rotation_matrix_ *= delta_rotation.matrix();

        transformation_.block<3,3>(0,0) = rotation_matrix_;
        transformation_.block<3,1>(0,3) = translation_;

    }

}
}

修改 front_end.cpp

//选择配准方式
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   if (registration_method == "ICP_MANUAL") {
        registration_ptr = std::make_shared<ICPRegistrationManual>(config_node[registration_method]);
    }
    else {
        LOG(ERROR) << "没找到与 " << registration_method << " 相对应的点云匹配方式!";
        return false;
    }

    return true;
}

修改 config.yaml

data_path: ./   # 数据存放路径

# 匹配
registration_method:  ICP_MANUAL   # 选择点云匹配方法，目前支持：NDT  ICP  ICP_MANUAL

# 局部地图
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

ICP_MANUAL:
    max_correspondence_distance :  1.0
    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]

icp manual 效果图

evo 精度评价结果

PCL-NDT PCL-ICP ICP 对比结果

ps： 对比图 来自 go on 助教的讲解