记录常见的配准方法（二）

#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>//icp配准
#include <pcl/visualization/pcl_visualizer.h>//可视化
#include <time.h>//时间using namespace std;typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
void visualize_pcd(PointCloud::Ptr pcd_tgt, PointCloud::Ptr pcd_final)
{//创建初始化目标pcl::visualization::PCLVisualizer viewer("registration Viewer");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.setBackgroundColor(255, 255, 255);viewer.addPointCloud(pcd_tgt, tgt_h, "tgt cloud");viewer.addPointCloud(pcd_final, final_h, "final cloud");while (!viewer.wasStopped()){viewer.spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}
}
int main()
{//--------------------加载待配准的两个点云文件// 1PointCloud::Ptr cloud_src_o(new PointCloud);pcl::PCDReader r1;r1.read("34.pcd", *cloud_src_o);// 2PointCloud::Ptr cloud_tgt_o(new PointCloud);//目标点云pcl::PCDReader r2;r2.read("12.pcd", *cloud_tgt_o);clock_t start = clock();//--------------------去除nan点--------------------//// 1 cerr << "去除  cloud_src_o nan点" << endl;vector<int> indices_src; //保存去除的点的索引pcl::removeNaNFromPointCloud(*cloud_src_o, *cloud_src_o, indices_src);// 2cout << "去除  cloud_tgt_o nan点" << endl;vector<int> indices_tgt;pcl::removeNaNFromPointCloud(*cloud_tgt_o, *cloud_tgt_o, indices_tgt);//--------------------下采样精简--------------------//// 1pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_1; //创建体素滤波对象voxel_grid_1.setLeafSize(0.01, 0.01, 0.01); //设置体素边长voxel_grid_1.setInputCloud(cloud_src_o);PointCloud::Ptr cloud_src(new PointCloud);voxel_grid_1.filter(*cloud_src);  //精简后的输入点云 放入cloud_src 中cerr << "初始点云数：" << cloud_src_o->size() << "-----精简之后点云数： " << cloud_src->size() << endl;// 2pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_2;voxel_grid_2.setLeafSize(0.01, 0.01, 0.01);voxel_grid_2.setInputCloud(cloud_tgt_o);PointCloud::Ptr cloud_tgt(new PointCloud);voxel_grid_2.filter(*cloud_tgt);//       cloud_tgtcerr << "初始点云数：" << cloud_tgt_o->size() << "-----精简之后点云数： " << cloud_tgt->size() << endl;//--------------------计算表面法线--------------------//// 1pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_src; //创建法线对象ne_src.setInputCloud(cloud_src);pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_src(new pcl::search::KdTree< pcl::PointXYZ>()); //创建搜索树对象ne_src.setSearchMethod(tree_src); //设置搜索方法pcl::PointCloud<pcl::Normal>::Ptr cloud_src_normals(new pcl::PointCloud< pcl::Normal>); //cloud_src_normals 存储法线向量ne_src.setRadiusSearch(0.02); //设置搜索半径ne_src.compute(*cloud_src_normals);  //计算表面法线向量存入 cloud_src_normals 中// 2pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_tgt;ne_tgt.setInputCloud(cloud_tgt);pcl::search::KdTree< pcl::PointXYZ>::Ptr tree_tgt(new pcl::search::KdTree< pcl::PointXYZ>());ne_tgt.setSearchMethod(tree_tgt);pcl::PointCloud<pcl::Normal>::Ptr cloud_tgt_normals(new pcl::PointCloud< pcl::Normal>);ne_tgt.setRadiusSearch(0.02);ne_tgt.compute(*cloud_tgt_normals);//--------------------计算FPFH--------------------//// 1pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_src; //创建fpfh对象fpfh_src.setInputCloud(cloud_src); //传入点云fpfh_src.setInputNormals(cloud_src_normals); //传入法线向量pcl::search::KdTree<PointT>::Ptr tree_src_fpfh(new pcl::search::KdTree<PointT>);fpfh_src.setSearchMethod(tree_src_fpfh);pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_src(new pcl::PointCloud<pcl::FPFHSignature33>());fpfh_src.setRadiusSearch(0.05);fpfh_src.compute(*fpfhs_src);  //计算结果保存fpfhs_src中std::cout << "compute *cloud_src fpfh" << endl;// 2pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_tgt;fpfh_tgt.setInputCloud(cloud_tgt);fpfh_tgt.setInputNormals(cloud_tgt_normals);pcl::search::KdTree<PointT>::Ptr tree_tgt_fpfh(new pcl::search::KdTree<PointT>);fpfh_tgt.setSearchMethod(tree_tgt_fpfh);pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs_tgt(new pcl::PointCloud<pcl::FPFHSignature33>());fpfh_tgt.setRadiusSearch(0.05);fpfh_tgt.compute(*fpfhs_tgt);std::cout << "compute *cloud_tgt fpfh" << endl;//--------------------SAC-IA采样一致性粗配准--------------------//pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> scia;scia.setInputSource(cloud_src);scia.setInputTarget(cloud_tgt);scia.setSourceFeatures(fpfhs_src);scia.setTargetFeatures(fpfhs_tgt);//scia.setMinSampleDistance(1);//scia.setNumberOfSamples(2);//scia.setCorrespondenceRandomness(20);PointCloud::Ptr sac_result(new PointCloud);scia.align(*sac_result); //执行配准存储变换后的点云到sac_result中Eigen::Matrix4f sac_trans;sac_trans = scia.getFinalTransformation();clock_t sac_time = clock();//--------------------ICP精配准--------------------//PointCloud::Ptr icp_result(new PointCloud);pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;icp.setInputSource(cloud_src);icp.setInputTarget(cloud_tgt_o);//Set the max correspondence distance to 4cm (e.g., correspondences with higher distances will be ignored)icp.setMaxCorrespondenceDistance(0.04);// 最大迭代次数icp.setMaximumIterations(100);// 两次变化矩阵之间的差值icp.setTransformationEpsilon(1e-10);// 均方误差icp.setEuclideanFitnessEpsilon(0.01);icp.align(*icp_result, sac_trans);Eigen::Matrix4f icp_trans;icp_trans = icp.getFinalTransformation();pcl::transformPointCloud(*cloud_src_o, *icp_result, icp_trans);pcl::PCDWriter w;w.write<pcl::PointXYZ>("final.pcd", *icp_result, false);visualize_pcd(cloud_tgt_o, icp_result);return 0;
}

