（C++）激光点云数据BEV视图生成方法（点云bin文件转pcl处理）

bev视图根据激光数据的高度、密度和强度构成RGB。

密度的定义为:

z为竖直方向点云数量。

代码如下

#include <iostream>
#include <algorithm>
#include <fstream>
#include <chrono>
#include <unistd.h>
#include <opencv2/core/core.hpp>
#include <vector>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <opencv2/opencv.hpp>
#include <pcl/features/normal_3d.h>
#include <pcl/features/principal_curvatures.h>

pcl::PointCloud<pcl::PointXYZI>::Ptr BEV_GEN(const cv::String &strFile, cv::Mat &BEV_IMG)
{

    int32_t num = 1000000;
    float *data = (float *)malloc(num * sizeof(float)); // void *malloc(size_t size) 分配所需的内存空间，并返回一个指向它的指针。

    // pointers
    float *px = data + 0;
    float *py = data + 1;
    float *pz = data + 2;
    float *pr = data + 3;

    // load point cloud
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud(new pcl::PointCloud<pcl::PointXYZI>());
    FILE *stream;
    std::string Filename = strFile;
    char ch[200];
    strcpy(ch, Filename.c_str());

    stream = fopen(ch, "rb");
    num = fread(data, sizeof(float), num, stream) / 4;
    point_cloud->width = num;      //设定长
    point_cloud->height = 1;       //设定高
    point_cloud->is_dense = false; //如果没有无效点（例如，具有NaN或Inf值），则为True
    for (int32_t i = 0; i < num; i++)
    {
        // vector<int32_t> point_cloud;
        pcl::PointXYZI point;
        point.x = *px;
        point.y = *py;
        point.z = *pz;
        point.intensity = *pr;
        point_cloud->points.push_back(point);
        px += 4;
        py += 4;
        pz += 4;
        pr += 4;
    }
    fclose(stream);
    free(data); //释放内存
    pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_new(new pcl::PointCloud<pcl::PointXYZI>());
    for (int i = 0; i < point_cloud->points.size(); i++)
    {
        if (point_cloud->points[i].x > -2 * 50 && point_cloud->points[i].x < 2 * 50 && point_cloud->points[i].y > -2 * 25 && point_cloud->points[i].y < 2 * 25 && point_cloud->points[i].z > -0.8 && point_cloud->points[i].z < 1.3)
        {
            point_cloud_new->push_back(point_cloud->points[i]);
            // cout<<point_cloud_out->points[j].x<<endl;
            // j++;
        }
    }
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_Curvature(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::copyPointCloud(*point_cloud_new, *cloud_Curvature); // src中的xyz覆盖掉src_PN中的xyz值，然后把xyz+normal的信息给src_PN
    // MaxCurvaturePoints(point_cloud_new, cloud_Curvature);    //点云曲率
    // point_cloud_new->points.
    //可视化

    int Height = 2 * 608 + 1;
    int Width = 2 * (608 + 1) * 2;
    float Discretization = 50.0 / 608.0;

    // pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_new_ = *point_cloud_new;

    cv::Mat height_map = cv::Mat::zeros(2 * 608, 2 * 1218, CV_8UC1);
    cv::Mat intensityMap = cv::Mat::zeros(2 * 608, 2 * 1218, CV_8UC1);
    cv::Mat densityMap = cv::Mat::zeros(2 * 608, 2 * 1218, CV_8UC1);
    cv::Mat CurvatureMap = cv::Mat::zeros(2 * 608, 2 * 1218, CV_8UC1);

    vector<vector<vector<cv::Point3f>>> PointCloud_frac(2 * 608, vector<vector<cv::Point3f>>(2 * 1218, vector<cv::Point3f>()));

    for (int i = 0; i < point_cloud_new->points.size(); i++)
    {
        point_cloud_new->points[i].y = (floor(point_cloud_new->points[i].y / Discretization) + Height / 2);
        point_cloud_new->points[i].x = (floor(point_cloud_new->points[i].x / Discretization) + Width / 2);
        cloud_Curvature->points[i].y = (floor(cloud_Curvature->points[i].y / Discretization) + Height / 2);
        cloud_Curvature->points[i].x = (floor(cloud_Curvature->points[i].x / Discretization) + Width / 2);
        // cout<<"int(point_cloud_new->points[i].x  " <<int(point_cloud_new->points[i].x  << "int(point_cloud_new->points[i].y  " <<int(point_cloud_new->points[i].y <<endl;
        cv::Point3f p;
        p.x = point_cloud_new->points[i].z + 1.2;
        p.y = point_cloud_new->points[i].intensity;
        // p.z = cloud_Curvature->points[i].intensity; //曲率
        p.z = cloud_Curvature->points[i].intensity;
        PointCloud_frac[int(point_cloud_new->points[i].y)][int(point_cloud_new->points[i].x)].push_back(p);
    }

    // for(int i =0;i<608;i++)
    // {
    //     for(int j =0;j<1218;j++)
    //     // cout<<"poincloud _size  " << PointCloud_frac[i][j].size()<<endl;
    // }

    for (int i = 0; i < 2 * 608; i++)
    {
        for (int j = 0; j < 2 * 1218; j++)
        {
            if (PointCloud_frac[i][j].size() > 0)
            {
                vector<float> temp_h, temp_I, temp_c;
                for (int k = 0; k < PointCloud_frac[i][j].size(); k++)
                {
                    temp_h.push_back(PointCloud_frac[i][j][k].x);
                    temp_I.push_back(PointCloud_frac[i][j][k].y);
                    temp_c.push_back(PointCloud_frac[i][j][k].z);
                }
                height_map.at<uchar>(i, j) = (int)(*max_element(temp_h.begin(), temp_h.end()) / 2.1 * 255.0);
                intensityMap.at<uchar>(i, j) = (int)(*max_element(temp_I.begin(), temp_I.end()) * 255.0);
                densityMap.at<uchar>(i, j) = (int)(min(1.0, log10(temp_h.size() + 1) / log10(64)) * 255.0);
                CurvatureMap.at<uchar>(i, j) = (int)min(255.0, (10000 * (*max_element(temp_c.begin(), temp_c.end())) * 255.0));
            }
        }
    }
    vector<cv::Mat> vimg;
    vimg.push_back(intensityMap);
    vimg.push_back(height_map);
    vimg.push_back(densityMap);
    // vimg.push_back(intensityMap);
    // vimg.push_back(height_map);
    // vimg.push_back(CurvatureMap);

    cv::merge(vimg, BEV_IMG);
    return point_cloud;
}

int main()
{
    cv::String pattern = string(argv[3]);
    std::vector<cv::String> fn;
    glob(pattern, fn, false);
    int nImages = fn.size();
    cv::Mat imRGB;
    for (int ni = 0; ni < nImages; ni++)
    {
        pcl::PointCloud<pcl::PointXYZI>::Ptr tmp;  //保存的原始点云数据，pcl格式

        tmp = BEV_GEN(fn[ni], imRGB);    //imRGB为BEV视图
    }
}