激光SLAM放弃指南4：PI-ICP的实现

参考链接：1.李太白lx 系列文章 从零开始搭二维激光SLAM link

计算运算时间差值

// step1 进行数据类型转换
std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now();
...
std::chrono::steady_clock::time_point end_time = std::chrono::steady_clock::now();
std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(end_time - start_time);
std::cout << "\n转换数据格式用时: " << time_used.count() << " 秒。" << std::endl;

ICP的函数

#include <iostream>                 //标准输入输出头文件
#include <pcl/io/pcd_io.h>          //I/O操作头文件
#include <pcl/point_types.h>        //点类型定义头文件
#define BOOST_TYPEOF_EMULATION  //要加在#include 前
#include <pcl/registration/icp.h>   //ICP配准类相关头文件
#include <pcl/visualization/cloud_viewer.h>//点云查看窗口头文件
#include <pcl/visualization/pcl_visualizer.h>//可视化头文件

pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;  //创建ICP的实例类
icp.setInputSource(cloud_sources); // 源点云。指将要被旋转和平移的点云。
icp.setInputTarget(cloud_target);// 目标点云。从Source到Target匹配，将源点云变换到目标点云的相对位置。
icp.setMaxCorrespondenceDistance(100);   //设置对应点对之间的最大距离（此值对配准结果影响较大）如果两个点云距离大于此值，则被忽略（PCL默认距离单位是m），一般根据两个点云之间的距离估计，这个值对点云匹配的结果影响较大。
icp.setTransformationEpsilon(1e-10); //  设置两次变化矩阵之间的差值（一般设置为1e-10即可）
icp.setEuclideanFitnessEpsilon(0.001);   //设置收敛条件是均方误差和小于阈值， 停止迭代；
nline void setEuclideanFitnessEpsilon (doubleepsilon)      前后两次迭代方差的差值，可以用来终止迭代次数。
icp.setMaximumIterations(100);     //  最大迭代次数，icp是一个迭代的方法，最多迭代这些次
icc.align(final)；  // final 为配准后点云。
inline Matrix4 getFinalTransformation ()    获取最终的转换矩阵。

如果由于设置相关参数过于苛刻，而造成没有匹配成功法， icp.hasConverged() 会提示匹配失败，并且 icp.getFitnessScore() 为0，相关的转换矩阵为单位矩阵。

 if (icp_.hasConverged() == false)
    {
     
        std::cout << "not Converged" << std::endl;
    }

运行的时候报错，暂时未解决

gsl: lu.c:266: ERROR: matrix is singular
Default GSL error handler invoked.