记录常见的配准方法（二）相关推荐

Selenium应用代码（常见封装的方法二）
滚动窗口: //将滚动条滚到适合的位置 , 方法一public static void setScroll(WebDriver driver,int height){ try {// String s ...
生活记录-常见水果清洗方法
摘要 1.面粉去除脏物 2.盐杀菌 3.流动水冲洗 1. 葡萄(葡萄食品) 葡萄表面有一层白霜,,学名叫「果粉」.其实不用怕,这层白霜是葡萄自身分泌的糖醇类物质,这层看起来脏脏的白色存在,不仅对人体无 ...
两种常见的点云配准方法ICPNDT
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者:于凡 https://zhuanlan.zhihu.com/p/96908474 本文转载自知乎 ...
横河川仪压力变送器故障代码_压力变送器常见故障解决方法
压力变送器常见故障解决方法在压力变送器的使用过程中难免出现故障,下面我们针对几种故障的解决方法进行介绍. 一.压力指示不正确 1: 压力变送器电源是否正常如果小于 12VDC ,则应检查回路中是否 ...
常见的加密方法与破解思路
电脑在我们身边逐步扮演着越来越重要的角色,越来越多的机密数据存放在我们的电脑中.随着人们安全意识的不断提高,我们也开始大量使用密码来保护自己的机密数据.然而,经常还能听到"XX网站被黑客攻击 ...
ARCGIS/MAPGIS/MAPINFO/CAD配准方法汇总
常见一些软件配准方法介绍 1.ARCGIS 软件配准 1.1. 栅格图像配准 1. 打开 ArcMap ,增加 Georeferencing 工具条. 2. 把需要进行纠正的影像增加到 ArcMap ...
DHCP服务搭建及常见错误解决方法
部署DHCP服务 · 相关介绍 · 相关术语 · 分配地址过程 · 实验设置 · 部署DHCP 1. 安装dhcp服务 2. 编辑dhcp主配置文件 ※主配置文件常见参数 3. 重启服务并加入开机启动 ...
链表问题13——删除无序单链表中值重复出现的节点(方法二)
题目同上一篇题目,本篇介绍方法二类似于选择排序的过程,时间复杂度为O(),额外空间复杂度为O(1), 思路按顺序遍历节点先是记录当前节点,然后去查看后面是否还有重复,有的话删掉,直到遍历完然后 ...
链表问题12——将单链表的每K个节点之间逆序(方法二)
题目题目同上一篇方法一中的题目. 思路方法二:在原链表中调整用变量记录每一组开始的第一个节点,最后一个节点,然后逆序即可. 源码 public class Node{public int val ...
针对地图可压缩性的点云配准方法评估（IROS 2021）
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨Penny@知乎来源丨https://zhuanlan.zhihu.com/p/2009241 ...

记录常见的配准方法（二）

记录常见的配准方法（二）相关推荐

最新文章

热门文章