From 61026b40b7d10ed2f9cfa9194f53d77f7764d12d Mon Sep 17 00:00:00 2001 From: Piotr Rybicki Date: Tue, 18 Oct 2022 13:00:31 +0200 Subject: [PATCH] Remove obsolete code (#38) --- include/Lidar.hpp | 71 ------------------- include/formatPCL.h | 18 ----- include/gaussianNoise.h | 27 ------- src/Lidar.cpp | 138 ------------------------------------ src/formatPCL.cu | 92 ------------------------ src/gaussianNoise.cu | 102 -------------------------- src/gaussianNoiseKernels.cu | 123 -------------------------------- 7 files changed, 571 deletions(-) delete mode 100644 include/Lidar.hpp delete mode 100644 include/formatPCL.h delete mode 100644 include/gaussianNoise.h delete mode 100644 src/Lidar.cpp delete mode 100644 src/formatPCL.cu delete mode 100644 src/gaussianNoise.cu delete mode 100644 src/gaussianNoiseKernels.cu diff --git a/include/Lidar.hpp b/include/Lidar.hpp deleted file mode 100644 index aa149ea6..00000000 --- a/include/Lidar.hpp +++ /dev/null @@ -1,71 +0,0 @@ -#pragma once - -#include "formatPCL.h" -#include "gaussianNoise.h" -#include - -#include -#include -#include -#include - -#include - -// Currently there is no explicit (documented) model of API / GPU synchronization -// Getting correct results is dependent on calling API calls in right order -// Failure to do so will likely result in segfaults / incorrect results -// TODO: fix this - -/** - * Lidar represents the following: - * - Description of a lidar model (list of ray transforms); a rough equivalent of the lidar's tech spec - * - Potential ray-casting and post-processing happening asynchronously (encapsulates a cudaStream) - * - Memory allocations required for the above - * - * In the future, the description part might be separated - * to avoid data duplication when multiple lidars of the same type are used. - */ -struct Lidar : APIObject -{ - Lidar(const Mat3x4f* rayPoses, int rayPosesCount); - - void setRingIds(const int* ringIds, size_t ringIdsCount); - void scheduleRaycast(std::shared_ptr scene); - int getResultsSize(); - void getResults(int format, void* data); - - Mat3x4f lidarPose = Mat3x4f::identity(); - Mat3x4f rosTransform{}; - float range; - -private: - cudaStream_t stream = nullptr; - std::optional currentJob; - std::optional densePointCount; - - // GPU INPUT - DeviceBuffer dCurrentJob; - DeviceBuffer dRayPoses; - DeviceBuffer dLidarArrayRingIds; - - // GPU OUTPUT - DeviceBuffer dWasHit; - DeviceBuffer dHitsBeforeIndex; - - DeviceBuffer dUnityVisualisationPoints; // Native output (for visualization in Unity) - DeviceBuffer dRosXYZ; // Native output (for publishing via ROS), contains non-hits - - // Buffers to be computed via formatPCLs - DeviceBuffer dDensePoint3f; - DeviceBuffer dDensePCL12; - DeviceBuffer dDensePCL24; - DeviceBuffer dDensePCL48; - - HostPinnedBuffer hDensePoint3f; - HostPinnedBuffer hDensePCL12; - HostPinnedBuffer hDensePCL24; - HostPinnedBuffer hDensePCL48; - - DeviceBuffer dRandomizationStates; - std::random_device randomDevice; -}; diff --git a/include/formatPCL.h b/include/formatPCL.h deleted file mode 100644 index 539ffd08..00000000 --- a/include/formatPCL.h +++ /dev/null @@ -1,18 +0,0 @@ -#pragma once - -#include -#include "data_types/PCLFormats.h" -#include "DeviceBuffer.hpp" - -void formatPCLsAsync( - cudaStream_t stream, - const DeviceBuffer& dWasHit, - DeviceBuffer& dHitsBeforeIndex, - const DeviceBuffer& dIn12, - const DeviceBuffer& dInPoint3f, - const DeviceBuffer& dInRingIds, - DeviceBuffer& dOutPoint3f, - DeviceBuffer& dOut12, - DeviceBuffer& dOut24, - DeviceBuffer& dOut48 -); diff --git a/include/gaussianNoise.h b/include/gaussianNoise.h deleted file mode 100644 index a56e68fc..00000000 --- a/include/gaussianNoise.h +++ /dev/null @@ -1,27 +0,0 @@ -#pragma once - -#include -#include - -#include "DeviceBuffer.hpp" -#include "data_types/PCLFormats.h" -#include "math/Mat3x4f.hpp" - -void setupGaussianNoiseGenerator(const unsigned seed, - cudaStream_t stream, - DeviceBuffer& dPHILOXStates); - -void addGaussianNoise(cudaStream_t stream, - const DeviceBuffer& dRayPoses, - const LidarNoiseParams& lidar_noise_params, - DeviceBuffer& dPHILOXStates, - DeviceBuffer& dRayPosesWithNoise); - -void addGaussianNoise(cudaStream_t stream, - const DeviceBuffer& dInputPointCloud, - const DeviceBuffer& dVisualizationPointCloud, - const Point3f visualization_point_cloud_origin, - const LidarNoiseParams& lidar_noise_params, - DeviceBuffer& dPHILOXStates, - DeviceBuffer& dResults, - DeviceBuffer& dVisualizationPointCloudWithNoise); diff --git a/src/Lidar.cpp b/src/Lidar.cpp deleted file mode 100644 index 0bf5ca7e..00000000 --- a/src/Lidar.cpp +++ /dev/null @@ -1,138 +0,0 @@ -#include - -#include -#include - -API_OBJECT_INSTANCE(Lidar); - -Lidar::Lidar(const Mat3x4f *rayPoses, int rayPosesCount) : range(std::numeric_limits::max()) -{ - CHECK_CUDA(cudaStreamCreate(&stream)); - - if (rayPosesCount <= 0) { - auto msg = fmt::format("LidarContext::LidarContext: rayPosesCount ({}) must be > 0", rayPosesCount); - throw std::logic_error(msg); - } - - dRayPoses.copyFromHost(rayPoses, rayPosesCount); - - // It should be possible to have no ring-ids, - // because user may be not interested in having them in the output format, however, - // current formatting hardcodes their usage; therefore, a default dummy value is set - // TODO: remove this once formatting code is improved - std::vector ringIds = {0}; - setRingIds(ringIds.data(), ringIds.size()); - - dUnityVisualisationPoints.resizeToFit(rayPosesCount); - dRosXYZ.resizeToFit(rayPosesCount); - dDensePoint3f.resizeToFit(rayPosesCount); - dDensePCL12.resizeToFit(rayPosesCount); - dDensePCL24.resizeToFit(rayPosesCount); - dDensePCL48.resizeToFit(rayPosesCount); - dWasHit.resizeToFit(rayPosesCount); - dHitsBeforeIndex.resizeToFit(rayPosesCount); - - dRandomizationStates.resizeToFit(rayPosesCount); - setupGaussianNoiseGenerator(randomDevice(), stream, dRandomizationStates); - - densePointCount = 0; -} - -void Lidar::setRingIds(const int *ringIds, size_t ringIdsCount) -{ - if (ringIdsCount <= 0) { - auto msg = fmt::format("LidarContext::LidarContext: lidarArrayRingCount ({}) must be > 0", ringIdsCount); - throw std::logic_error(msg); - } - dLidarArrayRingIds.copyFromHost(ringIds, ringIdsCount); -} - -void Lidar::scheduleRaycast(std::shared_ptr scene) -{ - densePointCount.reset(); - if (scene->getObjectCount() == 0) { - RGL_WARN("Requested raytracing on an empty scene"); - densePointCount = 0; - return; - } - auto sceneAS = scene->getAS(); - auto sceneSBT = scene->getSBT(); - currentJob = LaunchLidarParams{ - .range = range, - .rayCount = dRayPoses.getElemCount(), - .lidarPose = lidarPose, - .rosTransform = rosTransform, - .traversable = sceneAS, - .lidarNoiseParams = lidarNoiseParams, - .dRayPoses = dRayPoses.readDevice(), - .dUnityVisualisationPoints = dUnityVisualisationPoints.writeDevice(), - .dRosXYZ = dRosXYZ.writeDevice(), - .dWasHit = dWasHit.writeDevice(), - }; - dCurrentJob.copyFromHostAsync(¤tJob.value(), 1, stream); - - CUdeviceptr pipelineArgsPtr = dCurrentJob.readDeviceRaw(); - size_t pipelineArgsSize = dCurrentJob.getByteSize(); - dim3 launchDims = {static_cast(currentJob->rayCount), 1, 1}; - - addGaussianNoise(stream, dRayPoses, lidarNoiseParams, dRandomizationStates, dRayPoses); - CHECK_OPTIX(optixLaunch(Optix::instance().pipeline, stream, pipelineArgsPtr, pipelineArgsSize, &sceneSBT, launchDims.x, launchDims.y, launchDims.y)); - - Point3f lidar_origin_position = {lidarPose[3], lidarPose[7], lidarPose[11]}; - addGaussianNoise(stream, dRosXYZ, dUnityVisualisationPoints, lidar_origin_position, lidarNoiseParams, - dRandomizationStates, dRosXYZ, dUnityVisualisationPoints); - - formatPCLsAsync(stream, dWasHit, dHitsBeforeIndex, dRosXYZ, dUnityVisualisationPoints, dLidarArrayRingIds, - dDensePoint3f, dDensePCL12, dDensePCL24, dDensePCL48); - - hDensePoint3f.copyFromDeviceAsync(dDensePoint3f, stream); - hDensePCL12.copyFromDeviceAsync(dDensePCL12, stream); - hDensePCL24.copyFromDeviceAsync(dDensePCL24, stream); - hDensePCL48.copyFromDeviceAsync(dDensePCL48, stream); -} - -int Lidar::getResultsSize() -{ - if (!densePointCount.has_value()) { - densePointCount = -1; - // TODO: move this to scheduleRaycast - CHECK_CUDA(cudaMemcpyAsync(&densePointCount.value(), - dHitsBeforeIndex.readDevice() + dHitsBeforeIndex.getElemCount() - 1, - sizeof(densePointCount.value()), - cudaMemcpyDeviceToHost, - stream)); - CHECK_CUDA(cudaStreamSynchronize(stream)); - } - return densePointCount.value(); -} - -void Lidar::getResults(int format, void *data) -{ - CHECK_CUDA(cudaStreamSynchronize(stream)); - // densePointCount.has_value() guarantees that the stream has been synchronized - if (!densePointCount.has_value()) { - throw std::logic_error("getResults() has been called without a prior call to getResultsSize()"); - } - - if (densePointCount == 0) { - RGL_WARN("Returning an empty pointcloud"); - return; - } - - if (format == RGL_FORMAT_XYZ) { - memcpy(data, hDensePoint3f.readHost(), hDensePoint3f.getSizeOfElem() * densePointCount.value()); - return; - } - if (format == RGL_FORMAT_E2E_PCL12) { - memcpy(data, hDensePCL12.readHost(), hDensePCL12.getSizeOfElem() * densePointCount.value()); - return; - } - if (format == RGL_FORMAT_E2E_PCL24) { - memcpy(data, hDensePCL24.readHost(), hDensePCL24.getSizeOfElem() * densePointCount.value()); - return; - } - if (format == RGL_FORMAT_E2E_PCL48) { - memcpy(data, hDensePCL48.readHost(), hDensePCL48.getSizeOfElem() * densePointCount.value()); - return; - } -} diff --git a/src/formatPCL.cu b/src/formatPCL.cu deleted file mode 100644 index 8a574419..00000000 --- a/src/formatPCL.cu +++ /dev/null @@ -1,92 +0,0 @@ -#include - -#include "formatPCL.h" - -#include "HostPinnedBuffer.hpp" -#include -#include - -__global__ void kFormatAll(int sparsePointCount, - const int* wasHit, - const int* threadWriteIndex, - const PCL12* in12, - const Point3f* inPoint3f, - const int* inRingIds, - const int inRingCount, - Point3f* outPoint3f, - PCL12* out12, - PCL24* out24, - PCL48* out48) -{ - int rIdx = threadIdx.x + blockIdx.x * blockDim.x; - if (rIdx >= sparsePointCount) { - return; - } - if (!wasHit[rIdx]) { - return; - } - int wIdx = threadWriteIndex[rIdx] - 1; - - outPoint3f[wIdx] = inPoint3f[rIdx]; - out12[wIdx] = in12[rIdx]; - - const float x = in12[rIdx].x; - const float y = in12[rIdx].y; - const float z = in12[rIdx].z; - - out48[wIdx].x = out24[wIdx].x = x; - out48[wIdx].y = out24[wIdx].y = y; - out48[wIdx].z = out24[wIdx].z = z; - out48[wIdx].intensity = out24[wIdx].intensity = 100; - out48[wIdx].ring = out24[wIdx].ring = inRingIds[rIdx % inRingCount]; - - out48[wIdx].azimuth = 0; // TODO - out48[wIdx].distance = sqrt(x*x + y*y + z*z); - out48[wIdx].return_type = 0; // TODO - out48[wIdx].time_stamp = 0; // TODO -} - -void formatPCLsAsync(cudaStream_t stream, - const DeviceBuffer& dWasHit, - DeviceBuffer& dHitsBeforeIndex, - const DeviceBuffer& dIn12, - const DeviceBuffer& dInPoint3f, - const DeviceBuffer& dInRingIds, - DeviceBuffer& dOutPoint3f, - DeviceBuffer& dOut12, - DeviceBuffer& dOut24, - DeviceBuffer& dOut48 -) -{ - int sparsePointCount = dWasHit.getElemCount(); - - // First step: perform stream compaction - { - // beg and end could be used as const pointers, however thrust does not support it - auto beg = thrust::device_ptr(const_cast(dWasHit.readDevice())); - auto end = thrust::device_ptr(const_cast(dWasHit.readDevice()) + sparsePointCount); - auto dst = thrust::device_ptr(dHitsBeforeIndex.writeDevice()); - - // Note: this will compile only in a .cu file - thrust::inclusive_scan(thrust::cuda::par.on(stream), beg, end, dst); - } - - // Second step: format PCLs - { - kFormatAll<<<(sparsePointCount + 256) / 256, 256, 0, stream>>>( - sparsePointCount, - dWasHit.readDevice(), - dHitsBeforeIndex.readDevice(), - dIn12.readDevice(), - dInPoint3f.readDevice(), - dInRingIds.readDevice(), - dInRingIds.getElemCount(), - dOutPoint3f.writeDevice(), - dOut12.writeDevice(), - dOut24.writeDevice(), - dOut48.writeDevice() - ); - } - - -} \ No newline at end of file diff --git a/src/gaussianNoise.cu b/src/gaussianNoise.cu deleted file mode 100644 index fd81f8dc..00000000 --- a/src/gaussianNoise.cu +++ /dev/null @@ -1,102 +0,0 @@ -#include -#include - -#include "data_types/PCLFormats.h" -#include "data_types/LidarNoiseParams.h" -#define HOSTDEVICE __device__ -#include "linearGeometry.h" - -template -__device__ T rotatePointAroundY(const T& point, float angle_rad) { - return T { cos(angle_rad) * point.x - sin(angle_rad) * point.z, - point.y, - cos(angle_rad) * point.z + sin(angle_rad) * point.x }; -} - -template -__device__ float length(const T& vector) { - return sqrt(vector.x*vector.x + vector.y*vector.y + vector.z*vector.z); -} - -template -__device__ T scale(const T& point, float scale) { - return {point.x * scale, point.y * scale, point.z * scale}; -} - -template -__device__ T normalized(const T& point) { - return scale(point, 1.0f/length(point)); -} - -template -__device__ T addDistanceNoise(const T& point, float distance_error) { - T point_normalized = normalized(point); - - float distance = length(point); - float new_distance = distance + distance_error; - - return scale(point_normalized, new_distance); -} - -__device__ Mat3x4f addAngularGaussianNoise(const Mat3x4f& transform_to_rotate, - const LidarNoiseParams& lidar_noise_params, - curandStatePhilox4_32_10_t& randomization_state) { - float angle = - lidar_noise_params.angularNoiseMean + curand_normal(&randomization_state) * lidar_noise_params.angularNoiseStDev; - - return multiply3x4TransformMatrices(yAxisRotation3x4Matrix(angle), transform_to_rotate); -} - -__device__ void addAngularGaussianNoise(const PCL12& input_point, - const Point3f& input_visualization_point, - const Point3f& visualization_point_cloud_origin, - const LidarNoiseParams& lidar_noise_params, - curandStatePhilox4_32_10_t& randomization_state, - PCL12& result_point, - Point3f& result_visualization_point) { - float angular_offset = - lidar_noise_params.angularNoiseMean + curand_normal(&randomization_state) * lidar_noise_params.angularNoiseStDev; - - result_point = rotatePointAroundY(input_point, angular_offset); - - Point3f point = { - input_visualization_point.x - visualization_point_cloud_origin.x, - input_visualization_point.y - visualization_point_cloud_origin.y, - input_visualization_point.z - visualization_point_cloud_origin.z - }; - - Point3f point_with_noise = rotatePointAroundY(point, angular_offset); - - result_visualization_point.x = visualization_point_cloud_origin.x + point_with_noise.x; - result_visualization_point.y = visualization_point_cloud_origin.y + point_with_noise.y; - result_visualization_point.z = visualization_point_cloud_origin.z + point_with_noise.z; -} -__device__ void addDistanceGaussianNoise(const PCL12& input_point, - const Point3f& input_visualization_point, - const Point3f& visualization_point_cloud_origin, - const LidarNoiseParams& lidar_noise_params, - curandStatePhilox4_32_10_t& randomization_state, - PCL12& result_point, - Point3f& result_visualization_point) { - float distance = length(input_point); - float distance_induced_st_dev = distance * lidar_noise_params.distanceNoiseStDevRisePerMeter; - float total_st_dev = distance_induced_st_dev + lidar_noise_params.distanceNoiseStDevBase; - - float distance_error = - lidar_noise_params.distanceNoiseMean + curand_normal(&randomization_state) * total_st_dev; - - result_point = addDistanceNoise(input_point, distance_error); - - Point3f point = { - input_visualization_point.x - visualization_point_cloud_origin.x, - input_visualization_point.y - visualization_point_cloud_origin.y, - input_visualization_point.z - visualization_point_cloud_origin.z - }; - - Point3f point_with_noise = addDistanceNoise(point, distance_error); - - result_visualization_point.x = visualization_point_cloud_origin.x + point_with_noise.x; - result_visualization_point.y = visualization_point_cloud_origin.y + point_with_noise.y; - result_visualization_point.z = visualization_point_cloud_origin.z + point_with_noise.z; -} - diff --git a/src/gaussianNoiseKernels.cu b/src/gaussianNoiseKernels.cu deleted file mode 100644 index 79591b8f..00000000 --- a/src/gaussianNoiseKernels.cu +++ /dev/null @@ -1,123 +0,0 @@ -#include "gaussianNoise.h" - -#include -#include - -#include "gaussianNoise.cu" - -// Philox algorithm chosen based on performance -// https://stackoverflow.com/questions/18506697/curand-properties-of-generators - -__global__ void kSetupGaussianNoiseGenerator(unsigned int seed, int point_cloud_size, curandStatePhilox4_32_10_t *state) -{ - int id = threadIdx.x + blockIdx.x * blockDim.x; - if (id >= point_cloud_size) { - return; - } - /* Each thread gets same seed, a different sequence - number, no offset */ - curand_init(seed, id, 0, &state[id]); -} - - -__global__ void kAddGaussianNoise(int point_cloud_size, - const PCL12* input_point_cloud, - const Point3f* input_visualization_point_cloud, - const Point3f visualization_point_cloud_origin, - const LidarNoiseParams lidar_noise_params, - curandStatePhilox4_32_10_t *state, - PCL12 *result_point_cloud, - Point3f* result_visualization_point_cloud) -{ - const int id = threadIdx.x + blockIdx.x * blockDim.x; - if (id >= point_cloud_size) { - return; - } - - if (lidar_noise_params.angularNoiseType == rgl_angular_noise_type_t::RGL_ANGULAR_NOISE_TYPE_HITPOINT_BASED) { - addAngularGaussianNoise(input_point_cloud[id], - input_visualization_point_cloud[id], - visualization_point_cloud_origin, - lidar_noise_params, - state[id], - result_point_cloud[id], - result_visualization_point_cloud[id]); - } - - addDistanceGaussianNoise(result_point_cloud[id], - result_visualization_point_cloud[id], - visualization_point_cloud_origin, - lidar_noise_params, - state[id], - result_point_cloud[id], - result_visualization_point_cloud[id]); -} - -__global__ void kAddGaussianNoise(int point_cloud_size, - const Mat3x4f* ray_poses, - LidarNoiseParams lidar_noise_params, - curandStatePhilox4_32_10_t* state, - Mat3x4f *ray_poses_with_noise) { - const int id = threadIdx.x + blockIdx.x * blockDim.x; - if (id >= point_cloud_size) { - return; - } - if (lidar_noise_params.angularNoiseType == rgl_angular_noise_type_t::RGL_ANGULAR_NOISE_TYPE_RAY_BASED) { - ray_poses_with_noise[id] = addAngularGaussianNoise(ray_poses[id], lidar_noise_params, - state[id]); - } -} - -void setupGaussianNoiseGenerator(unsigned int seed, - cudaStream_t stream, - DeviceBuffer& dPHILOXStates) -{ - const int cloud_size = dPHILOXStates.getElemCount(); - const int thread_per_block_count = 256; - const int block_count = (cloud_size + thread_per_block_count) / thread_per_block_count; - - kSetupGaussianNoiseGenerator<<>>(seed, cloud_size, dPHILOXStates.writeDevice()); -} - - -void addGaussianNoise(cudaStream_t stream, - const DeviceBuffer& dRayPoses, - const LidarNoiseParams& lidar_noise_params, - DeviceBuffer& dPHILOXStates, - DeviceBuffer& dRayPosesWithNoise) { - - const int cloud_size = dRayPoses.getElemCount(); - const int thread_per_block_count = 256; - const int block_count = (cloud_size + thread_per_block_count) / thread_per_block_count; - - kAddGaussianNoise<<>>( - cloud_size, - dRayPoses.readDevice(), - lidar_noise_params, - dPHILOXStates.writeDevice(), - dRayPosesWithNoise.writeDevice()); -} - -void addGaussianNoise(cudaStream_t stream, - const DeviceBuffer& dInputPointCloud, - const DeviceBuffer& dVisualizationPointCloud, - const Point3f visualization_point_cloud_origin, - const LidarNoiseParams& lidar_noise_params, - DeviceBuffer& dPHILOXStates, - DeviceBuffer& dResults, - DeviceBuffer& dVisualizationPointCloudWithNoise) { - - const int cloud_size = dInputPointCloud.getElemCount(); - const int thread_per_block_count = 256; - const int block_count = (cloud_size + thread_per_block_count) / thread_per_block_count; - - kAddGaussianNoise<<>>( - cloud_size, - dInputPointCloud.readDevice(), - dVisualizationPointCloud.readDevice(), - visualization_point_cloud_origin, - lidar_noise_params, - dPHILOXStates.writeDevice(), - dResults.writeDevice(), - dVisualizationPointCloudWithNoise.writeDevice()); -}