点云配准方法：ICP与GICP

ICP已经成为点云配准的主流算法，因此，这两天测试了PCL库中的两个ICP函数：

pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >
pcl::IterativeClosestPoint< PointSource, PointTarget >

GICP的用法和ICP类似，照葫芦画瓢即可。为了加快配准速度，程序中对点云做了半径和体素滤波处理。

经过多次实验，可以得出如下结论：

无论是在配准速度还是精度上，ICP的配准效果好于GICP；
ICP 的配准速度受最大迭代次数影响，次数越多，耗时越长，GICP则不受最大迭代次数的影响；
在点云配准之前，是可以先进行滤波处理的，尤其是经过半径滤波和体素滤波，配准速度提升了6倍左右，精度则未受影响；
迭代的次数并不是越大越好，超过一定次数之后，配准精度提升微小；
即使设置了约束条件（ setMaximumIterations，setMaxCorrespondenceDistance，setEuclideanFitnessEpsilon ），出来的结果并不是最好的。

程序如下：

#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 <pcl/filters/filter.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/registration/gicp.h>
#include <pcl/common/time.h>
#include <pcl/filters/voxel_grid.h>typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;int main(int argc, char** argv)
{// 创建点云指针PointCloudT::Ptr cloud_in(new PointCloudT);  // 需要转换的点云PointCloudT::Ptr cloud_tr(new PointCloudT);  // 转换后的点云PointCloudT::Ptr cloud_icped(new PointCloudT);  // ICP后输出点云PointCloudT::Ptr cloud_icped1(new PointCloudT);  // ICP后输出点云PointCloudT::Ptr cloud_aim(new PointCloudT); // 向其转换的目标点云PointCloudT::Ptr cloud_in_src(new PointCloudT);PointCloudT::Ptr cloud_aim_src(new PointCloudT);PointCloudT::Ptr cloud_in_rad(new PointCloudT);PointCloudT::Ptr cloud_aim_rad(new PointCloudT);pcl::StopWatch timeer;//读取pcd文件if (pcl::io::loadPCDFile<pcl::PointXYZ>("test_pcd_seg_0.pcd", *cloud_aim_src) == -1){PCL_ERROR("Couldn't read file1 \n");return (-1);}std::cout << "Loaded " << cloud_aim_src->size() << " data points from file1" << std::endl;if (pcl::io::loadPCDFile<pcl::PointXYZ>("test_pcd_seg_1.pcd", *cloud_in_src) == -1){PCL_ERROR("Couldn't read file2 \n");return (-1);}std::cout << "Loaded " << cloud_in_src->size() << " data points from file2" << std::endl;std::vector<int> mapping_in;std::vector<int> mapping_out;pcl::removeNaNFromPointCloud(*cloud_in_src, *cloud_in_src, mapping_in);pcl::removeNaNFromPointCloud(*cloud_aim_src, *cloud_aim_src, mapping_out);// Perform the actual filteringpcl::RadiusOutlierRemoval<PointT> outrem;// build the filteroutrem.setInputCloud(cloud_in_src);outrem.setRadiusSearch(0.04);outrem.setMinNeighborsInRadius(80);// apply filteroutrem.filter(*cloud_in_rad);// build the filteroutrem.setInputCloud(cloud_aim_src);outrem.setRadiusSearch(0.04);outrem.setMinNeighborsInRadius(80);// apply filteroutrem.filter(*cloud_aim_rad);// downsample cloudspcl::VoxelGrid<PointT> vg;vg.setInputCloud(cloud_in_rad);vg.setLeafSize(0.01f, 0.01f, 0.01f);vg.filter(*cloud_in);vg.setInputCloud(cloud_aim_rad);vg.setLeafSize(0.01f, 0.01f, 0.01f);vg.filter(*cloud_aim);timeer.reset();//icp配准//pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp; //创建ICP对象，用于ICP配准//pcl::GeneralizedIterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;//icp.setMaximumIterations(500);    //设置最大迭代次数iterations=true//icp.setMaxCorrespondenceDistance(0.5);//icp.setTransformationEpsilon(1e-6);//icp.setEuclideanFitnessEpsilon(1);//icp.setInputCloud(cloud_in); //设置输入点云//icp.setInputTarget(cloud_aim); //设置目标点云（输入点云进行仿射变换，得到目标点云）//icp.align(*cloud_icped1);          //匹配后源点云pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp2; //创建ICP对象，用于ICP配准//icp2.setEuclideanFitnessEpsilon(1);icp2.setMaximumIterations(200);icp2.setInputCloud(cloud_in); //设置输入点云icp2.setInputTarget(cloud_aim); //设置目标点云（输入点云进行仿射变换，得到目标点云）icp2.align(*cloud_icped);          //匹配后源点云//icp.setMaximumIterations(1);  // 设置为1以便下次调用//std::cout << "Applied " << iterations << " ICP iteration(s)" << std::endl;if (icp2.hasConverged())//icp.hasConverged ()=1（true）输出变换矩阵的适合性评估{std::cout << "\nICP has converged, score is: " << icp2.getFitnessScore() << std::endl;std::cout << "\nICP has converged, Epsilon is: " << icp2.getEuclideanFitnessEpsilon() << std::endl;//std::cout << "\nICP transformation " << iterations << " : cloud_in -> cloud_aim" << std::endl;std::cout << "\nICP transformation is \n " << icp2.getFinalTransformation() << std::endl;//transformation_matrix = icp.getFinalTransformation().cast<double>();//print4x4Matrix(transformation_matrix);}else{PCL_ERROR("\nICP has not converged.\n");return (-1);}std::cout << "ICP run time: " << timeer.getTimeSeconds() << " s" << std::endl;//可视化pcl::visualization::PCLVisualizer viewer("ICP demo");// 创建两个观察视点int v1(0);int v2(1);viewer.createViewPort(0.0, 0.0, 0.5, 1.0, v1);viewer.createViewPort(0.5, 0.0, 1.0, 1.0, v2);// 定义显示的颜色信息float bckgr_gray_level = 0.0;  // Blackfloat txt_gray_lvl = 1.0 - bckgr_gray_level;// 原始的点云设置为白色的pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_aim_color_h(cloud_aim, (int)255 * txt_gray_lvl, (int)255 * txt_gray_lvl,(int)255 * txt_gray_lvl);viewer.addPointCloud(cloud_aim, cloud_aim_color_h, "cloud_aim_v1", v1);//设置原始的点云都是显示为白色viewer.addPointCloud(cloud_aim, cloud_aim_color_h, "cloud_aim_v2", v2);// 需要被转换的点云显示为绿色pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(cloud_in, 20, 180, 20);viewer.addPointCloud(cloud_in, cloud_in_color_h, "cloud_in_v1", v1);// ICP配准后的点云为红色pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_icped_color_h(cloud_icped, 180, 20, 20);viewer.addPointCloud(cloud_icped, cloud_icped_color_h, "cloud_icped_v2", v2);// 加入文本的描述在各自的视口界面//在指定视口viewport=v1添加字符串“white 。。。”其中"icp_info_1"是添加字符串的ID标志，（10，15）为坐标16为字符大小 后面分别是RGB值viewer.addText("White: Original point cloud\nGreen: Matrix transformed point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_1", v1);viewer.addText("White: Original point cloud\nRed: ICP aligned point cloud", 10, 15, 16, txt_gray_lvl, txt_gray_lvl, txt_gray_lvl, "icp_info_2", v2);// 设置背景颜色viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v1);viewer.setBackgroundColor(bckgr_gray_level, bckgr_gray_level, bckgr_gray_level, v2);// 设置相机的坐标和方向viewer.setCameraPosition(-3.68332, 2.94092, 5.71266, 0.289847, 0.921947, -0.256907, 0);viewer.setSize(1280, 1024);  // 可视化窗口的大小//显示while (!viewer.wasStopped()){viewer.spinOnce();}return 0;
}

