可视化八叉树程序（根据PCL官方示例修改）

最近在研究八叉树，看到了pcl有八叉树可视化程序，但是在我的电脑上运行不成功（pcl可视化体素是通过VTK，这个可能是主要原因），因此找到程序的源码进行了一些修改，成功实现八叉树可视化。
配置：PCL1.8.0+VTK8.0+VS2015
修改的八叉树可视化程序，主要有以下几个部分：
1.图例
2.按键回调
3.可视化八叉树
4.线框模式和实体表面模式
5.点模式
通过图例可以很好的操作程序，下面直接放上源码：

#include <pcl/io/pcd_io.h>
#include <pcl/common/time.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/point_cloud_handlers.h>
#include <pcl/visualization/common/common.h>

#include <pcl/octree/octree_pointcloud_voxelcentroid.h>

#include <pcl/filters/filter.h>
#include<pcl/filters/voxel_grid.h>
#include<pcl\filters\approximate_voxel_grid.h>
#include "boost.h"

#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkCubeSource.h>
#include <vtkOutputWindow.h>
#include<vtkAutoInit.h>
VTK_MODULE_INIT(vtkRenderingOpenGL);
VTK_MODULE_INIT(vtkInteractionStyle);
//=============================
// Displaying cubes is very long!
// so we limit their numbers.
const int MAX_DISPLAYED_CUBES(15000);
//=============================

class OctreeViewer
{
public:
	OctreeViewer(std::string &filename, double resolution) :
		viz("Octree visualizator"), cloud(new pcl::PointCloud<pcl::PointXYZ>()),
		displayCloud(new pcl::PointCloud<pcl::PointXYZ>()), octree(resolution), displayCubes(false),
		showPointsWithCubes(false), wireframe(true)
	{

		//try to load the cloud
		if (!loadCloud(filename))
			return;

		//register keyboard callbacks
		viz.registerKeyboardCallback(&OctreeViewer::keyboardEventOccurred, *this, 0);



		//set current level to half the maximum one
		displayedDepth = static_cast<int> (floor(octree.getTreeDepth() / 2.0));
		if (displayedDepth == 0)
			displayedDepth = 1;

		//show octree at default depth
		extractPointsAtLevel(displayedDepth);

		//reset camera
		viz.resetCameraViewpoint("cloud");

		//run main loop
		run();

	}

private:
	//========================================================
	// PRIVATE ATTRIBUTES
	//========================================================
	//visualizer
	pcl::PointCloud<pcl::PointXYZ>::Ptr xyz;
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr xyz_rgb;

	pcl::visualization::PCLVisualizer viz;
	//original cloud
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
	//displayed_cloud
	pcl::PointCloud<pcl::PointXYZ>::Ptr displayCloud;
	//octree
	pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ> octree;
	//level
	int displayedDepth;
	//bool to decide if we display points or cubes
	bool displayCubes, showPointsWithCubes, wireframe;
	double size = 4;
	//========================================================

	/* \brief Callback to interact with the keyboard
	*
	*/
	void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event, void *)
	{

		if (event.getKeySym() == "a" && event.keyDown())
		{
			IncrementLevel();
		}
		else if (event.getKeySym() == "z" && event.keyDown())
		{
			DecrementLevel();
		}
		else if (event.getKeySym() == "d" && event.keyDown())
		{
			displayCubes = !displayCubes;
			update();
		}
		else if (event.getKeySym() == "x" && event.keyDown())
		{
			showPointsWithCubes = !showPointsWithCubes;
			update();
		}
		else if (event.getKeySym() == "w" && event.keyDown())
		{
			if (!wireframe)
				wireframe = true;
			update();
		}
		else if (event.getKeySym() == "s" && event.keyDown())
		{
			if (wireframe)
				wireframe = false;
			update();
		}
		//else if (event.getKeySym()=="1"&&event.keyDown())
		//{
		//	viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size, "cloud");
		//	size++;
		//	update();
		//}
		//else if (event.getKeySym()=="down"&&event.keyDown())
		//{
		//	viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size, "cloud");
		//	if (size==1)
		//	{
		//		return;
		//	}
		//	size--;
		//	update();
		//}
	}

	/* \brief Graphic loop for the viewer
	*
	*/
	void run()
	{
		while (!viz.wasStopped())
		{
			//main loop of the visualizer
			viz.spinOnce(100);
			boost::this_thread::sleep(boost::posix_time::microseconds(100000));
		}
	}

	/* \brief Helper function that read a pointcloud file (returns false if pbl)
	*  Also initialize the octree
	*
	*/
	bool loadCloud(std::string &filename)
	{
		std::cout << "Loading file " << filename.c_str() << std::endl;
		//read cloud
		if (pcl::io::loadPCDFile(filename, *cloud))
		{
			std::cerr << "ERROR: Cannot open file " << filename << "! Aborting..." << std::endl;
			return false;
		}

		//remove NaN Points
		std::vector<int> nanIndexes;
		pcl::removeNaNFromPointCloud(*cloud, *cloud, nanIndexes);
		std::cout << "Loaded " << cloud->points.size() << " points" << std::endl;

		//create octree structure
		octree.setInputCloud(cloud);
		//update bounding box automatically
		octree.defineBoundingBox();
		//add points in the tree
		octree.addPointsFromInputCloud();
		return true;
	}

	/* \brief Helper function that draw info for the user on the viewer
	*
	*/
	void showLegend(bool showCubes)
	{
		//key legends
		viz.addText("Keys:", 0, 170, 0.0, 1.0, 0.0, "keys_t");
		viz.addText("a -> Increment displayed depth", 10, 155, 0.0, 1.0, 0.0, "key_a_t");
		viz.addText("z -> Decrement displayed depth", 10, 140, 0.0, 1.0, 0.0, "key_z_t");
		viz.addText("d -> Toggle Point/Cube representation", 10, 125, 0.0, 1.0, 0.0, "key_d_t");
		viz.addText("x -> Show/Hide original cloud", 10, 110, 0.0, 1.0, 0.0, "key_x_t");
		viz.addText("s/w -> Surface/Wireframe representation", 10, 95, 0.0, 1.0, 0.0, "key_sw_t");

		char dataDisplay[256];
		sprintf(dataDisplay, "Displaying data as %s", (showCubes) ? ("CUBES") : ("POINTS"));
		viz.removeShape("disp_t");
		viz.addText(dataDisplay, 0, 60, 1.0, 0.0, 0.0, "disp_t");

		char level[256];
		sprintf(level, "Displayed depth is %d on %d", displayedDepth, octree.getTreeDepth());
		viz.removeShape("level_t1");
		viz.addText(level, 0, 45, 1.0, 0.0, 0.0, "level_t1");

		viz.removeShape("level_t2");
		sprintf(level, "Voxel size: %.4fm [%lu voxels]", sqrt(octree.getVoxelSquaredSideLen(displayedDepth)),
			displayCloud->points.size());
		viz.addText(level, 0, 30, 1.0, 0.0, 0.0, "level_t2");

		viz.removeShape("org_t");
		if (showPointsWithCubes)
			viz.addText("Displaying original cloud", 0, 15, 1.0, 0.0, 0.0, "org_t");
	}

	/* \brief Visual update. Create visualizations and add them to the viewer
	*
	*/
	void update()
	{
		//remove existing shapes from visualizer
		clearView();

		//prevent the display of too many cubes
		bool displayCubeLegend = displayCubes && static_cast<int> (displayCloud->points.size()) <= MAX_DISPLAYED_CUBES;

		showLegend(displayCubeLegend);

		if (displayCubeLegend)
		{
			//show octree as cubes
			showCubes(sqrt(octree.getVoxelSquaredSideLen(displayedDepth)));
			if (showPointsWithCubes)
			{
				//add original cloud in visualizer
				pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(cloud, "z");
				viz.addPointCloud(cloud, color_handler, "cloud");
				viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size, "cloud");
			}
		}
		else
		{
			//add current cloud in visualizer
			pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(displayCloud, "z");
			viz.addPointCloud(displayCloud, color_handler, "cloud");
			viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size, "cloud");
		}
	}

	/* \brief remove dynamic objects from the viewer
	*
	*/
	void clearView()
	{
		//remove cubes if any
		//vtkRenderer *renderer = viz.getRenderWindow()->GetRenderers()->GetFirstRenderer();
		//while (renderer->GetActors()->GetNumberOfItems() > 0)
		//	renderer->RemoveActor(renderer->GetActors()->GetLastActor());
		viz.removeAllShapes();
		//for (size_t i = 0; i < displayCloud->size(); i++)
		//{

		//	viz.removeShape("cube" + std::to_string(i));
		//}
		//remove point clouds if any
		viz.removePointCloud("cloud");
	}

	/* \brief Create a vtkSmartPointer object containing a cube
	*
	*/
	//vtkSmartPointer<vtkPolyData> GetCuboid(double minX, double maxX, double minY, double maxY, double minZ, double maxZ)
	//{
	//	vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New();
	//	cube->SetBounds(minX, maxX, minY, maxY, minZ, maxZ);
	//	return cube->GetOutput();
	//}

	/* \brief display octree cubes via vtk-functions
	*
	*/
	void showCubes(double voxelSideLen)
	{


		//get the renderer of the visualizer object
		//vtkRenderer *renderer = viz.getRenderWindow()->GetRenderers()->GetFirstRenderer();
		//
		//vtkSmartPointer<vtkAppendPolyData> treeWireframe = vtkSmartPointer<vtkAppendPolyData>::New();
		size_t i;
		double s = voxelSideLen / 2.0;
		for (i = 0; i < displayCloud->points.size(); i++)
		{
			
			double x = displayCloud->points[i].x;
			double y = displayCloud->points[i].y;
			double z = displayCloud->points[i].z;
			viz.addCube(x - s, x + s, y - s, y + s, z - s, z + s,0.5,0.5,0.5,"cube"+std::to_string(i));
//#if VTK_MAJOR_VERSION < 6
//			treeWireframe->AddInput(GetCuboid(x - s, x + s, y - s, y + s, z - s, z + s));
//#else
//			treeWireframe->AddInputData(GetCuboid(x - s, x + s, y - s, y + s, z - s, z + s));
//#endif
		}
//
//		vtkSmartPointer<vtkActor> treeActor = vtkSmartPointer<vtkActor>::New();
//
//		vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New();
//#if VTK_MAJOR_VERSION < 6
//		mapper->SetInput(treeWireframe->GetOutput());
//#else
//		mapper->SetInputData(treeWireframe->GetOutput());
//#endif
//		treeActor->SetMapper(mapper);
//
//		treeActor->GetProperty()->SetColor(1.0, 1.0, 1.0);
//		treeActor->GetProperty()->SetLineWidth(2);
		if (wireframe)
		{
			viz.setRepresentationToWireframeForAllActors();

			//treeActor->GetProperty()->SetRepresentationToWireframe();
			//treeActor->GetProperty()->SetOpacity(0.35);
		}
		else
			viz.setRepresentationToSurfaceForAllActors();
			//treeActor->GetProperty()->SetRepresentationToSurface();
//
//		renderer->AddActor(treeActor);
	}

	/* \brief Extracts all the points of depth = level from the octree
	*
	*/
	void extractPointsAtLevel(int depth)
	{
		displayCloud->points.clear();
		
		pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::Iterator tree_it;
		pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::Iterator tree_it_end = octree.end();
		//pcl::octree::OctreePointCloudVoxelCentroid<pcl::PointXYZ>::DepthFirstIterator tree_it;

		pcl::PointXYZ pt;
		std::cout << "===== Extracting data at depth " << depth << "... " << std::flush;
		double start = pcl::getTime();

		for (tree_it = octree.begin(depth); tree_it != tree_it_end; ++tree_it)
		{

			//if (tree_it.isBranchNode())
			//{
			//	pcl::octree::OctreeBranchNode<pcl::octree::OctreeContainerEmpty>* branch=tree_it.getCurrentOctreeNode();
			//}
			if (tree_it.getCurrentOctreeDepth()!=depth)
			{
				continue;
			}
			Eigen::Vector3f voxel_min, voxel_max;
			octree.getVoxelBounds(tree_it, voxel_min, voxel_max);

			pt.x = (voxel_min.x() + voxel_max.x()) / 2.0f;
			pt.y = (voxel_min.y() + voxel_max.y()) / 2.0f;
			pt.z = (voxel_min.z() + voxel_max.z()) / 2.0f;
			displayCloud->points.push_back(pt);
		}

		double end = pcl::getTime();
		printf("%lu pts, %.4gs. %.4gs./pt. =====\n", displayCloud->points.size(), end - start,
			(end - start) / static_cast<double> (displayCloud->points.size()));

		update();
	}

	/* \brief Helper function to increase the octree display level by one
	*
	*/
	bool IncrementLevel()
	{
		if (displayedDepth < static_cast<int> (octree.getTreeDepth()))
		{
			displayedDepth++;
			extractPointsAtLevel(displayedDepth);
			return true;
		}
		else
			return false;
	}

	/* \brief Helper function to decrease the octree display level by one
	*
	*/
	bool DecrementLevel()
	{
		if (displayedDepth > 0)
		{
			displayedDepth--;
			extractPointsAtLevel(displayedDepth);
			return true;
		}
		return false;
	}

};

int main(int argc, char ** argv)
{

	//防止VTK的警告
	//vtkOutputWindow::SetGlobalWarningDisplay(0);
	std::string cloud_path("bunny.pcd");
	OctreeViewer v(cloud_path, 0.01);
}

生成结果如下图：