From dbc23e7df70477eea47eb0c3b86611cc6ba7670d Mon Sep 17 00:00:00 2001 From: Tom Schammo Date: Thu, 15 Feb 2024 12:23:06 +0100 Subject: [PATCH] feat: Add `fog` implementation and adjust `fog` header Add prelimiary implementation for `fog` simulation. However there are a few things that need to be reviewed. This implementation probably has a lot of problems. --- LidarAug/cpp/include/weather.hpp | 7 +- LidarAug/cpp/src/weather.cpp | 127 +++++++++++++++++++++++++++++++ 2 files changed, 131 insertions(+), 3 deletions(-) create mode 100644 LidarAug/cpp/src/weather.cpp diff --git a/LidarAug/cpp/include/weather.hpp b/LidarAug/cpp/include/weather.hpp index dd1f996..22c822f 100644 --- a/LidarAug/cpp/include/weather.hpp +++ b/LidarAug/cpp/include/weather.hpp @@ -2,12 +2,13 @@ #ifndef WEATHER_HPP #define WEATHER_HPP -#include +#include #include typedef enum { DIST, CHAMFER } fog_metric; -void fog(at::Tensor point_cloud, std::size_t prob, fog_metric metric, - float sigma, int mean); +[[nodiscard]] std::optional> +fog(const torch::Tensor &point_cloud, float prob, fog_metric metric, + float sigma, int mean); #endif // !WEATHER_HPP diff --git a/LidarAug/cpp/src/weather.cpp b/LidarAug/cpp/src/weather.cpp new file mode 100644 index 0000000..dffdfe4 --- /dev/null +++ b/LidarAug/cpp/src/weather.cpp @@ -0,0 +1,127 @@ + +#include "../include/weather.hpp" +#include "../include/stats.hpp" +#include "../include/tensor.hpp" +#include +#include +#include +#include +#include + +using namespace torch::indexing; + +[[nodiscard]] inline torch::Tensor +select_points(const torch::Tensor &point_cloud, tensor_size_t num_items, + float viewing_dist, float extinction_factor, + float delete_probability, float beta, + std::uniform_real_distribution &percentage_distrib) { + + const auto dist = point_cloud.index({Slice(), Slice(None, 3)}).pow(2).sqrt(); + const auto modify_probability = 1 - exp(-extinction_factor * dist); + + const auto threshold_data_size = modify_probability.size(0); + + // NOTE(tom): `from_blob` only provides a view into the vector, so the + // vector has to outlive the tensor + const auto modify_threshold = + torch::from_blob(std::get(draw_values(percentage_distrib, + threshold_data_size)) + .data(), + {threshold_data_size}, torch::kF32); + + const auto selected = modify_threshold < modify_probability; + + // NOTE(tom): `from_blob` only provides a view into the vector, so the + // vector has to outlive the tensor + const auto delete_threshold = torch::from_blob( + std::get(draw_values(percentage_distrib, num_items)) + .data(), + {num_items}, torch::kF32); + + const auto deleted = + selected.logical_and(delete_threshold < delete_probability); + + // changing intensity of unaltered points according to beer lambert law + point_cloud.index({selected.logical_not(), 3}) *= + exp(-(2.99573 / viewing_dist) * 2 * dist[selected.logical_not()]); + + const auto altered_points = selected.logical_and(deleted.logical_not()); + const auto num_altered_points = + point_cloud.index({altered_points, Slice(None, 3)}).item(); + + if (num_altered_points > 0) { + std::exponential_distribution<> exp_d(beta); + const auto new_dist = + torch::from_blob( + std::get(draw_values(exp_d, num_altered_points)) + .data(), + {num_altered_points}, torch::kF32) + + 1.3; + point_cloud.index({altered_points, Slice(None, 3)}) *= + (new_dist / dist.index({altered_points})).reshape({-1, 1}); + } + + std::uniform_real_distribution d(0, 82); + + // TODO(tom): This needs review! + point_cloud.index({altered_points, 3}) = torch::from_blob( + std::get( + draw_values(d, static_cast(num_altered_points))) + .data(), + {num_altered_points}, torch::kF32); + + return point_cloud.index({deleted.logical_not(), Slice()}); +} + +[[nodiscard]] std::optional> +fog(const torch::Tensor &point_cloud, float prob, fog_metric metric, + float sigma, int mean) { + + auto rng = get_rng(); + std::uniform_real_distribution distrib(0, HUNDRED_PERCENT - 1); + auto rand = distrib(rng); + + if (prob > rand) { + + auto viewing_dist = get_truncated_normal_value(mean, sigma, 10, mean); + + const auto calculate_factors = + [metric, viewing_dist]() -> std::tuple { + switch (metric) { + case DIST: { + const float extinction_factor = 0.32 * exp(-0.022 * viewing_dist); + const float beta = (-0.00846 * viewing_dist) + 2.29; + const float delete_probability = -0.63 * exp(-0.02 * viewing_dist) + 1; + return {extinction_factor, beta, delete_probability}; + } + case CHAMFER: { + const float extinction_factor = 0.23 * exp(-0.0082 * viewing_dist); + const float beta = (-0.006 * viewing_dist) + 2.31; + const float delete_probability = -0.7 * exp(-0.024 * viewing_dist) + 1; + return {extinction_factor, beta, delete_probability}; + } + } + }; + + const auto [extinction_factor, beta, delete_probability] = + calculate_factors(); + + const dimensions pc_dims = {point_cloud.size(0), point_cloud.size(1), + point_cloud.size(2)}; + + torch::List batch; + batch.reserve(static_cast(pc_dims.batch_size)); + for (tensor_size_t i = 0; i < pc_dims.batch_size; i++) { + auto new_pc = + select_points(point_cloud.index({i}), pc_dims.num_items, viewing_dist, + extinction_factor, delete_probability, beta, distrib); + + batch.emplace_back(new_pc); + } + + return batch; + } else { + // NOTE(tom): prob <= rand + return std::nullopt; + } +}