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