bev视图根据激光数据的高度、密度和强度构成RGB。
密度的定义为:
z为竖直方向点云数量。
代码如下
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
pcl::PointCloud::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::Ptr point_cloud(new pcl::PointCloud());
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 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::Ptr point_cloud_new(new pcl::PointCloud());
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<points[j].x<::Ptr cloud_Curvature(new pcl::PointCloud);
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::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>> PointCloud_frac(2 * 608, vector>(2 * 1218, vector()));
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 " <points[i].x << "int(point_cloud_new->points[i].y " <points[i].y <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()< 0)
{
vector 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(i, j) = (int)(*max_element(temp_h.begin(), temp_h.end()) / 2.1 * 255.0);
intensityMap.at(i, j) = (int)(*max_element(temp_I.begin(), temp_I.end()) * 255.0);
densityMap.at(i, j) = (int)(min(1.0, log10(temp_h.size() + 1) / log10(64)) * 255.0);
CurvatureMap.at(i, j) = (int)min(255.0, (10000 * (*max_element(temp_c.begin(), temp_c.end())) * 255.0));
}
}
}
vector 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 fn;
glob(pattern, fn, false);
int nImages = fn.size();
cv::Mat imRGB;
for (int ni = 0; ni < nImages; ni++)
{
pcl::PointCloud::Ptr tmp; //保存的原始点云数据,pcl格式
tmp = BEV_GEN(fn[ni], imRGB); //imRGB为BEV视图
}
}