技術標籤：點雲資料處理c++

前兩天做了一個點雲降取樣的專案，用pcl自帶的降取樣方法出來的結果不是很理想，於是就自己寫了一個。為了使程式碼執行效率高點就採用了基於點雲索引的方式。
本文使用的方法為：首先，計算點雲群的Bounding Box;然後，根據一定的解析度將空間點雲體素化，並記錄下每個體素所包含點雲的索引（體素化的實質就是給點雲賦予體素標籤）；最後，遍歷體素，根據每個體素內點雲的索引取點雲座標，計算每個體素重心的座標，保留體素內距離重心最近的點。從而實現降取樣。
經過以上操作獲取的點雲密度相對較為均勻。

1、點雲索引計算公式（即計算每個點雲屬於哪個體素）

2、重心座標計算公式

#include <liblas\liblas.hpp>
#include <fstream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/common/common.h>
#include <algorithm>

using namespace pcl;
 


void Subsample(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &in_cloud , pcl::PointCloud<pcl::PointXYZRGB>::Ptr &filtered_cloud)
{

	//一、體素化（實質為給每個點雲賦予體素標籤）
	pcl::PointXYZRGB min_pt, max_pt;
	pcl::getMinMax3D(*in_cloud,min_pt,max_pt);
	double resolution = 0.1;
	int x_size = ceil((max_pt.
 
x - min_pt.x)/resolution);
	int y_size = ceil((max_pt.y - min_pt.y)/resolution);
	int z_size = ceil((max_pt.z - min_pt.z)/resolution);
	int voxel_number = x_size*y_size*z_size;
	   //為每個點雲附體素編號，並在相應體素中記錄其點雲索引
	std::vector<std::vector<size_t>> all_label;
	all_label.resize(voxel_number);
	for (size_t i = 0; i < in_cloud->size(); ++i)
	{
		int x_index = (in_cloud->points[i].x - min_pt.x) / resolution;
		int y_index = (in_cloud->points[i].y - min_pt.y) / resolution;
		int z_index = (in_cloud->points[i].z - min_pt.z) / resolution;
		int voxel_index = z_index * (x_size*y_size) + y_index * x_size + x_index;
		all_label[voxel_index].push_back(i);
	}
	
	//二、計算相同標籤點雲的重心座標，並求出距離重心最近的點雲，儲存該點雲至filter_cloud
		//每個體素中點雲的個數有三種：0,1，>1
	for (size_t i = 0; i < voxel_number; ++i)
	{
		if (all_label[i].size() == 0)   //體素中無點雲
		{
			continue;
		}
		else
		{
			if (all_label[i].size() == 1)   //體素中只有一個點雲
			{
				int pt_index_0 = all_label[i][0];
				pcl::PointXYZRGB pt1;
				pt1.x = in_cloud->points[pt_index_0].x;
				pt1.y = in_cloud->points[pt_index_0].y;
				pt1.z = in_cloud->points[pt_index_0].z;
				pt1.r = in_cloud->points[pt_index_0].r;
				pt1.g = in_cloud->points[pt_index_0].g;
				pt1.b = in_cloud->points[pt_index_0].b;
				filtered_cloud->points.push_back(pt1);
				
			}
			else     //體素中點雲個數大於1
			{
				//計算重心座標
				double sum_x = 0;
				double sum_y = 0;
				double sum_z = 0;
				for (size_t j = 0; j < all_label[i].size(); ++j)      
				{
					int pt_index_1 = all_label[i][j];
					double sum_x0 = in_cloud->points[pt_index_1].x;       //fuck !!!!!
					double sum_y0 = in_cloud->points[pt_index_1].y;       //fuck !!!!!
					double sum_z0 = in_cloud->points[pt_index_1].z;       //fuck !!!!!
					sum_x = sum_x0++;
					sum_y = sum_y0++;
					sum_z = sum_z0++;
				}
				int pt_number = all_label[i].size();
				double center_x = sum_x / pt_number;
				double center_y = sum_y / pt_number;
				double center_z = sum_z / pt_number;

				//尋找最近點
				   //計算體素中每個點與重心的距離
				std::vector<size_t> distance;
				for (size_t k = 0; k < all_label[i].size(); ++k)
				{
					int pt_index2 = all_label[i][k];
					double pt_x = in_cloud->points[pt_index2].x;
					double pt_y = in_cloud->points[pt_index2].y;
					double pt_z = in_cloud->points[pt_index2].z;
					double dx = pt_x - center_x;
					double dy = pt_y - center_y;
					double dz = pt_z - center_z;
					double dis = sqrt(dx*dx+dy*dy+dz*dz);
					distance.push_back(dis);
				}

				   //記錄最近點的索引
			    auto min_dis = std::min_element(std::begin(distance), std::end(distance));
			    int min_dis_index = std::distance(std::begin(distance),min_dis);
				int pt_index = all_label[i][min_dis_index];

				   //儲存最近點
				pcl::PointXYZRGB pt;
				pt.x = in_cloud->points[pt_index].x;
				pt.y = in_cloud->points[pt_index].y;
				pt.z = in_cloud->points[pt_index].z;
				pt.r = in_cloud->points[pt_index].r;
				pt.g = in_cloud->points[pt_index].g;
				pt.b = in_cloud->points[pt_index].b;
				filtered_cloud->points.push_back(pt);
			}
		}

	}
	all_label.clear();

}

