原理
octree是一种管理稀疏3D数据的树状结构,利用octree实现多个无序点云之间的空间变化检测,这些点云可能在尺寸。分辨率 密度,和点顺序等方面有所差异,通过递归的比较octree的树结构,可以鉴定出由octree产生的体素组成的区别所代表的空间变化,并且通过octree的“双缓冲”技术,可以实时的探测多个点云之间的空间组成的差异。
对无序点云在空间变化上的检测,其实是对前后两幅点云在八叉树结构下的差异检测。因此我们要首先载入一个原始点云,并生成第一个八叉树结构;然后切缓冲,载入第二个点云,生成第二个八叉树结构;最后进行比较,如果有一些叶子结点在第二个八叉树上,但是不在第一个八叉树上,那么就认为这些叶子节点内的点是空间上变化多出来的点。
Code
=====================================================
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <iostream>
#include <vector>
#include <ctime>
包含的头文件
=====================================================
//创建时间随机种子 取随机数用的
srand ((unsigned int) time (NULL));
//声明一个点云 A 指针 并分配空间
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA (new pcl::PointCloud<pcl::PointXYZ> );
//定义点云大小 点云个数128个,无序
cloudA->width =128;
cloudA->height =1;
cloudA->points.resize (cloudA->width *cloudA->height);
//循环给点云赋值 通过 随机数 点云的坐标 0-64
for (size_t i=0; i<cloudA->points.size (); ++i)
{
cloudA->points[i].x =64.0f* rand () / (RAND_MAX +1.0f);
cloudA->points[i].y =64.0f* rand () / (RAND_MAX +1.0f);
cloudA->points[i].z =64.0f* rand () / (RAND_MAX +1.0f);
}
创建一个点云 cloudA
个数 128个 x,y,z的坐标 0-64 取随机值
=====================================================
/* 设置分辨率 描述的是最低一级 八叉树 的 最小体素 的 尺寸*/
float resolution =32.0f;
/* 构建 八叉树 变化 检测 的 类 并设置 分辨率 */
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ>octree (resolution);
构建 八叉树 变化 检测 的 类 并设置 分辨率
=====================================================
/* 赋值八叉树的 点云 */
octree.setInputCloud (cloudA);//设置输入的点云
octree.addPointsFromInputCloud ();//将输入的点云添加到八叉树
将 cloudA 输入 八叉树 ,构建其数据结构 将其作为基础点云
后面可检测比其多的点云多的点。但是不能检测 第二个点云比其少的点
=====================================================
/*交换八叉树缓存,但是cloudA对应的八叉树仍在内存中*/
octree.switchBuffers ();
点云 cloudA 是 参考点云 ,用其建立的八叉树对象 描述它的空间分布
OctreePointCloudChangeDetector继承自 Octree2BufBase 类,该类允许同时在内存中保存和管理两个八叉树
它应用了内存池,重复利用已经分配的节点对象,减少内存分配和释放操作
通过访问octree.switchBuffers () 重置了八叉树对象的缓存区,但把之前的八叉树仍然留着缓存中
=====================================================
//声明一个点云 B 指针 并分配空间
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );
//定义点云大小 点云个数128个,无序
cloudB->width =128;
cloudB->height =1;
cloudB->points.resize (cloudB->width *cloudB->height);
//循环给点云赋值 通过 随机数 点云的坐标 0-64
for (size_t i=0; i<cloudB->points.size (); ++i)
{
cloudB->points[i].x =90.0f* rand () / (RAND_MAX +1.0f);
cloudB->points[i].y =90.0f* rand () / (RAND_MAX +1.0f);
cloudB->points[i].z =90.0f* rand () / (RAND_MAX +1.0f);
}
声明一个点云 B 指针 并分配空间
个数 128个 x,y,z的坐标 0-90 取随机值
范围比A 大一点 ,容易有效果
=====================================================
/*声明 B 比 A 多 出 的点云 的索引值 保存 向量*/
std::vector<int>newPointIdxVector;
/* 得到B比A 多出来的 索引值 注意 B 比 A 少的 不会得到 */
octree.getPointIndicesFromNewVoxels (newPointIdxVector);
通过 函数 getPointIndicesFromNewVoxels 得 到 B比A 多出来的 索引值
=====================================================
// 终端打印 B 比 A 多出 来的 点云
std::cout<<"Output from getPointIndicesFromNewVoxels:"<<std::endl;
for (size_t i=0; i<newPointIdxVector.size (); ++i)
std::cout<<i<<"# Index:"<<newPointIdxVector[i]
<<" Point:"<<cloudB->points[newPointIdxVector[i]].x <<" "
<<cloudB->points[newPointIdxVector[i]].y <<" "
<<cloudB->points[newPointIdxVector[i]].z <<std::endl;
终端打印 B 比 A 多出 来的 点云
=====================================================
//声明一个点云 C 指针 并分配空间
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudC (new pcl::PointCloud<pcl::PointXYZ> );
//定义点云大小 点云个数上面计算的 B 比 A 多出的 个,无序
cloudC->width =newPointIdxVector.size ();
cloudC->height =1;
cloudC->points.resize (cloudC->width *cloudC->height);
for(size_t i=0;i<newPointIdxVector.size ();++i)
{
cloudC->points[i]=cloudB->points[newPointIdxVector[i]];
}
新建一个点云C 来保存 B比A 多出来的点
=====================================================
//可视化模块
pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewwe"));//创建指针viewer
viewer->initCameraParameters();//初始化相机参数
添加可视模块 来 把 点云显示出来
=====================================================
int v1(0);//第一个窗口的参数
viewer->createViewPort(0.0,0.0,0.33,1,v1); //设置第一个窗口的大小,位于屏幕左侧
viewer->setBackgroundColor(0,0,0,v1);//background of first port
viewer->addText("cloudA",10,10,"cloudA",v1);//好像是一个便签
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color1(cloudA,255,0,0);//设置第一个点云的颜色
viewer->addPointCloud<pcl::PointXYZ>(cloudA,single_color1,"cloudA",v1);//显示第一个点云
把 点云A 显示为 红色
=====================================================
int v2(0);//第一个窗口的参数
viewer->createViewPort(0.33,0,0.66,1,v2);//设置第二个窗口的大小,位于屏幕右侧
viewer->setBackgroundColor(0,0,0,v2);//background of second port
viewer->addText("cloudB",10,9,"cloudB",v2);//输出一行文字
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color2(cloudB,0,255,0);//设置第二个点云的颜色
viewer->addPointCloud<pcl::PointXYZ>(cloudB,single_color2,"simple_cloud2",v2);//显示第二个点云
第二个窗口 点云 B 显示 为绿色
=====================================================
//第三个点云窗口
int v3(0);//第三个窗口的参数
viewer->createViewPort(0.66,0,1,1,v3);//窗口大小
viewer->setBackgroundColor(0,0,0,v3);//背景颜色
viewer->addText("cloud_result",10,8,"cloud_result",v3);//好像是一个便签
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color3(cloudC,255,255,255);//点云颜色
viewer->addPointCloud<pcl::PointXYZ>(cloudA,single_color1,"cloud1",v3);//显示点云
viewer->addPointCloud<pcl::PointXYZ>(cloudB,single_color2,"cloud2",v3);//显示点云
viewer->addPointCloud<pcl::PointXYZ>(cloudC,single_color3,"cloud3",v3);//显示点云
viewer->addCoordinateSystem(2);//添加坐标系
viewer->spin();
第三个窗口将 点云A、B、C 显示在一起。来看下整体效果
=====================================================
Result
终端打印的 B 比A 多出的 点云
点云显示的点
左边为点云A ,中间为点云B
右边为三个点云的组合;
红色的是A点云有B点云没有的;绿色为A和B共有的;白色为B比A 多的。