PCL1.8.0+VS2013编写,利用ICP进行粗略配准,利用NDT进行精确配准,

  • 虫虫123456
    了解作者
  • matlab
    开发工具
  • 9.1MB
    文件大小
  • zip
    文件格式
  • 0
    收藏次数
  • 5 积分
    下载积分
  • 0
    下载次数
  • 2022-07-19 22:29
    上传日期
PCL1.8.0+VS2013编写,利用ICP进行粗略配准,利用NDT进行精确配准,当两个点云重叠率较大时有较好的效果,点云数据是用bun000和bun045,今后可能会上传NDT+ICP进行配的代码。
XPVIkALuSW.zip
内容介绍
#include <iostream> #include <pcl/io/pcd_io.h> #include <pcl/point_types.h> #include <pcl/point_cloud.h> #include <pcl/point_representation.h> #include <pcl/filters/voxel_grid.h> #include <pcl/filters/filter.h> #include <pcl/features/normal_3d.h> //配准 #include <pcl/registration/icp.h> //ICP方法 #include <pcl/registration/icp_nl.h> #include <pcl/registration/transforms.h> #include <pcl/registration/ndt.h> #include <pcl/filters/approximate_voxel_grid.h> #include <pcl/visualization/pcl_visualizer.h> #include <boost/thread/thread.hpp> //命名空间 using pcl::visualization::PointCloudColorHandlerGenericField; using pcl::visualization::PointCloudColorHandlerCustom; //定义类型的别名 typedef pcl::PointXYZ PointT; typedef pcl::PointCloud<PointT> PointCloud; typedef pcl::PointNormal PointNormalT; typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals; //void pairAlign(const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false) //void loadData(int argc, char **argv, std::vector<PCD, Eigen::aligned_allocator<PCD> > &models) //全局变量 //可视化对象 pcl::visualization::PCLVisualizer *p; //左视区和右视区,可视化窗口分成左右两部分 int vp_1, vp_2; //定义结构体,用于处理点云 struct PCD { PointCloud::Ptr cloud; //点云指针 std::string f_name; //文件名 //构造函数 PCD() : cloud(new PointCloud) {}; //初始化 }; // 定义新的点表达方式< x, y, z, curvature > 坐标+曲率 class MyPointRepresentation : public pcl::PointRepresentation <PointNormalT> //继承关系 { using pcl::PointRepresentation<PointNormalT>::nr_dimensions_; public: MyPointRepresentation() { //指定维数 nr_dimensions_ = 4; } //重载函数copyToFloatArray,以定义自己的特征向量 virtual void copyToFloatArray(const PointNormalT &p, float * out) const { //< x, y, z, curvature > 坐标xyz和曲率 out[0] = p.x; out[1] = p.y; out[2] = p.z; out[3] = p.curvature; } }; // 读取一系列的PCD文件(希望配准的点云文件) // 参数argc 参数的数量(来自main()) // 参数argv 参数的列表(来自main()) // 参数models 点云数据集的结果向量 void loadData(int argc, char **argv, std::vector<PCD, Eigen::aligned_allocator<PCD> > &models) { std::string extension(".pcd"); //声明并初始化string类型变量extension,表示文件后缀名 // 通过遍历文件名,读取pcd文件 for (int i = 1; i < argc; i++) //遍历所有的文件名(略过程序名) { std::string fname = std::string(argv[i]); if (fname.size() <= extension.size()) //文件名的长度是否符合要求 continue; std::transform(fname.begin(), fname.end(), fname.begin(), (int(*)(int))tolower); //将某操作(小写字母化)应用于指定范围的每个元素 //检查文件是否是pcd文件 if (fname.compare(fname.size() - extension.size(), extension.size(), extension) == 0) { // 读取点云,并保存到models PCD m; m.f_name = argv[i]; pcl::io::loadPCDFile(argv[i], *m.cloud); //读取点云数据 //去除点云中的NaN点(xyz都是NaN) std::vector<int> indices; //保存去除的点的索引 pcl::removeNaNFromPointCloud(*m.cloud, *m.cloud, indices); //去除点云中的NaN点 models.push_back(m); } } } //简单地配准一对点云数据,并返回结果 //参数cloud_src 源点云 //参数cloud_tgt 目标点云 //参数output 输出点云 //参数final_transform 成对变换矩阵 //参数downsample 是否下采样 void pairAlign(const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = false) { // //为了一致性和速度,下采样 // \note enable this for large datasets PointCloud::Ptr src(new PointCloud); //创建点云指针 PointCloud::Ptr tgt(new PointCloud); pcl::VoxelGrid<PointT> grid; //VoxelGrid 把一个给定的点云,聚集在一个局部的3D网格上,并下采样和滤波点云数据 if (downsample) //下采样 { grid.setLeafSize(0.05, 0.05, 0.05); //设置体元网格的叶子大小 //下采样 源点云 grid.setInputCloud(cloud_src); //设置输入点云 grid.filter(*src); //下采样和滤波,并存储在src中 //下采样 目标点云 grid.setInputCloud(cloud_tgt); grid.filter(*tgt); } else //不下采样 { src = cloud_src; //直接复制 tgt = cloud_tgt; } //计算曲面的法向量和曲率 PointCloudWithNormals::Ptr points_with_normals_src(new PointCloudWithNormals); //创建源点云指针(注意点的类型包含坐标和法向量) PointCloudWithNormals::Ptr points_with_normals_tgt(new PointCloudWithNormals); //创建目标点云指针(注意点的类型包含坐标和法向量) pcl::NormalEstimation<PointT, PointNormalT> norm_est; //该对象用于计算法向量 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>()); //创建kd树,用于计算法向量的搜索方法 norm_est.setSearchMethod(tree); //设置搜索方法 norm_est.setKSearch(30); //设置最近邻的数量 norm_est.setInputCloud(src); //设置输入云 norm_est.compute(*points_with_normals_src); //计算法向量,并存储在points_with_normals_src pcl::copyPointCloud(*src, *points_with_normals_src); //复制点云(坐标)到points_with_normals_src(包含坐标和法向量) norm_est.setInputCloud(tgt); //这3行计算目标点云的法向量,同上 norm_est.compute(*points_with_normals_tgt); pcl::copyPointCloud(*tgt, *points_with_normals_tgt); //创建一个 自定义点表达方式的 实例 MyPointRepresentation point_representation; //加权曲率维度,以和坐标xyz保持平衡 float alpha[4] = { 1.0, 1.0, 1.0, 1.0 }; point_representation.setRescaleValues(alpha); //设置缩放值(向量化点时使用) //创建非线性ICP对象 并设置参数 pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg; //创建非线性ICP对象(ICP变体,使用Levenberg-Marquardt最优化) reg.setTransformationEpsilon(1e-6); //设置容许的最大误差(迭代最优化) //***** 注意:根据自己数据库的大小调节该参数 reg.setMaxCorrespondenceDistance(0.1); //设置对应点之间的最大距离(0.1m),在配准过程中,忽略大于该阈值的点 reg.setPointRepresentation(boost::make_shared<const MyPointRepresentation>(point_representation)); //设置点表达 //设置源点云和目标点云 //reg.setInputSource (points_with_normals_src); //版本不符合,使用下面的语句 reg.setInputCloud(points_with_normals_src); //设置输入点云(待变换的点云) reg.setInputTarget(points_with_normals_tgt); //设置目标点云 reg.setMaximumIterations(2); //设置内部优化的迭代次数 // Run the same optimization in a loop and visualize the results Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity(), prev, targetToSource; PointCloudWithNormals::Ptr reg_result = points_with_normals_src; //用于存储结果(坐标+法向量) for (int i = 0; i < 30; ++i) //迭代 { PCL_INFO("Iteration Nr. %d.\n", i); //命令行显示迭代的次数 //保存点云,用于可视化 points_with_normals_src = reg_result; // //估计 //reg.setInputSource (points_with_normals_src); reg.setInputCloud(points_with_normals_src); //重新设置输入点云(待变换的点云),因为经过上一次迭代,已经发生变换了 reg.align(*reg_result); //对齐(配准)两个点云 Ti = reg.getFinalTransformation() * Ti; //累积(每次迭代的)变换矩阵 //如果这次变换和上次变换的误差比阈值小,通过减小最大的对应点距离的方法来进一步细化 if (fabs((reg.getLastIncrementalTransformation() - prev).sum()) < reg.getTransformationEpsilon()) reg.setMaxCorrespondenceDistance(reg.getMaxCorrespondenceDistance() - 0.001); //减小对应点之间的最大距离(上面设置过) prev = reg.getLastIncrementalTransformation(); //上一次变换的误差 //显示当前配准状态,在窗口的右视区,简单的显示源点云和目标点云 //showCloudsRight(points_with_normals_tgt, points_with_normals_src); } targetToSource = Ti.inverse(); //计算从目标点云到源点云的变换矩阵 // pcl::transformPointCloud(*cloud_tgt, *output, targetToSource); //将目标点云 变换回到 源点云帧 // p->removePointCloud("source"); //根据给定的ID,从屏幕中去除一个点云。参数是ID // p->removePointCloud("target"); // PointCloudColorHandlerCustom<PointT> cloud_tgt_h(output, 0, 255, 0); //设置点云显示颜色,下同 // PointCloudColorHandlerCustom<PointT> cloud_src_h(cloud_src, 255, 0, 0); // p->addPointCloud(output, cloud_tgt_h, "targ
评论
    相关推荐
    • 基于PCL开源库fpfh+icp算法实现点云配准
      利用PCL开源库编写代码FPFH+ICP算法实现点云高精度配准,并计算配准误差!基于PCL库版本1.9!
    • 基于pcl中ndt房子配准.zip
      文件为基于pcl中ndt算法的点云数据配准程序。且其中含有两个待配准数据进行实验。配准pcl环境后可直接使用。
    • PCL资料.rar
      PCL 点云图处理库 的资料,3D点云特征描述与提取是点云信息处理中最基础也是最关键的一部分,点云的识别。分割,重采样,配准曲面重建等处理大部分算法,都严重依赖特征描述与提取的结果。从尺度上来分,一般分为...
    • pcl1.9.1源码
      PCL(Point Cloud Library)是在吸收了前人点云相关研究基础上建立起来的大型跨平台开源C++编程库,它实现了大量点云相关的通用算法和高效数据结构,涉及到点云获取、滤波、分割、配准、检索、特征提取、识别、追踪...
    • VS2013 PCL template
      PCL模板,拷贝到VS安装目录下的模板目录,系统会自动生成一个新项目的选项,选择该项目,会生成一个新项目,新项目是已经配准好的PCL项目。 假设PCL all in one 安装在c:\pcl1.7.2\目录下,如果有不同,需要自行更改...
    • ICP with PCL
      使用PCL进行ICP点云配准,直接输入点云的txt文件,可以输出显示效果和配准的RT矩阵
    • 点云粗配准算法
      写了几个点云配准的算法,主要包括PFH、FPFH、icp、NDT、3Dsc几种粗配准算法,并计算出误差。
    • pcl-1.9.0源码
      PCL(Point Cloud Library)是在吸收了前人点云相关研究基础上建立起来的大型跨平台开源C++编程库,它实现了大量点云相关的通用算法和高效数据结构,涉及到点云获取、滤波、分割、配准、检索、特征提取、识别、追踪...
    • PCL点云学习教程
      PCL(Point Cloud Library)是在吸收了前人点云相关研究基础上建立起来的大型跨平台开源C++编程库,它实现了大量点云相关的通用算法和高效数据结构,涉及到点云获取、滤波、分割、配准、检索、特征提取、识别、追踪...
    • libiconv-1.1.tar.gz
      字符集转换程序