接上一篇讲的SAC-IA+ICP的配准方案,这一篇讲一下SAC-IA+ICP的方案。
正态分布变换算法(3D-NDT)
正态分布变换算法应用于三维点的统计模型,使用标准最优化技术来确定两个点云间的最优的匹配,因为其在配准过程中不利用对应点的特征计算和匹配,所以时间比其他方法快,可用于点较多时的配准过程。其原理可参考Martin Magnusson的The Three-Dimensional Normal-Distributions Transform— an Efficient Representation for Registration,Surface Analysis, and Loop Detection
3D-NDT算法对参数敏感,主要涉及到以下四个参数:最小转换差异(TransformationEpsilon)、More-Thuente线搜索的最大步长(StepSize)、NDT网格结构的分辨率(Resolution)、匹配迭代的最大次数(MaximumIterations)。
在使用此算法时,可以给定或者不给定初始变换矩阵。
还是直接上代码吧:
#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/point_representation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/transforms.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)
{
pcl::visualization::PCLVisualizer viewer("registration Viewer");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h (pcd_src, 0, 255, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h (pcd_tgt, 255, 0, 0);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h (pcd_final, 0, 0, 255);
viewer.addPointCloud (pcd_src, src_h, "source cloud");
viewer.addPointCloud (pcd_tgt, tgt_h, "tgt cloud");
viewer.addPointCloud (pcd_final, final_h, "final cloud");
//viewer.addCoordinateSystem(1.0);
while (!viewer.wasStopped())
{
viewer