初学PCL（四）：ICP算法代码实现

最新推荐文章于 2022-05-17 21:10:30 发布

eidolon412

最新推荐文章于 2022-05-17 21:10:30 发布

阅读量6.5k

点赞数 2

文章标签：算法计算机视觉人工智能

本文链接：https://blog.csdn.net/eidolon412/article/details/123579146

版权

本文介绍了初学者如何实现点云库（PCL）中的ICP（Iterative Closest Point）算法，详细阐述了算法步骤，包括点集匹配、刚体变换的求解方法，并提供了SVD法的应用。通过创建ICP实例类执行算法，然后验证结果，比较ICP计算的变换矩阵与实际坐标变换的吻合度。

摘要由CSDN通过智能技术生成

算法输入：两个点集，source和target

算法输出：旋转矩阵R，平移矩阵T

算法步骤：

1、采集点集src属于source(均匀、随即、法失) （不一定必须要）

2、对于src中每一个点，在target中找到对应的最近点，组成点集tgt（点到点、点到面。。。）

3、求得使src刚体变换后与上述tgt对应点平均距离最小的旋转矩阵R和平移矩阵T（svd、四元数法）

4、对src做上一步得到的旋转、平移变换得到新的变换点集src'

5、如果新的变换点集src'与目标点集tgt满足目标函数（两点集的平均距离小于给定的阈值或大于某一迭代次数），则停止迭代，否则新的变换点集src'作为新的src继续迭代，直到达到目标函数要求

SVD方法求解：《视觉slam十四讲》173页

算法原理：百度搜

代码实现：参考PCL之ICP算法实现

代码目标：实现ICP并验证结果

代码实现思路：

（1）实现ICP：

1、建立源点云、目标点云和变换的点云

2、创建ICP的实例类并执行ICP

（2）验证结果：

1、读相机坐标系下变换数据（1秒到0秒的）

2、读雷达坐标系变换到相机坐标系的变换矩阵Tr

3、计算激光雷达坐标系下1-0变换矩阵

4、对比ICP得到的变换矩阵和上述计算的激光雷达下变换矩阵

代码：icpV2

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <pcl/console/parse.h>  //pcl控制台解析
// Eigen 部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>

int main ()
{
    //****ICP求变换矩阵
    //建立源点云、目标点云和变换后的点云
    pcl::PointCloud<p