点云配准方法：ICP与GICP相关推荐

GICP：基于体素泛化ICP方式的准确快速点云配准方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达标题: Voxelized GICP for Fast and Accurate 3D Point C ...
针对地图可压缩性的点云配准方法评估（IROS 2021）
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨Penny@知乎来源丨https://zhuanlan.zhihu.com/p/2009241 ...
基于“分布 —— 多分布” 的点云配准方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:iceytan | 来源:知乎 https://zhuanlan.zhihu.com/p/135 ...
KSS-ICP: 基于形状分析技术的点云配准方法
目录 1. 概述 2. 算法实现 3. 实验结果总结 Reference 三维点云配准是三维视觉领域一个经典问题,涉及三维重建,定位,SLAM等具体应用问题.传统的配准可以被分为两条技术路线,即基于 ...
视频+课件| PointDSC：基于特征匹配的点云配准方法（CVPR2021）
写在前面感谢「3D视觉从入门到精通」知识星球嘉宾白旭阳博士为我们带来的主题为PointDSC:基于特征匹配的点云配准方法(CVPR2021)视频讲解,星球成员可免费观看学习.备注:白旭阳,香港科技大 ...
点云配准NDT+ICP
点云配准NDT+ICP #include <iostream> #include <pcl/io/pcd_io.h> #include <pcl/point_types. ...
点云配准之icp的简单实现
C++实现经典的点云配准算法--ICP,根据高博视觉14讲的推导(具体过程看书,还是很好理解的),ICP的计算过程如下所示: 具体的C++代码实现如下: void pose_estimation_3d ...
点云配准方法原理（NDT、ICP）
配准是点云处理中的一个基础问题,众多学者此问题进行了广泛而深入的研究,也出现了一系列优秀成熟的算法,在三维建模.自动驾驶等领域发挥着重要的作用. 本文主要介绍粗配准NDT (Normal Distri ...
干货 | 三维点云配准：ICP 算法原理及推导
编者荐语点云配准可以分为粗配准(Coarse Registration)和精配准(Fine Registration)两步.粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准,目的主 ...

点云配准方法：ICP与GICP

点云配准方法：ICP与GICP相关推荐

最新文章

热门文章