技術標籤：C++Studyc++

基於法向量去除平面（C++）

一、基本介紹

本程式碼是統計所有點雲法向量最多方向，後去除滿足法向量方向點雲；
功能：想去除點雲中平面

#include <iostream>
#include <fstream>
#include <vector>

struct POINTCLOUD3D
{
	double px, py, pz, nx, ny, nz;
	POINTCLOUD3D(){};
	POINTCLOUD3D(double val1, double val2, double val3)
	{
		px = val1;
		py = val2;
		pz = val3;
	}
	POINTCLOUD3D(double val1, double val2, double val3, double val4, double val5, double val6)
	{
		px = val1;
		py = val2;
		pz = val3;
		nx = val4;
		ny = val5;
		nz = val6;
	}
};
struct NORMALVECTOR
{
	double n_x, n_y, n_z;
	NORMALVECTOR(){};
};

//讀取檔案，輸出點雲vector
void ImportPointCloud(char &filename, std::vector<POINTCLOUD3D> &importpt)
{
	POINTCLOUD3D temp;
	std::ifstream import;
	import.open(&filename);
	while (import.peek() != EOF)
	{
		double x;
		import >> temp.px >> temp.py >> temp.pz >> temp.nx >> temp.ny >> temp.nz;
		//import >> temp.px >> temp.py >> temp.pz;
		importpt.push_back(temp);
	}
	import.close();
}

//歸一化法向量
void NormalizedNormalVector(std::vector<POINTCLOUD3D> &importpt, std::vector<NORMALVECTOR> &outputvector)
{
	double V;
	NORMALVECTOR NorVec;
	for (int i = 0; i < importpt.size(); i++)
	{
		if (importpt[i].nx == 0 || importpt[i].ny == 0 || importpt[i].nz == 0)
		{
			NorVec.n_x = 0.0;
			NorVec.n_y = 0.0;
			NorVec.n_z = 0.0;
			continue;
		}
		V = sqrt(importpt[i].nx*importpt[i].nx + importpt[i].ny*importpt[i].ny + importpt[i].nz*importpt[i].nz);
		NorVec.n_x = (double)importpt[i].nx / V;
		NorVec.n_y = (double)importpt[i].ny / V;
		NorVec.n_z = (double)importpt[i].nz / V;
		//outputvector.push_back(NorVec);
		//NorVec.n_x = (double)importpt[i].nx;
		//NorVec.n_y = (double)importpt[i].ny;
		//NorVec.n_z = (double)importpt[i].nz;
		outputvector.push_back(NorVec);
	}
}


//統計法向量存在最多點
void RemovePlanePoint(std::vector<POINTCLOUD3D> &importpt, std::vector<NORMALVECTOR> &outputvector, double interval, std::vector<POINTCLOUD3D> &leftpt)
{
	//先根據間隔把輸入的法向量整數化、
	for (int j = 0; j < outputvector.size(); j++)
	{
		outputvector[j].n_x = round((outputvector[j].n_x + 1.0) / interval); //第幾份向下取整
		outputvector[j].n_y = round((outputvector[j].n_y + 1.0) / interval);
		outputvector[j].n_z = round((outputvector[j].n_z + 1.0) / interval);
	}
	//-1與-180°與第幾份建立關係
	//定義3個一維陣列，分別統計分量個數
	//int number;
	int number = round(2.0 / interval);

	int a, b, c;
	//x方向
	int *Statistics_x = new int[number]();     //堆區開闢空間
	for (int k = 0; k < outputvector.size(); k++)
	{
		Statistics_x[(int)outputvector[k].n_x]++;
	}

	int Max_x = 0, NUM_x; //x方向上定義最大值，和對應的第幾個
	for (a = 0; a < number; a++)
	{
		if (Statistics_x[a] > Max_x)
		{
			Max_x = Statistics_x[a];
			NUM_x = a;
		}
	}
	//delete[]Statistics_x;


	
	//y方向
	int *Statistics_y = new int[number]();     //堆區開闢空間
	for (int k = 0; k < outputvector.size(); k++)
	{
		Statistics_y[(int)outputvector[k].n_y]++;
	}

	int Max_y = 0, NUM_y; //y方向上定義最大值，和對應的第幾個
	for (b = 0; b < number; b++)
	{
		if (Statistics_y[b] > Max_y)
		{
			Max_y = Statistics_y[b];
			NUM_y = b;
		}
	}
	//delete[]Statistics_y;

	//z方向
	int *Statistics_z = new int[number]();     //堆區開闢空間
	for (int k = 0; k < outputvector.size(); k++)
	{
		Statistics_z[(int)outputvector[k].n_z]++;
	}

	int Max_z = 0, NUM_z; //z方向上定義最大值，和對應的第幾個
	for (c = 0; c < number; c++)
	{
		if (Statistics_z[c] > Max_z)
		{
			Max_z = Statistics_z[c];
			NUM_z = c;
		}
	}
	//delete[]Statistics_z;


	POINTCLOUD3D left;
	//判斷3個方向同時滿足向量
	for (int k = 0; k < outputvector.size(); k++)
	{
		if (outputvector[k].n_x != NUM_x || outputvector[k].n_y != NUM_y || outputvector[k].n_z != NUM_z)
		//if (outputvector[k].n_x == NUM_x && outputvector[k].n_y == NUM_y && outputvector[k].n_z == NUM_z)
		{
			left.px = importpt[k].px;
			left.py = importpt[k].py;
			left.pz = importpt[k].pz;
			left.nx = importpt[k].nx;
			left.ny = importpt[k].ny;
			left.nz = importpt[k].nz;
			leftpt.push_back(left);
		}
	}


	//迴圈求出陣列中個數最多的第幾份
	//定義一個vector存第幾個點
	//統計不同時滿足三個條件最多份數的具體數值點struct的n
	//根據點到處點雲


}

void OutputPointCloud(char &filename, std::vector<POINTCLOUD3D> &output)
{
	std::ofstream out;
	out.open(&filename);
	for (int i = 0; i < output.size(); i++)
	{
		out << output[i].px << " " << output[i].py << " " << output[i].pz << " "
			<< output[i].nx << " " << output[i].ny << " " << output[i].nz << " " << std::endl;
	}
	out.close();
}



int main()
{
	//讀取檔案
	std::vector<POINTCLOUD3D> importpt;
	char filename[255];
	sprintf_s(filename, 255, "sss4.asc");
	ImportPointCloud(*filename, importpt);
	//
	double interval = 0.1;
	std::vector<NORMALVECTOR> outputvector;
	std::vector<POINTCLOUD3D> leftpt;
	NormalizedNormalVector(importpt, outputvector);
	RemovePlanePoint(importpt, outputvector, interval, leftpt);

	//std::cout << leftpt[4000].px << std::endl;


	sprintf_s(filename, 255, "sss4-1.asc");
	OutputPointCloud(*filename, leftpt);
	

	return 0;
}
 

去點雲前圖

去點雲後圖