三维点云映射二维距离图像

//C++
#include 
#include 
#include 
#include 
#include 
#include 
//PCL
#include 
#include 
#include 
#include
//EIGEN
#include 
#include 
//OPENCV
#include 
#include 
#include 
//namespace
using namespace std;
using namespace Eigen;
using namespace cv;
//define global variable
#define laser 64
#define rotation 2088
#define distance_resolution 0.002f

void readTXT(vector<float>& vec, string path)
{
	ifstream txt(path);
	if (!txt) {
		cout << "ERROR" << endl;
		return;
	}
	for (int i = 0; i < vec.size(); i++)
	{
		txt >> vec[i];
	}
	txt.close();
}
int findPosition(const vector<float>& vec, const float& val)
{
	int found = 0;
	if (vec.front() < vec.back()) {
		found = upper_bound(vec.begin(), vec.end(), val) - vec.begin();
	}
	else {
		found = vec.rend() - upper_bound(vec.rbegin(), vec.rend(), val);
	}
	if (found == 0) {
		return found;
	}
	if (found == vec.size()) {
		return found - 1;
	}
	auto diff_next = fabs(vec[found] - val);
	auto diff_prev = fabs(val - vec[found - 1]);
	return diff_next < diff_prev ? found : found - 1;
}
MatrixXf repairDepth(MatrixXf &depth_image, const int step, const float depth_threshold)
{
	MatrixXf inpainted_depth = depth_image;
	for (int c = 0; c < rotation; ++c)
	{
		for (int r = 0; r < laser; ++r)
		{
			float& curr_depth = inpainted_depth(r, c);
			if (curr_depth < 0.001f) {
				int counter = 0;
				float sum = 0.0f;
				for (int i = 1; i < step; ++i) {
					if (r - i < 0) {
						continue;
					}
					for (int j = 1; j < step; ++j) {
						if (r + j > laser - 1) {
							continue;
						}
						const float& prev = inpainted_depth(r - i, c);
						const float& next = inpainted_depth(r + j, c);
						if (prev > 0.001f && next > 0.001f &&
							fabs(prev - next) < depth_threshold) {
							sum += prev + next;
							counter += 2;
						}
					}
				}
				if (counter > 0) {
					curr_depth = sum / counter;
				}
			}
		}
	}
	return inpainted_depth;
}

int main(){
	string plypath = "0000000001p.ply";
	string imgpath = "0000000001p.png";
	pcl::PointCloud<pcl::PointXYZI> cloud;
	pcl::io::loadPLYFile(plypath, cloud);
	MatrixXf RangeImage = MatrixXf::Zero(laser, rotation);
	vector<float> pitch_table(laser, 0);
	vector<float> yaw_table(rotation, 0);
	readTXT(pitch_table, "pitch.txt");
	readTXT(yaw_table, "yaw.txt");
	//自己计算俯仰角与偏航角
	/*
	vector yaw_table_a(rotation, 0);
	vector pitch_table_a(laser, 0);
	yaw_table_a[0] = 0;//设置起始角度
	pitch_table_a[0] = 2;
	for (int i = 1; i < laser; i++)
	{
		pitch_table_a[i] = pitch_table_a[i - 1] - 0.41875;
	}
	for (int i = 0; i < laser; i++)
	{

		pitch_table[i] = pcl::deg2rad(pitch_table_a[i]);
	}
	for (int i = 1; i < rotation; i++)
	{
		yaw_table_a[i] = yaw_table_a[i - 1] + 0.2;
	}

	for (int i = 0; i < rotation; i++)
	{
		yaw_table[i] = pcl::deg2rad(yaw_table_a[i]);
	}
	*/
	
	for (int i = 0; i < cloud.points.size(); i++) {
		//Make sure the resolution is meters
		float x = cloud.points[i].x/1000;
		float y = cloud.points[i].y/1000;
		float z = cloud.points[i].z/1000;
		float r = sqrt((x * x) + (y * y) + (z * z));

		if (r < 0.001f) continue;
		float pitch = asin(z / r);
		float yaw = atan2(x, y);

		int nx = findPosition(pitch_table, pitch);
		int ny = findPosition(yaw_table, yaw);
		auto image_depth = RangeImage(nx, ny);
		if (image_depth < r) {
			RangeImage(nx, ny) = r;
		}
	}
	//generate rangeimage
	MatrixXf repair_depth = repairDepth(RangeImage, 5, 1.0f);
	repair_depth /= distance_resolution;
	Matrix<uint16_t, Dynamic, Dynamic> RangeImage_return = repair_depth.cast<uint16_t>();
	//save to png image
	vector<int> compression_params;
	compression_params.push_back(IMWRITE_PNG_COMPRESSION);
	compression_params.push_back(9);
	cv::Mat image_mat(laser, rotation, CV_16U);
	cv::eigen2cv(RangeImage_return, image_mat);
	cv::imwrite(imgpath, image_mat, compression_params);
 
	return 0;
}