void las2las(std::string fname)
{

	//開啟las檔案
	std::ifstream ifs(fname, std::ios::in | std::ios::binary);
	liblas::ReaderFactory f;
	liblas::Reader reader = f.CreateWithStream(ifs);

	//讀取las檔案資訊頭
	liblas::Header const& header = reader.GetHeader();
	int nbPoints = header.GetPointRecordsCount();

	//設定初始偏移量（這一步沒有的話會報錯，偏移量寫入輸出部分的檔案頭。保證兩份資料在相同座標體系下）
	double x_setoff = header.GetOffsetX();
	double y_setoff = header.GetOffsetY();
	double z_setoff = header.GetOffsetZ();

	//轉換為pcl格式
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr in_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	in_cloud->resize(nbPoints);

	int i = 0;
	while (reader.ReadNextPoint())
	{
		//座標資訊
		in_cloud->points[i].x = reader.GetPoint().GetX();
		in_cloud->points[i].y = reader.GetPoint().GetY();
		in_cloud->points[i].z = reader.GetPoint().GetZ();
		//顏色資訊
		uint16_t r1 = reader.GetPoint().GetColor().GetRed();
		uint16_t g1 = reader.GetPoint().GetColor().GetGreen();
		uint16_t b1 = reader.GetPoint().GetColor().GetBlue();
		uint32_t r2 = ceil(((float)r1 / 65536)*(float)256);
		uint32_t g2 = ceil(((float)g1 / 65536)*(float)256);
		uint32_t b2 = ceil(((float)b1 / 65536)*(float)256);
		uint32_t rgb = ((int)r2) << 16 | ((int)g2) << 8 | ((int)b2);
		in_cloud->points[i].rgb = *reinterpret_cast<float*>(&rgb);

		i++;
	}

	//自制取樣
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr filteredCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	//以0.1m為解析度進行體素化->尋找每個體素中，距離體素重心最近的點雲保留下來
	Subsample(in_cloud,filteredCloud);


	//寫入las檔案
	fname.replace(fname.find("in"), 2, "out");
	std::ofstream ofs = std::ofstream(fname, std::ios::out | std::ios::binary);

	   //設定檔案頭、點數、格式、縮放因子、偏移量
	liblas::Header f_header;
	f_header.SetVersionMajor(1);
	f_header.SetVersionMinor(2);
	f_header.SetDataFormatId(liblas::PointFormatName::ePointFormat3);
	f_header.SetOffset(x_setoff,y_setoff,z_setoff);   //這裡注意偏移量要從讀取部分的標頭檔案獲取
	f_header.SetScale(0.001, 0.001, 0.001);
	int out_p_n = filteredCloud->size();
	f_header.SetPointRecordsCount(out_p_n);
	liblas::Writer writer(ofs, f_header);
	liblas::Point point(&f_header);

	   //轉換為las
	for (size_t i = 0; i < filteredCloud->size(); ++i)
	{
		double x = filteredCloud->points[i].x;
		double y = filteredCloud->points[i].y;
		double z = filteredCloud->points[i].z;
		point.SetX(x);
		point.SetY(y);
		point.SetZ(z);
		uint32_t r = (uint32_t)filteredCloud->points[i].r;
		uint32_t g = (uint32_t)filteredCloud->points[i].g;
		uint32_t b = (uint32_t)filteredCloud->points[i].b;
		liblas::Color pointColor(r, g, b);
		point.SetColor(pointColor);
		writer.WritePoint(point);
	}

	writer.SetHeader(f_header);
	ofs.flush();
	ofs.close();
	std::cout << fname << "     " <<"finished!"<<std::endl;

	in_cloud->clear();
	filteredCloud->clear();

}

int main()
{

	測試
	//las2las("E:/temp/in/Tile_1321222320002302112.las");

	
	std::ifstream ifs;
	ifs.open("G:/Production_3 (2)_in/dir.txt");
	std::vector<std::string> dir;
	
	while (!ifs.eof())
	{
		std::string str;
		std::getline(ifs, str);
		dir.push_back(str);
	}
	ifs.close();
	//std::cout << dir.size() << std::endl;
	for (size_t i = 0; i < dir.size()-1; ++i)
	{
		las2las(dir[i]);
	}



	return EXIT_SUCCESS;

}