SAC-IA粗配准+ICP精配准

最新推荐文章于 2024-07-29 06:30:00 发布

peach_blossom

最新推荐文章于 2024-07-29 06:30:00 发布

阅读量4.5w

点赞数 69

分类专栏： pcl点云库文章标签： pcl

本文链接：https://blog.csdn.net/peach_blossom/article/details/78506184

版权

最近一直在看点云配准相关的算法，在这里记录一下我试验过的配准算法。
第一弹：SAC-IA粗配准+ICP精配准

采样一致性初始配准算法（Sample Consensus Initial Aligment , SAC-IA)
此算法依赖于点特征直方图，所以在执行此算法之前，应该先计算点云的FPFH，算法的大致思路如下：
(1) 从待配准点云P中选取n个采样点，为了尽量保证所采样的点具有不同的FPFH特征，采样点两两之间的距离应满足大于预先给定最小距离阈值d。
(2) 在目标点云Q中查找与点云P中采样点具有相似FPFH特征的一个或多个点，从这些相似点中随机选取一个点作为点云P在目标点云Q中的一一对应点。
(3) 计算对应点之间刚体变换矩阵, 然后通过求解对应点变换后的“距离误差和”函数来判断当前配准变换的性能。此处的距离误差和函数多使用Huber罚函数表示，记为这里写图片描述，其中：

式中：mi为一预先给定值，li为第i组对应点变换之后的距离差。上述配准的最终目的是在所有变换中找到一组最优的变换，使得误差函数的值最小，此时的变换即为最终的配准变换矩阵，进一步可得到配准结果。
SAC-IA得到的变换矩阵不精确，所以它只能用于粗配准，在PCL库中的registration模块可实现SAC-IA算法。
在点数量较多时，计算FPFH特征较慢，使得SAC-IA算法效率很低，此时，需要先对点云进行下采样处理，以减少点的数量，但这会造成部分特征点丢失，使得配准准确度降低。

迭代最近点算法(Iterative Cloest Point, ICP)
ICP算法基于SVD，其大致思路如下:
(1) 将初始配准后的两片点云P′(经过坐标变换后的源点云)和Q，作为精配准的初始点集；
(2) 对源点云P’中的每一点pi，在目标点云Q中寻找距离最近的对应点qi，作为该点在目标点云中的对应点，组成初始对应点对；
(3) 初始对应点集中的对应关系并不都是正确的，错误的对应关系会影响最终的配准结果，采用方向向量阈值剔除错误的对应点对；
(4) 计算旋转矩阵R和平移向量T，使最小，即对应点集之间的均方误差最小；
(5) 设定某一阈值ε=dk-dk-1和最大迭代次数Nmax, 将上一步得到的刚体变换作用于源点云P′，得到新点云P”，计算P”和Q的距离误差,，如果两次迭代的误差小于阈值ε或者当前迭代次数大于Nmax，则迭代结束，否则将初始配准的点集更新为P”和Q，继续重复上述步骤，直至满足收敛条件。
ICP算法对参数敏感，在使用前要设置一下几个参数：
1.setMaximumIterations，最大迭代次数，icp是一个迭代的方法，最多迭代这些次；
2. setEuclideanFitnessEpsilon，设置收敛条件是均方误差和小于阈值，停止迭代；
3. setTransformtionEpsilon, 设置两次变化矩阵之间的差值（一般设置为1e-10即可）；
4. setMaxCorrespondenaceDistance,设置对应点对之间的最大距离（此值对配准结果影响较大）。
在两点云相差较大的情况下，ICP算法容易陷入局部最优解，从而无法得到较好的匹配结果，故需要先给定一个初始变换矩阵。在pcl库中的registration模块可实现ICP算法。

完整代码如下：
实验中所用的数据为bunny模型

#include <pcl/registration/ia_ransac.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/search/kdtree.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <time.h>

using pcl::NormalEstimation;
using pcl::search::KdTree;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;

//点云可视化
void visualize_pcd(PointCloud::Ptr pcd_src,
   PointCloud::Ptr pcd_tgt,
   PointCloud::Ptr pcd_final)
{
   //int vp_1, vp_2;
   // Create a PCLVisualizer object
   pcl::visualization::PCLVisualizer viewer("registration Viewer");
   //viewer.createViewPort (0.0, 0, 0.5, 1.0, vp_1);
  // viewer.createViewPort (0.5, 0, 1.0, 1.0, vp_2);
   pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ&