八叉树图及点云构建

1、构建原理

常见的八叉树的构建是根据递归思想构建，在递归划分时，一般按照格网大小，或者是格网内点数进行约束，对格网均匀划分成8个格网，直至满足条件为止。如下为一个立方体被划分成8个子节点示意图。

根据需要，对点云进行八叉树构造，将点云划分成多个小的块。

2、部分核心代码展示

（源码下载地址：机载LiDAR、车载、地基点云数据构建八叉树_八叉树的构建原理-算法与数据结构文档类资源-CSDN下载）

#include "arrayoperation.h"
#include "DataStruct.h"
#include "IO.h"
#include "CalculateFeatures.h"
#include<iostream>
#include<time.h>
#include<stdlib.h>
#include"ConnectLabel.h"
using namespace std;
struct OctreeNode 
{
	pcl::PointXYZ LeftBottom, RightUp;//该立方体的左下角、右上角点坐标
	vector<pcl::PointXYZ> points;
	PlaneError PlanePara;//该节点内点平面参数
	OctreeNode *Top_left_front, *Top_left_back;// 1  2
	OctreeNode *Top_right_front, *Top_right_back;// 3  4
	OctreeNode *Bottom_left_front, *Bottom_left_back;// 5  6
	OctreeNode *Bottom_right_front, *Bottom_right_back;//  7  8
	bool NodeType;//该节点是否可以继续划分  若可以，则是true；不可再分则为false
};

//一个立方体的左下点 与  右上点
struct Cubic 
{
	pcl::PointXYZ Buttomleft;
	pcl::PointXYZ Upright;
};

//判断一个点是否在一个立方体内
//point        待判断的点
//cubics       立方体
bool IsinCubic(pcl::PointXYZ point, Cubic cubics)
{
	if (point.x <= cubics.Upright.x && point.y <= cubics.Upright.y && point.z <= cubics.Upright.z && point.x > cubics.Buttomleft.x && point.y > cubics.Buttomleft.y && point.z > cubics.Buttomleft.z)
	{
		return true;
	}
	else
	{
		return false;
	}
}


//构造八叉树，终止条件是子节点内点集可以拟合成一个面（拟合误差小于阈值）；或者子节点内点数少于小于等于3个
//root         构建好的根节点指针
//residual     平面拟合误差阈值
void BuildOctree(OctreeNode * &root, vector<pcl::PointXYZ> points,double residual)
{
	CalculateFeatures calObject;
	arrayoperation arrObject;

	//创建一个新的根
	root = new OctreeNode;
	
	pcl::PointXYZ RightUp, LeftBottom;
	vector<double> Xvec, Yvec, Zvec;
	for (int i = 0; i < points.size(); i++)
	{
		Xvec.push_back(points[i].x);
		Yvec.push_back(points[i].y);
		Zvec.push_back(points[i].z);
	}
	RightUp.x = arrObject.getMax_vector(Xvec)+0.1;
	RightUp.y = arrObject.getMax_vector(Yvec)+0.1;
	RightUp.z = arrObject.getMax_vector(Zvec)+0.1;

	LeftBottom.x = arrObject.getMin_vector(Xvec)-0.1;
	LeftBottom.y = arrObject.getMin_vector(Yvec)-0.1;
	LeftBottom.z = arrObject.getMin_vector(Zvec)-0.1;


	root->LeftBottom = LeftBottom;
	root->RightUp = RightUp;
	

	root->points = points;
	root->PlanePara = calObject.estimatePlaneError(root->points);
	if (root->PlanePara.sigma < residual)
	{
		root->NodeType = false;//不可分	

		root->Top_left_front = NULL;
		root->Top_left_back = NULL;
		root->Top_right_front = NULL;
		root->Top_right_back = NULL;
		root->Bottom_left_front = NULL;
		root->Bottom_left_back = NULL;
		root->Bottom_right_front = NULL;
		root->Bottom_right_back = NULL;
	}
	else
	{
		root->NodeType = true;//可继续划分
		//x、y、z轴分辨率
		double gridsizex = (RightUp.x - LeftBottom.x) / 2;
		double gridsizey = (RightUp.y - LeftBottom.y) / 2;
		double gridsizez = (RightUp.z - LeftBottom.z) / 2;
		double xmin = LeftBottom.x;
		double ymin = LeftBottom.y;
		double zmin = LeftBottom.z;

		double xmax = RightUp.x;
		double ymax = RightUp.y;
		double zmax = RightUp.z;

		//8个立方体的左下角  右上角顶点坐标
		pcl::PointXYZ LeftBottom_1, LeftBottom_2, LeftBottom_3, LeftBottom_4, LeftBottom_5, LeftBottom_6, LeftBottom_7, LeftBottom_8;//8个子立方体的左下顶点坐标
		pcl::PointXYZ RightUp_1, RightUp_2, RightUp_3, RightUp_4, RightUp_5, RightUp_6, RightUp_7, RightUp_8;//8个子立方体的右上顶点坐标

		LeftBottom_1.x = xmin; LeftBottom_1.y = ymin; LeftBottom_1.z = zmin + gridsizez;
		RightUp_1.x = xmin + gridsizex; RightUp_1.y = ymin + gridsizey; RightUp_1.z = zmax;

		LeftBottom_2.x = xmin; LeftBottom_2.y = ymin + gridsizey; LeftBottom_2.z = zmin + gridsizez;
		RightUp_2.x = xmin + gridsizex; RightUp_2.y = ymax; RightUp_2.z = zmax;

		LeftBottom_3.x = xmin + gridsizex; LeftBottom_3.y = ymin; LeftBottom_3.z = zmin + gridsizez;
		RightUp_3.x = xmax; RightUp_3.y = ymin + gridsizey; RightUp_3.z = zmax;

		LeftBottom_4.x = xmin + gridsizex; LeftBottom_4.y = ymin + gridsizey; LeftBottom_4.z = zmin + gridsizez;
		RightUp_4.x = xmax; RightUp_4.y = ymax; RightUp_4.z = zmax;

		LeftBottom_5.x = xmin; LeftBottom_5.y = ymin; LeftBottom_5.z = zmin;
		RightUp_5.x = xmin + gridsizex; RightUp_5.y = ymin + gridsizey; RightUp_5.z = zmin + gridsizez;

		LeftBottom_6.x = xmin; LeftBottom_6.y = ymin + gridsizey; LeftBottom_6.z = zmin;
		RightUp_6.x = xmin + gridsizex; RightUp_6.y = ymax; RightUp_6.z = zmin + gridsizez;

		LeftBottom_7.x = xmin + gridsizex; LeftBottom_7.y = ymin; LeftBottom_7.z = zmin;
		RightUp_7.x = xmax; RightUp_7.y = ymin + gridsizey; RightUp_7.z = zmin + gridsizez;

		LeftBottom_8.x = xmin + gridsizex; LeftBottom_8.y = ymin + gridsizey; LeftBottom_8.z = zmin;
		RightUp_8.x = xmax; RightUp_8.y = ymax; RightUp_8.z = zmin + gridsizez;

		Cubic cube_1, cube_2, cube_3, cube_4, cube_5, cube_6, cube_7, cube_8;
		cube_1.Buttomleft = LeftBottom_1; cube_1.Upright = RightUp_1;
		cube_2.Buttomleft = LeftBottom_2; cube_2.Upright = RightUp_2;
		cube_3.Buttomleft = LeftBottom_3; cube_3.Upright = RightUp_3;
		cube_4.Buttomleft = LeftBottom_4; cube_4.Upright = RightUp_4;
		cube_5.Buttomleft = LeftBottom_5; cube_5.Upright = RightUp_5;
		cube_6.Buttomleft = LeftBottom_6; cube_6.Upright = RightUp_6;
		cube_7.Buttomleft = LeftBottom_7; cube_7.Upright = RightUp_7;
		cube_8.Buttomleft = LeftBottom_8; cube_8.Upright = RightUp_8;
		//将点划入到8个子节点下  
		vector<pcl::PointXYZ> points_1, points_2, points_3, points_4, points_5, points_6, points_7, points_8;//每个节点存储的点
		for (int i = 0; i < root->points.size(); i++)
		{
			pcl::PointXYZ curpoint = root->points[i];

			if (IsinCubic(curpoint, cube_1) == true)
			{
				points_1.push_back(curpoint);
			}
			else if (IsinCubic(curpoint, cube_2) == true)
			{
				points_2.push_back(curpoint);
			}
			else if (IsinCubic(curpoint, cube_3) == true)
			{
				points_3.push_back(curpoint);
			}
			else if (IsinCubic(curpoint, cube_4) == true)
			{
				points_4.push_back(curpoint);
			}
			else if (IsinCubic(curpoint, cube_5) == true)
			{
				points_5.push_back(curpoint);
			}
			else if (IsinCubic(curpoint, cube_6) == true)
			{
				points_6.push_back(curpoint);
			}
			else if (IsinCubic(curpoint, cube_7) == true)
			{
				points_7.push_back(curpoint);
			}
			else if (IsinCubic(curpoint, cube_8) == true)
			{
				points_8.push_back(curpoint);
			}
		}//end for

		//第一个cubic1
		PlaneError fitplane = calObject.estimatePlaneError(points_1);
		if (fitplane.sigma > residual && points_1.size() >=4)//点数也要有一定要求
		{
			root->Top_left_front = new OctreeNode();
			 vector<pcl::PointXYZ> temp= points_1;
			 BuildOctree(root->Top_left_front, temp, residual);
		}
		else//如果可以拟合，直接赋值给下面的孩子
		{
			root->Top_left_front = new OctreeNode();
			root->Top_left_front->LeftBottom = LeftBottom_1; 
			root->Top_left_front->RightUp = RightUp_1;
			root->Top_left_front->points = points_1;
			root->Top_left_front->PlanePara = fitplane;
			root->Top_left_front->NodeType = false;//不可再分

			root->Top_left_front->Top_left_front = NULL;
			root->Top_left_front->Top_left_back = NULL;
			root->Top_left_front->Top_right_front = NULL;
			root->Top_left_front->Top_right_back = NULL;
			root->Top_left_front->Bottom_left_front = NULL;
			root->Top_left_front->Bottom_left_back = NULL;
			root->Top_left_front->Bottom_right_front = NULL;
			root->Top_left_front->Bottom_right_back = NULL;

		}

		//第二个cubic2
		fitplane = calObject.estimatePlaneError(points_2);
		if (fitplane.sigma > residual && points_2.size() > 3)//点数也要有一定要求
		{
			root->Top_left_back = new OctreeNode();
			vector<pcl::PointXYZ> temp = points_2;
			BuildOctree(root->Top_left_back, temp, residual);
		}
		else
		{
			root->Top_left_back = new OctreeNode();
			root->Top_left_back->LeftBottom = LeftBottom_2;
			root->Top_left_back->RightUp = RightUp_2;
			root->Top_left_back->points = points_2;
			root->Top_left_back->PlanePara = fitplane;
			root->Top_left_back->NodeType = false;//不可再分

			//其他变成NULL
			root->Top_left_back->Top_left_front = NULL;
			root->Top_left_back->Top_left_back = NULL;
			root->Top_left_back->Top_right_front = NULL;
			root->Top_left_back->Top_right_back = NULL;
			root->Top_left_back->Bottom_left_front = NULL;
			root->Top_left_back->Bottom_left_back = NULL;
			root->Top_left_back->Bottom_right_front = NULL;
			root->Top_left_back->Bottom_right_back = NULL;

		}

		//第三个cubic3
		fitplane = calObject.estimatePlaneError(points_3);
		if (fitplane.sigma > residual && points_3.size() > 3)//点数也要有一定要求
		{
			root->Top_right_front = new OctreeNode();
			vector<pcl::PointXYZ> temp = points_3;
			BuildOctree(root->Top_right_front, temp, residual);
		}
		else
		{
			root->Top_right_front = new OctreeNode();
			root->Top_right_front->LeftBottom = LeftBottom_3;
			root->Top_right_front->RightUp = RightUp_3;
			root->Top_right_front->points = points_3;
			root->Top_right_front->PlanePara = fitplane;
			root->Top_right_front->NodeType = false;//不可再分

			root->Top_right_front->Top_left_front = NULL;
			root->Top_right_front->Top_left_back = NULL;
			root->Top_right_front->Top_right_front = NULL;
			root->Top_right_front->Top_right_back = NULL;
			root->Top_right_front->Bottom_left_front = NULL;
			root->Top_right_front->Bottom_left_back = NULL;
			root->Top_right_front->Bottom_right_front = NULL;
			root->Top_right_front->Bottom_right_back = NULL;
		}



		//第四个cubic4
		fitplane = calObject.estimatePlaneError(points_4);
		if (fitplane.sigma > residual && points_4.size() > 3)//点数也要有一定要求
		{
			root->Top_right_back = new OctreeNode();
			vector<pcl::PointXYZ> temp = points_4;
			BuildOctree(root->Top_right_back, temp, residual);
		}
		else
		{
			root->Top_right_back = new OctreeNode();
			root->Top_right_back->LeftBottom = LeftBottom_4;
			root->Top_right_back->RightUp = RightUp_4;
			root->Top_right_back->points = points_4;
			root->Top_right_back->PlanePara = fitplane;
			root->Top_right_back->NodeType = false;//不可再分

			root->Top_right_back->Top_left_front = NULL;
			root->Top_right_back->Top_left_back = NULL;
			root->Top_right_back->Top_right_front = NULL;
			root->Top_right_back->Top_right_back = NULL;
			root->Top_right_back->Bottom_left_front = NULL;
			root->Top_right_back->Bottom_left_back = NULL;
			root->Top_right_back->Bottom_right_front = NULL;
			root->Top_right_back->Bottom_right_back = NULL;
		}


		//第五个cubic5
		fitplane = calObject.estimatePlaneError(points_5);
		if (fitplane.sigma > residual && points_5.size() > 3)//点数也要有一定要求
		{
			root->Bottom_left_front = new OctreeNode();
			vector<pcl::PointXYZ> temp = points_5;
			BuildOctree(root->Bottom_left_front, temp, residual);
		}
		else
		{
			root->Bottom_left_front = new OctreeNode();
			root->Bottom_left_front->LeftBottom = LeftBottom_5;
			root->Bottom_left_front->RightUp = RightUp_5;
			root->Bottom_left_front->points = points_5;
			root->Bottom_left_front->PlanePara = fitplane;
			root->Bottom_left_front->NodeType = false;//不可再分

			root->Bottom_left_front->Top_left_front = NULL;
			root->Bottom_left_front->Top_left_back = NULL;
			root->Bottom_left_front->Top_right_front = NULL;
			root->Bottom_left_front->Top_right_back = NULL;
			root->Bottom_left_front->Bottom_left_front = NULL;
			root->Bottom_left_front->Bottom_left_back = NULL;
			root->Bottom_left_front->Bottom_right_front = NULL;
			root->Bottom_left_front->Bottom_right_back = NULL;
		}


		//第六个cubic6
		fitplane = calObject.estimatePlaneError(points_6);
		if (fitplane.sigma > residual && points_6.size() > 3)//点数也要有一定要求
		{
			root->Bottom_left_back = new OctreeNode();
			vector<pcl::PointXYZ> temp = points_6;
			BuildOctree(root->Bottom_left_back, temp, residual);
		}
		else
		{
			root->Bottom_left_back = new OctreeNode();
			root->Bottom_left_back->LeftBottom = LeftBottom_6;
			root->Bottom_left_back->RightUp = RightUp_6;
			root->Bottom_left_back->points = points_6;
			root->Bottom_left_back->PlanePara = fitplane;
			root->Bottom_left_back->NodeType = false;//不可再分

			root->Bottom_left_back->Top_left_front = NULL;
			root->Bottom_left_back->Top_left_back = NULL;
			root->Bottom_left_back->Top_right_front = NULL;
			root->Bottom_left_back->Top_right_back = NULL;
			root->Bottom_left_back->Bottom_left_front = NULL;
			root->Bottom_left_back->Bottom_left_back = NULL;
			root->Bottom_left_back->Bottom_right_front = NULL;
			root->Bottom_left_back->Bottom_right_back = NULL;
		}



		//第七个cubic7
		fitplane = calObject.estimatePlaneError(points_7);
		if (fitplane.sigma > residual && points_7.size() > 3)//点数也要有一定要求
		{
			root->Bottom_right_front = new OctreeNode();
			vector<pcl::PointXYZ> temp = points_7;
			BuildOctree(root->Bottom_right_front, temp, residual);
		}
		else
		{
			root->Bottom_right_front = new OctreeNode();
			root->Bottom_right_front->LeftBottom = LeftBottom_7;
			root->Bottom_right_front->RightUp = RightUp_7;
			root->Bottom_right_front->points = points_7;
			root->Bottom_right_front->PlanePara = fitplane;
			root->Bottom_right_front->NodeType = false;//不可再分

			root->Bottom_right_front->Top_left_front = NULL;
			root->Bottom_right_front->Top_left_back = NULL;
			root->Bottom_right_front->Top_right_front = NULL;
			root->Bottom_right_front->Top_right_back = NULL;
			root->Bottom_right_front->Bottom_left_front = NULL;
			root->Bottom_right_front->Bottom_left_back = NULL;
			root->Bottom_right_front->Bottom_right_front = NULL;
			root->Bottom_right_front->Bottom_right_back = NULL;
		}



		//第八个cubic8
		fitplane = calObject.estimatePlaneError(points_8);
		if (fitplane.sigma > residual && points_8.size() > 3)//点数也要有一定要求
		{
			root->Bottom_right_back = new OctreeNode();
			vector<pcl::PointXYZ> temp = points_8;
			BuildOctree(root->Bottom_right_back, temp, residual);
		}
		else
		{
			root->Bottom_right_back = new OctreeNode();
			root->Bottom_right_back->LeftBottom = LeftBottom_8;
			root->Bottom_right_back->RightUp = RightUp_8;
			root->Bottom_right_back->points = points_8;
			root->Bottom_right_back->PlanePara = fitplane;
			root->Bottom_right_back->NodeType = false;//不可再分

			root->Bottom_right_back->Top_left_front = NULL;
			root->Bottom_right_back->Top_left_back = NULL;
			root->Bottom_right_back->Top_right_front = NULL;
			root->Bottom_right_back->Top_right_back = NULL;
			root->Bottom_right_back->Bottom_left_front = NULL;
			root->Bottom_right_back->Bottom_left_back = NULL;
			root->Bottom_right_back->Bottom_right_front = NULL;
			root->Bottom_right_back->Bottom_right_back = NULL;
		}

	}
	
}
//将平整度好的点集和平整度不好的点集分开存放
//root              八叉树节点
//planecluster      平整度好的点集
//nonplanecluster   平整度不好的点集
void FalseCluster(OctreeNode * &root, vector<vector<pcl::PointXYZ>> &planecluster, vector<vector<pcl::PointXYZ>> &nonplanecluster)
{
	if (root != NULL)
	{
		vector<vector<pcl::PointXYZ>> tempplanecluster, tempnonplanecluster;
		tempplanecluster = planecluster;
		planecluster.clear();
		tempnonplanecluster = nonplanecluster;
		nonplanecluster.clear();

		if (root->NodeType == false)//不可再分  子节点
		{
			if (root->points.size() > 5)
			{
				tempplanecluster.push_back(root->points);
			}
			else
			{
				tempnonplanecluster.push_back(root->points);
			}
		}
		planecluster = tempplanecluster;
		nonplanecluster = tempnonplanecluster;

		FalseCluster(root->Top_left_front, planecluster, nonplanecluster);
		FalseCluster(root->Top_left_back, planecluster, nonplanecluster);
		FalseCluster(root->Top_right_front, planecluster, nonplanecluster);
		FalseCluster(root->Top_right_back, planecluster, nonplanecluster);

		FalseCluster(root->Bottom_left_front, planecluster, nonplanecluster);
		FalseCluster(root->Bottom_left_back, planecluster, nonplanecluster);
		FalseCluster(root->Bottom_right_front, planecluster, nonplanecluster);
		FalseCluster(root->Bottom_right_back, planecluster, nonplanecluster);
	}
}

void main()
{
	IO ioObject;
	vector<pcl::PointXYZ> cloud;
	cloud = ioObject.ReadPointXYZIntoVector("XX.xyz");
	
	OctreeNode *root = new OctreeNode();
	BuildOctree(root, cloud, 0.1);

	vector<vector<pcl::PointXYZ>> planecluster, nonplanecluster;
	FalseCluster(root, planecluster, nonplanecluster);

	ofstream outfile("multiple_whole.txt", ios::out);
	srand((int)time(0));
	for (int i = 0; i < planecluster.size(); i++)
	{
		int R = GetRandomNumber();
		int G = GetRandomNumber();
		int B = GetRandomNumber();
		for (int j = 0; j < planecluster[i].size(); j++)
		{
			outfile << planecluster[i][j].x << "\t" << planecluster[i][j].y << "\t" << planecluster[i][j].z << "\t" << R << "\t" << G << "\t" << B << endl;
		}
	}
	outfile.close();
	cout << "finish" << endl;

	system("pause");
}

构建的八叉树指针，可见一层一层往下进行的

点云根据构建的八叉树，划分的结果如下：