C++:激光点云bev预处理代码实现

点云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()
{
    // checkCudaErrors(cudaFree(mask_));

    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];
        // float point_r = sqrt(point.x * point.x + point.y * point.y + point.z * point.z) * point.w;
        // std::cerr << i << ": "<< point.x << " " << point.y << " " << point.z << " " << point.w << " "<< point_r << std::endl;
        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;
            // std::cerr << nnn << ": "<< point.z << " "<< point.w << " " << point_r << " " << voxel_idx << " " << voxel_idy << std::endl;

            // unsigned int voxel_index = voxel_idy * proj_x_size + voxel_idx;            
            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);
            // std::cerr << nnn << ": "<< point.z << " "<< point.w << " " << point_r << " " << voxel_idx << " " << voxel_idy << " " << voxel_index << std::endl;          

            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;
        }
        
    }

}