一、点云

我们在做 3D 视觉的时候，处理的主要是点云，点云就是一些点的集合。相对于图像，点云有其不可替代的优势——深度，也就是说三维点云直接提供了三维空间的数据，而图像则需要通过透视几何来反推三维数据。
而实际上点云是某个坐标系下的点的数据集。点包含了丰富的信息，包括三维坐标 X，Y，Z、颜色、分类值、强度值、时间等等。点云在组成特点上分为两种，一种是有序点云，一种是无序点云。

• 有序点云：一般由深度图还原的点云，有序点云按照图方阵一行一行的，从左上角到右下角排列，当然其中有一些无效点因为。有序点云按顺序排列，可以很容易的找到它的相邻点信息。有序点云在某些处理的时候还是很便利的，但是很多情况下是无法获取有序点云的。
• 无序点云：无序点云就是其中的点的集合，点排列之间没有任何顺序，点的顺序交换后没有任何影响。是比较普遍的点云形式，有序点云也可看做无序点云来处理。

而目前为止我常用的点云格式为PCD，PCD 格式具有文件头，用于描绘点云的整体信息：定义数字的可读头、尺寸、点云的维数和数据类型；一种数据段，可以是 ASCII 码或二进制码。数据本体部分由点的笛卡尔坐标构成，文本模式下以空格做分隔符。其中比较重要的是VIEWPOINT，这个字段存储了点云坐标系和世界参考坐标系之间的四元数关系。

二、icp算法

学习链接：https://blog.csdn.net/qq_41685265/article/details/107140349
icp算法本质不难，其实就是对源点云图和当前点云图进行迭代匹配，求出旋转矩阵（R）和平移向量（t）。
对于R和t而言，有许多办法，博客介绍了一种SVD算法：
首先定义两种点云的质心，,并作出如下处理：


至此，我们知道了icp的原理和求R和t的方法。

三、gicp算法

在诸多大佬24赛季的开源中，提到使用icp算法进行重定位，但都存在一个问题是，使用该算法占用资源较大，在cpu调度下可能只有10Hz左右，诸如中南大学等学校使用icp仅仅作为初始定位的工具，后续定位纯依靠里程计来维持，这样虽然能避免icp占用资源较大的问题，但是如若场上遇到突发情况，比如撞车等，把里程计撞飘了之后，导航就寄了，还是不可逆的，而深北佬24赛季所用的slamtoolbox虽然占用资源少，但是在靠近墙体时定位还是会飘，所以急需一种，定位准确且轻量化的算法，然后我就发现了gicp算法。
学习链接：https://www.guyuehome.com/detail?id=1825491470694281217
具体推导我也看不太懂，就从代码出发看看究竟是个什么事。

int main()
{
​
    pcl::console::TicToc time;
    // -----------------加载点云数据---------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr target(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("30m1A.pcd", *target);
    pcl::PointCloud<pcl::PointXYZ>::Ptr source(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile<pcl::PointXYZ>("30m2A.pcd", *source);
    cout << "读取源点云个数：" << source->points.size() << endl;
    cout << "读取目标点云个数：" << target->points.size() << endl;
    time.tic();
    //-----------------初始化GICP对象-------------------------
    pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> gicp; 
    //-----------------KD树加速搜索---------------------------
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1(new pcl::search::KdTree<pcl::PointXYZ>);
    tree1->setInputCloud(source);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2(new pcl::search::KdTree<pcl::PointXYZ>);
    tree2->setInputCloud(target);
    gicp.setSearchMethodSource(tree1);
    gicp.setSearchMethodTarget(tree2);
    //-----------------设置GICP相关参数-----------------------
    gicp.setInputSource(source);  //源点云
    gicp.setInputTarget(target);  //目标点云
    gicp.setMaxCorrespondenceDistance(100); //设置对应点对之间的最大距离
    gicp.setTransformationEpsilon(1e-10);   //为终止条件设置最小转换差异
     /* gicp.setSourceCovariances(source_covariances);
    gicp.setTargetCovariances(target_covariances);*/
    gicp.setEuclideanFitnessEpsilon(0.001);  //设置收敛条件是均方误差和小于阈值， 停止迭代
    gicp.setMaximumIterations(35); //最大迭代次数  
    //gicp.setUseReciprocalCorrespondences(true);//使用相互对应关系
    // 计算需要的刚体变换以便将输入的源点云匹配到目标点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    gicp.align(*icp_cloud);
    //---------------输出必要信息到显示--------------------
    cout << "Applied " << 35 << " GICP iterations in " << time.toc()/1000 << " s" << endl;
    cout << "\nGICP has converged, score is " << gicp.getFitnessScore() << endl;
    cout << "变换矩阵：\n" << gicp.getFinalTransformation() << endl;
    // 使用变换矩阵对为输入点云进行变换
    pcl::transformPointCloud(*source, *icp_cloud, gicp.getFinalTransformation());
    //pcl::io::savePCDFileASCII (".pcd", *icp_cloud);
   // -----------------点云可视化--------------------------
    boost::shared_ptr<pcl::visualization::PCLVisualizer>
        viewer_final(new pcl::visualization::PCLVisualizer("配准结果"));
    viewer_final->setBackgroundColor(0, 0, 0);  //设置背景颜色为黑色
    // 对目标点云着色可视化 (red).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
        target_color(target, 255, 0, 0);
    viewer_final->addPointCloud<pcl::PointXYZ>(target, target_color, "target cloud");
    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
        1, "target cloud");
    // 对源点云着色可视化 (green).
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
        input_color(source, 0, 255, 0);
    viewer_final->addPointCloud<pcl::PointXYZ>(source, input_color, "input cloud");
    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
        1, "input cloud");
    // 对转换后的源点云着色 (blue)可视化.
    pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
        output_color(icp_cloud, 0, 0, 255);
    viewer_final->addPointCloud<pcl::PointXYZ>(icp_cloud, output_color, "output cloud");
    viewer_final->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
        1, "output cloud");
​
    while (!viewer_final->wasStopped())
    {
        viewer_final->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
​
    return (0);
}

来源链接：https://www.cnblogs.com/wentZh2004/p/18679528