《多传感器融合定位》 3D激光里程计（二）

续： 《多传感器融合定位》 3D激光里程计（一）.

三、 自己实现一个激光匹配的方法,可以是 ICP, NDT 或者是 LOAM,新建一个接口类的实例,同样要求可在配置文件中对各方法进行切换。最后和作业 2 中所用的开源的 ICP,NDT 进行精度比较。

1.实现高斯牛顿的迭代方法实现ICP的求解

参考链接： https://blog.csdn.net/weixin_41281151/article/details/109439049.

(1)基本思路

构建最小二乘问题：

输入：
目标点云（构建kdtree，方便待匹配点云寻找最近点）
待匹配点云
初始位姿R和t
输出：
精确的R和t
关键：
在非线性优化中，迭代问题的最重要在于雅克比矩阵的求解，显然这个问题的模型是典型的李代数扰动模型（单个误差项关于位姿导数）：

求解：
单个点：

计算所有点的H和b，求解状态增量：

不断迭代直到满足条件.

(2)代码实现

添加Sophus库：
(在学习视觉slam14讲时安装了该库)
在工程的CMakeLists.txt下添加

find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
target_link_libraries(front_end_node ${Sophus_LIBRARIES})

实现icp_registration_manual.hpp
在头文件中主要的不同是数据成员，增加了变换所需的T，R，t，以及一个kdtree的指针用来保存上一帧的点云数据（按照kdtree的格式）

#ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_REGISTRATION_MANUAL_HPP_
#define LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP_REGISTRATION_MANUAL_HPP_

#include "sophus/se3.hpp"
#include 
#include 
#include "lidar_localization/models/registration/registration_interface.hpp"

namespace lidar_localization
{

class ICPRegistrationManual : public RegistrationInterface
{
public:
    ICPRegistrationManual(const YAML::Node& node);
    ICPRegistrationManual(float max_dist,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_dist, int max_iter);
    void CalculateTrans(const CloudData::CLOUD_PTR& input_source);

private:
    float max_dist_;
    int max_iter_;
    CloudData::CLOUD_PTR input_target_;
    pcl::KdTreeFLANN<CloudData::POINT>::Ptr kdtree_ptr_;
    Eigen::Matrix<float,3,3> R_;
    Eigen::Vector3f t_;
    Eigen::Matrix4f T_;
};

}

#endif

实现icp_registration_manual.cpp
其主要是实现ICP的迭代匹配

#include "lidar_localization/models/registration/icp_registration_manual.hpp"
#include 
#include "glog/logging.h"

namespace lidar_localization
{
    ICPRegistrationManual::ICPRegistrationManual(const YAML::Node& node):kdtree_ptr_(new pcl::KdTreeFLANN<CloudData::POINT>)
    {
        float max_dist = node["max_dist"].as<float>();
        int max_iter = node["max_iter"].as<int>();
        SetRegistrationParam(max_dist,  max_iter);
    }

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

    bool ICPRegistrationManual::SetRegistrationParam(float max_dist, int max_iter) 
    {
        max_dist_=max_dist;
        max_iter_=max_iter;
        LOG(INFO) << "ICP MANUAL的匹配参数为：" << std::endl
                << "max_dist: " << max_dist << ", "
                << "max_iter: " << max_iter 
                << std::endl << std::endl;

        return true;
    }

    bool ICPRegistrationManual::SetInputTarget(const CloudData::CLOUD_PTR& input_target) 
    {
        input_target_.reset(new CloudData::CLOUD);
        input_target_=input_target;
        kdtree_ptr_->setInputCloud(input_target_);
        return true;
    }
    /*
    ScanMatch（）
    输入当前帧点云 input_source和初始的变换矩阵
    */
    bool ICPRegistrationManual::ScanMatch(const CloudData::CLOUD_PTR& input_source, 
                   const Eigen::Matrix4f& predict_pose, 
                   CloudData::CLOUD_PTR& result_cloud_ptr,
                   Eigen::Matrix4f& result_pose)
    {
        T_=predict_pose;
        R_=T_.block<3,3>(0,0);
        t_=T_.block<3,1>(0,3);
        //ICP的高斯牛顿法计算变换矩阵
        CalculateTrans(input_source);
        pcl::transformPointCloud(*input_source,*result_cloud_ptr,T_);
        result_pose=T_;
        return true;

    }
    void ICPRegistrationManual::CalculateTrans(const CloudData::CLOUD_PTR& input_source)
    {
        CloudData::CLOUD_PTR temp_cloud(new CloudData::CLOUD); 
        //最近邻搜索的最小距离
        int knn=1;//设定k值，以寻找待搜索点的k个最近临点
        //进行max_iter_次迭代
        for (int iterator_num=0;iterator_num<max_iter_;iterator_num++)
        {
            pcl::transformPointCloud(*input_source,*temp_cloud,T_);
            Eigen::Matrix<float,6,6> H;
            Eigen::Matrix<float,6,1> b;
            H.setZero();
            b.setZero();
            //对每个点云ICP匹配
            for(size_t i=0;i<input_source->size();i++)
            {
                auto source_point=input_source->at(i);
                //无穷远点排除
                if (!pcl::isFinite(source_point))
                {
                    continue;
                }
                auto  temp_point=temp_cloud->at(i);
                std::vector<float> distance;
                std::vector<int> index;
                //寻找与target点云中距离最近的点
                //在点云中寻找点searchPoint的k近邻的值，返回下标pointSearchInd和距离pointSearchSqDis
                kdtree_ptr_->nearestKSearch(temp_point,knn,index,distance);
                //若计算出的距离大于设定的最大距离，抛弃
                //我迷惑了已经：distance，index设置在循环里面？？？？
                //然后每次使用0索引？？？？？？ 真令人头大！
                if (distance[0]>max_dist_)
                {
                    continue;
                }
                //得到与source_point最近的input_target_中最近的point
                Eigen::Vector3f closet_point=Eigen::Vector3f(input_target_->at(index[0]).x,input_target_->at(index[0]).y,input_target_->at(index[0]).z);
                //计算误差
                Eigen::Vector3f error=Eigen::Vector3f(temp_point.x,temp_point.y,temp_point.z)-closet_point;
                //求解雅克比
                Eigen::Matrix<float,3,6> J=Eigen::Matrix<float,3,6>::Zero();
                J.block<3,3>(0,0)=Eigen::Matrix3f::Identity();
                J.block<3,3>(0,3)=-R_*Sophus::SO3f::hat(Eigen::Vector3f(source_point.x,source_point.y,source_point.z));
                //求解H和b
                H+=J.transpose()*J;
                b+=-J.transpose()*error;
            }
            if (H.determinant()==0)
            {
                continue;
            }
            //乔利斯基分解求解
            Eigen::Matrix<float,6,1> dx=H.ldlt().solve(b);
            //先平移 再旋转 注意顺序
            t_+=dx.head<3>();
            R_*=Sophus::SO3f::exp(dx.tail<3>()).matrix();

            T_.block<3,3>(0,0)=R_;
            T_.block<3,1>(0,3)=t_;
        }
        
        
    }
} // namespace lidar_localization

实现icp_registration_manual的接口
config.yaml

ICP:
    max_dist : 1.0
    trans_eps : 0.01
    eculi_eps : 0.01
    max_iter : 30

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;

    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;
}

front_end.hpp

#include "lidar_localization/models/registration/icp_registration_manual.hpp"

(3)运行和评估

rivz显示：

保存地图：

分段评估：


整体评估：

结论：对比三种效果，ICP_MANIAL>NDT>ICP