//C++
#include <iostream>
#include <io.h>
#include <direct.h>
#include <fstream>
#include <vector>
#include <string>
//PCL
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include<pcl/common/angles.h>
//EIGEN
#include <Eigen/core>
#include <Eigen/Dense>
//OPENCV
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
//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<double> yaw_table_a(rotation, 0);
vector<double> 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;
}
三维点云映射二维距离图像
于 2023-03-21 19:10:12 首次发布