点云bev预处理
#ifndef PREPROCESS_H_
#define PREPROCESS_H_
#include "kernel.h"
class PreProcessCuda
{
private:
Params params_;
cudaStream_t stream_ = 0;
unsigned int *mask_;
public:
PreProcessCuda();
~PreProcessCuda();
void generateBevProjection(float *points, size_t points_size, float *bevimage);
};
#endif
#include "preprocess.h"
#include <assert.h>
#include <iostream>
#include <math.h>
PreProcessCuda::PreProcessCuda()
{
return;
}
PreProcessCuda::~PreProcessCuda()
{
return;
}
void PreProcessCuda::generateBevProjection(float *points, size_t points_size, float *bevImage)
{
float min_x_range = params_.min_x_range;
float max_x_range = params_.max_x_range;
float min_y_range = params_.min_y_range;
float max_y_range = params_.max_y_range;
float min_z_range = params_.min_z_range;
float max_z_range = params_.max_z_range;
float voxel_x_size = params_.voxel_x_size;
float voxel_y_size = params_.voxel_y_size;
int proj_x_size = params_.grid_x_size;
int proj_y_size = params_.grid_y_size;
float z_value_min = params_.z_value_min;
float z_value_max = params_.z_value_max;
int i_value_min = params_.i_value_min;
int i_value_max = params_.i_value_max;
int r_value_min = params_.r_value_min;
int r_value_max = params_.r_value_max;
int nnn = 0;
for (size_t i = 0; i < points_size; ++i)
{
float4 point = ((float4*)points)[i];
if (point.x > min_x_range && point.x < max_x_range &&
point.y > min_y_range && point.y < max_y_range &&
point.z > min_z_range && point.z < max_z_range)
{
float point_r = sqrt(point.x * point.x + point.y * point.y + point.z * point.z) * point.w;
int voxel_idx = floorf((point.x - min_x_range) / voxel_x_size);
int voxel_idy = floorf((point.y - min_y_range) / voxel_y_size);
nnn +=1;
unsigned int voxel_index = voxel_idx * proj_x_size + voxel_idy;
if (point.z < z_value_min) point.z = z_value_min;
if (point.z > z_value_max) point.z = z_value_max;
point.z = (point.z - z_value_min)/(z_value_max - z_value_min);
if (point.w < i_value_min) point.w = i_value_min;
if (point.w > i_value_max) point.w = i_value_max;
point.w = (point.w - i_value_min)/(i_value_max - i_value_min);
if (point_r < r_value_min) point_r = r_value_min;
if (point_r > r_value_max) point_r = r_value_max;
point_r = (point_r - r_value_min)/(r_value_max - r_value_min);
bevImage[proj_x_size * proj_x_size * 0 + voxel_index] = point.z;
bevImage[proj_x_size * proj_x_size * 1 + voxel_index] = point.w;
bevImage[proj_x_size * proj_x_size * 2 + voxel_index] = point_r;
}
}
}