PCL系列——怎样逐渐地配准一对点云
欢迎訪问 博客新址
PCL系列
- PCL系列——读入PCD格式文件操作
- PCL系列——将点云数据写入PCD格式文件
- PCL系列——拼接两个点云
- PCL系列——从深度图像(RangeImage)中提取NARF关键点
- PCL系列——怎样可视化深度图像
- PCL系列——怎样使用迭代近期点法(ICP)配准
- PCL系列——怎样逐渐地配准一对点云
- PCL系列——三维重构之泊松重构
- PCL系列——三维重构之贪婪三角投影算法
- PCL系列——三维重构之移动立方体算法
说明
通过本教程,我们将会学会:
- 怎样配准多个点云图。
- 配准的方法是:点云图两两配准,计算它们的变换矩阵。然后计算总的变换矩阵。
- 两个点云配准使用的是非线性ICP算法,它是ICP的算法的变体,使用Levenberg-Marquardt最优化。
操作
- 在VS2010 中新建一个文件
pairwise_incremental_registration.cpp
,然后将以下的代码拷贝到文件里。 - 參照之前的文章。配置项目的属性。设置包括文件夹和库文件夹和附加依赖项。
#include <boost/make_shared.hpp> //共享指针
//点/点云
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
//pcd文件输入/输出
#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/icp.h> //ICP方法
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
//可视化
#include <pcl/visualization/pcl_visualizer.h>
//命名空间
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;
//全局变量
//可视化对象
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;
}
};
//在窗体的左视区。简单的显示源点云和目标点云
void showCloudsLeft(const PointCloud::Ptr cloud_target, const PointCloud::Ptr cloud_source)
{
p->removePointCloud ("vp1_target"); //依据给定的ID。从屏幕中去除一个点云。參数是ID
p->removePointCloud ("vp1_source"); //
PointCloudColorHandlerCustom<PointT> tgt_h (cloud_target, 0, 255, 0); //目标点云绿色
PointCloudColorHandlerCustom<PointT> src_h (cloud_source, 255, 0, 0); //源点云红色
p->addPointCloud (cloud_target, tgt_h, "vp1_target", vp_1); //载入点云
p->addPointCloud (cloud_source, src_h, "vp1_source", vp_1);
PCL_INFO ("Press q to begin the registration.\n"); //在命令行中显示提示信息
p-> spin();
}
//在窗体的右视区,简单的显示源点云和目标点云
void showCloudsRight(const PointCloudWithNormals::Ptr cloud_target, const PointCloudWithNormals::Ptr cloud_source)
{
p->removePointCloud ("source"); //依据给定的ID。从屏幕中去除一个点云。
參数是ID
p->removePointCloud ("target");
PointCloudColorHandlerGenericField<PointNormalT> tgt_color_handler (cloud_target, "curvature"); //目标点云彩色句柄
if (!tgt_color_handler.isCapable ())
PCL_WARN ("Cannot create curvature color handler!");
PointCloudColorHandlerGenericField<PointNormalT> src_color_handler (cloud_source, "curvature"); //源点云彩色句柄
if (!src_color_handler.isCapable ())
PCL_WARN ("Cannot create curvature color handler!");
p->addPointCloud (cloud_target, tgt_color_handler, "target", vp_2); //载入点云
p->addPointCloud (cloud_source, src_color_handler, "source", vp_2);
p->spinOnce();
}
// 读取一系列的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, "target", vp_2); //加入点云数据。下同
p->addPointCloud (cloud_src, cloud_src_h, "source", vp_2);
PCL_INFO ("Press q to continue the registration.\n");
p->spin ();
p->removePointCloud ("source");
p->removePointCloud ("target");
//add the source to the transformed target
*output += *cloud_src; // 拼接点云图(的点)点数数目是两个点云的点数和
final_transform = targetToSource; //终于的变换。目标点云到源点云的变换矩阵
}
//**************** 入口函数 ************************
//主函数
int main (int argc, char** argv)
{
//读取数据
std::vector<PCD, Eigen::aligned_allocator<PCD> > data; //模型
loadData (argc, argv, data); //读取pcd文件数据,定义见上面
//检查用户数据
if (data.empty ())
{
PCL_ERROR ("Syntax is: %s <source.pcd> <target.pcd> [*]", argv[0]); //语法
PCL_ERROR ("[*] - multiple files can be added. The registration results of (i, i+1) will be registered against (i+2), etc"); //能够使用多个文件
return (-1);
}
PCL_INFO ("Loaded %d datasets.", (int)data.size ()); //显示读取了多少个点云文件
//创建一个 PCLVisualizer 对象
p = new pcl::visualization::PCLVisualizer (argc, argv, "Pairwise Incremental Registration example"); //p是全局变量
p->createViewPort (0.0, 0, 0.5, 1.0, vp_1); //创建左视区
p->createViewPort (0.5, 0, 1.0, 1.0, vp_2); //创建右视区
//创建点云指针和变换矩阵
PointCloud::Ptr result (new PointCloud), source, target; //创建3个点云指针,分别用于结果。源点云和目标点云
//全局变换矩阵,单位矩阵。成对变换
//逗号表达式,先创建一个单位矩阵,然后将成对变换 赋给 全局变换
Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;
//遍历全部的点云文件
for (size_t i = 1; i < data.size (); ++i)
{
source = data[i-1].cloud; //源点云
target = data[i].cloud; //目标点云
showCloudsLeft(source, target); //在左视区。简单的显示源点云和目标点云
PointCloud::Ptr temp (new PointCloud); //创建暂时点云指针
//显示正在配准的点云文件名称和各自的点数
PCL_INFO ("Aligning %s (%d points) with %s (%d points).\n", data[i-1].f_name.c_str (), source->points.size (), data[i].f_name.c_str (), target->points.size ());
//********************************************************
//配准2个点云。函数定义见上面
pairAlign (source, target, temp, pairTransform, true);
//将当前的一对点云数据,变换到全局变换中。
pcl::transformPointCloud (*temp, *result, GlobalTransform);
//更新全局变换
GlobalTransform = GlobalTransform * pairTransform;
//********************************************************
// 保存成对的配准结果。变换到第一个点云帧
std::stringstream ss; //这两句是生成文件名称
ss << i << ".pcd";
pcl::io::savePCDFile (ss.str (), *result, true); //保存成对的配准结果
}
}
- 又一次生成项目。
- 到改项目的Debug文件夹下。按住Shift。同一时候点击鼠标右键,在当前窗体打开CMD窗体。
- 在命令行中输入
pairwise_incremental_registration.exe capture0001.pcd capture0002.pcd capture0003.pcd capture0004.pcd capture0005.pcd
,运行程序。得到例如以下图所看到的的结果。
參考
PCL系列——怎样逐渐地配准一对点云的更多相关文章
- 实时音视频互动系列(上):又拍云UTUN网络详解
如何定义实时音视频互动, 延迟 400ms 内才能无异步感 实时音视频互动如果存在1秒左右的延时会给交流者带来异步感,必须将视频播放延迟限制在400ms以内,才能给用户较好的交互体验. 当延迟控制在4 ...
- pcl学习笔记(二):点云类型
不同的点云类型 前面所说的,pcl::PointCloud包含一个域,它作为点的容器,这个域是PointT类型的,这个域是PointT类型的是pcl::PointCloud类的模板参数,定义了点云的存 ...
- PCL学习(二)三维模型转点云 obj转pcd----PCL实现
#include <pcl/io/io.h> #include <pcl/io/pcd_io.h> #include <pcl/io/obj_io.h> #incl ...
- 带着canvas去流浪系列之九 粒子动画【华为云技术分享】
版权声明:本文为博主原创文章,遵循CC 4.0 BY-SA版权协议,转载请附上原文出处链接和本声明. 本文链接:https://blog.csdn.net/devcloud/article/detai ...
- CODING DevOps 系列第三课:云计算、云原生模式下 DevOps 的建设
本文首先会和大家分享当前整个应用生命周期的演变历程,然后讲解云计算模式下 DevOps 建设包含的过程.流程规范和标准,最后讲解云原生时代到来会带来哪些改变,以及标准化的建设会有哪些改变和突破. 应用 ...
- 踩坑系列《十二》解决连接云服务器的redis失败
在本地连接服务器redis的时候,发现连接失败,这是因为服务器上的redis开启保护模式运行,该模式下是无法进行远程连接的.只需要修改redis目录下的redis.conf文件,找到 protecte ...
- 踩坑系列《十一》完美解决阿里云vod视频点播无法播放音频和视频点播控制台里的媒资库里面的视频无法播放
刚开始项目部署的时候,音频还是正常播放,后面直接报了 获取m3u8文件失败(manifestLoadError) 的错误,原因是 我的域名 xxx.com 这个域名没有解析到点播提供的CNAME上,所 ...
- PCL点云配准(1)
在逆向工程,计算机视觉,文物数字化等领域中,由于点云的不完整,旋转错位,平移错位等,使得要得到的完整的点云就需要对局部点云进行配准,为了得到被测物体的完整数据模型,需要确定一个合适的坐标系,将从各个视 ...
- PCL特征点与配准(1)
关于输入一个具体的物体的点云,从场景中找出与该物体点云相匹配的,这种方法可以用来抓取指定的物体等等,具体的代码的解释如下,需要用到的一些基础的知识,在之前的博客中都有提及,其中用到的一些方法可以翻阅前 ...
随机推荐
- 汇编代码中db,dw,dd的区别
db定义字节类型变量,一个字节数据占1个字节单元,读完一个,偏移量加1 dw定义字类型变量,一个字数据占2个字节单元,读完一个,偏移量加2 dd定义双字类型变量,一个双字数据占4个字节单元,读完一个, ...
- 在MacBook下安装http链接的性能测试工具httping
一.如果没有brew,先安装brew. 安装命令如下:curl -LsSf http://github.com/mxcl/homebrew/tarball/master | sudo tar xvz ...
- [COGS 2066]七十与十七
http://218.28.19.228/cogs/problem/problem.php?pid=2066 [题目描述] 七十君最近爱上了排序算法,于是Ta让十七君给Ta讲冒泡排序. 十七君给七十君 ...
- 【转载】CMarkup函数说明
1.初始化Load 导入一个XML文件到CMarkup的对象中,并对它进行解析.类似C#的Load.SetDoc 从字符串中导入XML数据,并对它解析.类似C#的LoadXml. 2.输出Sa ...
- Elasticsearch中document的基础知识
写在前面的话:读书破万卷,编码如有神-------------------------------------------------------------------- 参考内容: <Ela ...
- [置顶] Spring的自动装配
采用构造函数注入,以及setter方法注入都需要写大量的XML配置文件,这时可以采用另一种方式,就是自动装,由Spring来给我们自动装配我们的Bean. Spring提供了四种自动装配类型 1:By ...
- C++STL源代码学习(之slist篇)
///stl_slist.h ///list为双向循环链表,slist为单向链表.某些操作效率更高 ///slist是SGI额外提供的单向链表,不属于C++标准 struct _Slist_node_ ...
- 图片未完成加载显示loading
<html> <title>js图片未加载完显示loading效果</title> <body> <style> img{float:lef ...
- Introducing .NET Core
At connect(), we announced that .NET Core will be entirely released as open source software. I also ...
- mybatis配置文件,注意标签配置顺序。否则报错The content of element type "configuration" must match "(properties?,settings?,...怎么解决
感谢原作者http://www.cnblogs.com/zhoumingming/p/5417014.html 注意每个标签必须按照顺序写,不然就会提示错误 顺序是 <?xml version= ...