PCL 八叉树实现空间变化检测

一、算法原理

见《点云库PCL从入门到精通》P111-P113

二、代码实现

#include <iostream>
#include <vector>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <pcl/io/pcd_io.h>	
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
using namespace std;
int
main (int*argc, char*argv)
{

// 八叉树分辨率 即体素的大小
float resolution =0.1f;
// 初始化空间变化检测对象
pcl::octree::OctreePointCloudChangeDetector<pcl::PointXYZ>octree (resolution);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile<pcl::PointXYZ>("30Kicp.pcd", *cloudA);
//添加点云到八叉树，建立八叉树
octree.setInputCloud (cloudA);
octree.addPointsFromInputCloud ();
// 交换八叉树缓存，但是cloudA对应的八叉树仍在内存中
octree.switchBuffers ();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB (new pcl::PointCloud<pcl::PointXYZ> );
pcl::io::loadPCDFile<pcl::PointXYZ>("30mC1.pcd", *cloudB);

//添加 cloudB到八叉树
octree.setInputCloud (cloudB);
octree.addPointsFromInputCloud ();
vector<int>newPointIdxVector;
//获取前一cloudA对应的八叉树在cloudB对应八叉树中没有的体素
octree.getPointIndicesFromNewVoxels (newPointIdxVector);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_change(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloudB, newPointIdxVector, *cloud_change);
//打印输出点
cout<<"Output from getPointIndicesFromNewVoxels:"<<endl;
for (size_t i=0; i<newPointIdxVector.size (); ++i)
cout<<i<<"# Index:"<<newPointIdxVector[i]
<<"  Point:"<<cloudB->points[newPointIdxVector[i]].x <<" "
<<cloudB->points[newPointIdxVector[i]].y <<" "
<<cloudB->points[newPointIdxVector[i]].z <<endl;


// 初始化点云可视化对象
boost::shared_ptr<pcl::visualization::PCLVisualizer>viewer(new pcl::visualization::PCLVisualizer("显示点云"));
viewer->setBackgroundColor(0, 0, 0);  //设置背景颜色为黑色

// 对cloudA点云着色可视化 (白色).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>cloudA_color(cloudA, 255, 255, 255);
viewer->addPointCloud<pcl::PointXYZ>(cloudA, cloudA_color, "cloudA");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloudA");
// 对cloudB点云着色可视化 (白色).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>cloudB_color(cloudB, 255, 255, 255);
viewer->addPointCloud<pcl::PointXYZ>(cloudB, cloudB_color, "cloudB");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloudB");
// 对检测出来的变化区域可视化.
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>change_color(cloud_change, 255, 0, 0);
viewer->addPointCloud<pcl::PointXYZ>(cloud_change, change_color, "cloud_change");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud_change");

// 启动可视化
//viewer->addCoordinateSystem(0.1);  //显示XYZ指示轴
//viewer->initCameraParameters();   //初始化摄像头参数

// 等待直到可视化窗口关闭
while (!viewer->wasStopped())
{
	viewer->spinOnce(100);
	boost::this_thread::sleep(boost::posix_time::microseconds(1000));
}

return (0);
}

三、结果展示

四、官网链接pcl::octree::OctreePointCloudChangeDetector