From 7a5eafd7e98da0bef225f6c690db39f46bff224c Mon Sep 17 00:00:00 2001 From: Piotr Rybicki Date: Thu, 7 Mar 2024 12:47:40 +0100 Subject: [PATCH 01/16] Add power computation --- src/graph/NodesCore.hpp | 5 ++- src/graph/RadarPostprocessPointsNode.cpp | 46 +++++++++++++++++------- 2 files changed, 38 insertions(+), 13 deletions(-) diff --git a/src/graph/NodesCore.hpp b/src/graph/NodesCore.hpp index 4ad7c0c2f..6a13279fb 100644 --- a/src/graph/NodesCore.hpp +++ b/src/graph/NodesCore.hpp @@ -506,6 +506,10 @@ struct RadarPostprocessPointsNode : IPointsNodeSingleInput DeviceAsyncArray>>::create(arrayMgr); HostPinnedArray>>::Ptr outBUBRFactorHost = HostPinnedArray>>::create(); + std::vector::type> clusterRcsHost; + std::vector::type> clusterPowerHost; + std::vector::type> clusterNoiseHost; + std::vector::type> clusterSnrHost; float rayAzimuthStepRad; float rayElevationStepRad; @@ -530,7 +534,6 @@ struct RadarPostprocessPointsNode : IPointsNodeSingleInput Field::type findDirectionalCenterIndex(const Field::type* azimuths, const Field::type* elevations) const; - private: std::vector::type> indices; Vector<2, Field::type> minMaxDistance; Vector<2, Field::type> minMaxAzimuth; diff --git a/src/graph/RadarPostprocessPointsNode.cpp b/src/graph/RadarPostprocessPointsNode.cpp index 50dc56786..d6ccef6fa 100644 --- a/src/graph/RadarPostprocessPointsNode.cpp +++ b/src/graph/RadarPostprocessPointsNode.cpp @@ -18,6 +18,11 @@ #include #include +// For now, values are hardcoded for the target radar. Everything in SI units. +constexpr auto POWER_TRANSMITTER = std::numbers::pi_v; +constexpr auto ANTENNA_GAIN = 27.0f; +constexpr auto ANTENNA_EFFECTIVE_AREA = std::numbers::e_v; + inline static std::optional getRadarScopeWithinDistance(const std::vector& radarScopes, Field::type distance) { @@ -66,18 +71,6 @@ void RadarPostprocessPointsNode::enqueueExecImpl() outBUBRFactorHost->copyFrom(outBUBRFactorDev); CHECK_CUDA(cudaStreamSynchronize(getStreamHandle())); - // Computing RCS (example for all points) - // std::complex AU = 0; - // std::complex AR = 0; - // for (int hitIdx = 0; hitIdx < outBUBRFactorHost->getCount(); ++hitIdx) { - // std::complex BU = {outBUBRFactorHost->at(hitIdx)[0].real(), outBUBRFactorHost->at(hitIdx)[0].imag()}; - // std::complex BR = {outBUBRFactorHost->at(hitIdx)[1].real(), outBUBRFactorHost->at(hitIdx)[1].imag()}; - // std::complex factor = {outBUBRFactorHost->at(hitIdx)[2].real(), outBUBRFactorHost->at(hitIdx)[2].imag()}; - // AU += BU * factor; - // AR += BR * factor; - // } - // float rcs = 10.0f * log10f(4.0f * M_PIf * (pow(abs(AU), 2) + pow(abs(AR), 2))); - if (input->getPointCount() == 0) { filteredIndices->resize(0, false, false); return; @@ -140,6 +133,35 @@ void RadarPostprocessPointsNode::enqueueExecImpl() filteredIndices->copyFromExternal(filteredIndicesHost.data(), filteredIndicesHost.size()); + // Compute per-cluster properties + clusterRcsHost.resize(filteredIndicesHost.size()); + clusterPowerHost.resize(filteredIndicesHost.size()); + clusterNoiseHost.resize(filteredIndicesHost.size()); + clusterSnrHost.resize(filteredIndicesHost.size()); + for (int clusterIdx = 0; clusterIdx < filteredIndicesHost.size(); ++clusterIdx) { + std::complex AU = 0; + std::complex AR = 0; + auto&& cluster = clusters[clusterIdx]; + for (int pointInCluster = 0; pointInCluster < outBUBRFactorHost->getCount(); ++pointInCluster) { + std::complex BU = {outBUBRFactorHost->at(pointInCluster)[0].real(), + outBUBRFactorHost->at(pointInCluster)[0].imag()}; + std::complex BR = {outBUBRFactorHost->at(pointInCluster)[1].real(), + outBUBRFactorHost->at(pointInCluster)[1].imag()}; + std::complex factor = {outBUBRFactorHost->at(pointInCluster)[2].real(), + outBUBRFactorHost->at(pointInCluster)[2].imag()}; + AU += BU * factor; + AR += BR * factor; + } + clusterRcsHost[clusterIdx] = 10.0f * log10f(4.0f * M_PIf * (pow(abs(AU), 2) + pow(abs(AR), 2))); + auto distance = (cluster.minMaxDistance[0] + cluster.minMaxDistance[1]) / 2.0f; + auto sphereArea = 4.0f * std::numbers::pi_v * pow(distance, 2.0f); + // https://en.wikipedia.org/wiki/Radar_cross_section#Formulation + clusterPowerHost[clusterIdx] = POWER_TRANSMITTER * ANTENNA_GAIN * ANTENNA_EFFECTIVE_AREA * clusterRcsHost[clusterIdx] / + pow(sphereArea, 2.0f); + clusterNoiseHost[clusterIdx] = 1.0f; + clusterSnrHost[clusterIdx] = clusterPowerHost[clusterIdx] - clusterNoiseHost[clusterIdx]; + } + // getFieldData may be called in client's thread from rgl_graph_get_result_data // Doing job there would be: // - unexpected (job was supposed to be done asynchronously) From 550d99c31705077045ed38c765c8b84257114549 Mon Sep 17 00:00:00 2001 From: Piotr Rybicki Date: Thu, 7 Mar 2024 15:41:31 +0100 Subject: [PATCH 02/16] Fix static_asserts in core.h --- include/rgl/api/core.h | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h index 868ffad1b..c337cc678 100644 --- a/include/rgl/api/core.h +++ b/include/rgl/api/core.h @@ -18,6 +18,10 @@ #include #include +#ifdef __cplusplus +#include +#endif + #ifdef __cplusplus #define NO_MANGLING extern "C" #else // NOT __cplusplus @@ -61,7 +65,7 @@ typedef struct float value[2]; } rgl_vec2f; -#ifndef __cplusplus +#ifdef __cplusplus static_assert(sizeof(rgl_vec2f) == 2 * sizeof(float)); static_assert(std::is_trivial_v); static_assert(std::is_standard_layout_v); @@ -75,7 +79,7 @@ typedef struct float value[3]; } rgl_vec3f; -#ifndef __cplusplus +#ifdef __cplusplus static_assert(sizeof(rgl_vec3f) == 3 * sizeof(float)); static_assert(std::is_trivial_v); static_assert(std::is_standard_layout_v); @@ -89,7 +93,7 @@ typedef struct int32_t value[3]; } rgl_vec3i; -#ifndef __cplusplus +#ifdef __cplusplus static_assert(sizeof(rgl_vec3i) == 3 * sizeof(int32_t)); static_assert(std::is_trivial_v); static_assert(std::is_standard_layout_v); @@ -104,7 +108,7 @@ typedef struct float value[3][4]; } rgl_mat3x4f; -#ifndef __cplusplus +#ifdef __cplusplus static_assert(sizeof(rgl_mat3x4f) == 3 * 4 * sizeof(float)); static_assert(std::is_trivial_v); static_assert(std::is_standard_layout_v); @@ -137,10 +141,10 @@ typedef struct float azimuth_separation_threshold; } rgl_radar_scope_t; -#ifndef __cplusplus -static_assert(sizeof(rgl_radar_separations_t) == 5 * sizeof(float)); -static_assert(std::is_trivial_v); -static_assert(std::is_standard_layout_v); +#ifdef __cplusplus +static_assert(sizeof(rgl_radar_scope_t) == 5 * sizeof(float)); +static_assert(std::is_trivial_v); +static_assert(std::is_standard_layout_v); #endif /** From ded3701fe2295ce1ba0018f2324852101475809f Mon Sep 17 00:00:00 2001 From: Piotr Rybicki Date: Fri, 8 Mar 2024 15:09:05 +0100 Subject: [PATCH 03/16] Working prototype, need to fix values --- src/gpu/optixPrograms.cu | 4 +- src/graph/NodesCore.hpp | 14 ++- src/graph/RadarPostprocessPointsNode.cpp | 41 +++++--- test/CMakeLists.txt | 8 +- test/include/helpers/sceneHelpers.hpp | 5 +- test/src/scene/radarTest.cpp | 117 +++++++++++++++++++++++ 6 files changed, 167 insertions(+), 22 deletions(-) create mode 100644 test/src/scene/radarTest.cpp diff --git a/src/gpu/optixPrograms.cu b/src/gpu/optixPrograms.cu index fad61c9ff..c775b0b05 100644 --- a/src/gpu/optixPrograms.cu +++ b/src/gpu/optixPrograms.cu @@ -112,7 +112,7 @@ __forceinline__ __device__ void saveNonHitRayResult(float nonHitDistance) displacement = {isnan(displacement.x()) ? 0 : displacement.x(), isnan(displacement.y()) ? 0 : displacement.y(), isnan(displacement.z()) ? 0 : displacement.z()}; Vec3f xyz = origin + displacement; - saveRayResult(xyz, nonHitDistance, 0, RGL_ENTITY_INVALID_ID, Vec3f{NAN}, Vec3f{NAN}, NAN, Vec3f{NAN}, NAN); + saveRayResult(xyz, nonHitDistance, 0, RGL_ENTITY_INVALID_ID, Vec3f{NAN}, Vec3f{NAN}, 0.001f, Vec3f{NAN}, NAN); } extern "C" __global__ void __raygen__() @@ -228,7 +228,7 @@ extern "C" __global__ void __closesthit__() Vec3f absPointVelocity{NAN}; Vec3f relPointVelocity{NAN}; - float radialSpeed{NAN}; + float radialSpeed{0.014}; bool isVelocityRequested = ctx.pointAbsVelocity != nullptr || ctx.pointRelVelocity != nullptr || ctx.radialSpeed != nullptr; if (ctx.sceneDeltaTime > 0 && isVelocityRequested) { Vec3f displacementFromTransformChange = {0, 0, 0}; diff --git a/src/graph/NodesCore.hpp b/src/graph/NodesCore.hpp index 6a13279fb..a4d5c798c 100644 --- a/src/graph/NodesCore.hpp +++ b/src/graph/NodesCore.hpp @@ -506,10 +506,16 @@ struct RadarPostprocessPointsNode : IPointsNodeSingleInput DeviceAsyncArray>>::create(arrayMgr); HostPinnedArray>>::Ptr outBUBRFactorHost = HostPinnedArray>>::create(); - std::vector::type> clusterRcsHost; - std::vector::type> clusterPowerHost; - std::vector::type> clusterNoiseHost; - std::vector::type> clusterSnrHost; + + HostPageableArray::type>::Ptr clusterRcsHost = HostPageableArray::type>::create(); + HostPageableArray::type>::Ptr clusterPowerHost = HostPageableArray::type>::create(); + HostPageableArray::type>::Ptr clusterNoiseHost = HostPageableArray::type>::create(); + HostPageableArray::type>::Ptr clusterSnrHost = HostPageableArray::type>::create(); + + DeviceAsyncArray::type>::Ptr clusterRcsDev = DeviceAsyncArray::type>::create(arrayMgr); + DeviceAsyncArray::type>::Ptr clusterPowerDev = DeviceAsyncArray::type>::create(arrayMgr); + DeviceAsyncArray::type>::Ptr clusterNoiseDev = DeviceAsyncArray::type>::create(arrayMgr); + DeviceAsyncArray::type>::Ptr clusterSnrDev = DeviceAsyncArray::type>::create(arrayMgr); float rayAzimuthStepRad; float rayElevationStepRad; diff --git a/src/graph/RadarPostprocessPointsNode.cpp b/src/graph/RadarPostprocessPointsNode.cpp index d6ccef6fa..3a5709354 100644 --- a/src/graph/RadarPostprocessPointsNode.cpp +++ b/src/graph/RadarPostprocessPointsNode.cpp @@ -21,7 +21,7 @@ // For now, values are hardcoded for the target radar. Everything in SI units. constexpr auto POWER_TRANSMITTER = std::numbers::pi_v; constexpr auto ANTENNA_GAIN = 27.0f; -constexpr auto ANTENNA_EFFECTIVE_AREA = std::numbers::e_v; +constexpr auto ANTENNA_EFFECTIVE_AREA = std::numbers::e_v / 10'000; // A few cm2 inline static std::optional getRadarScopeWithinDistance(const std::vector& radarScopes, Field::type distance) @@ -134,10 +134,10 @@ void RadarPostprocessPointsNode::enqueueExecImpl() filteredIndices->copyFromExternal(filteredIndicesHost.data(), filteredIndicesHost.size()); // Compute per-cluster properties - clusterRcsHost.resize(filteredIndicesHost.size()); - clusterPowerHost.resize(filteredIndicesHost.size()); - clusterNoiseHost.resize(filteredIndicesHost.size()); - clusterSnrHost.resize(filteredIndicesHost.size()); + clusterRcsHost->resize(filteredIndicesHost.size(), false, false); + clusterPowerHost->resize(filteredIndicesHost.size(), false, false); + clusterNoiseHost->resize(filteredIndicesHost.size(), false, false); + clusterSnrHost->resize(filteredIndicesHost.size(), false, false); for (int clusterIdx = 0; clusterIdx < filteredIndicesHost.size(); ++clusterIdx) { std::complex AU = 0; std::complex AR = 0; @@ -152,15 +152,21 @@ void RadarPostprocessPointsNode::enqueueExecImpl() AU += BU * factor; AR += BR * factor; } - clusterRcsHost[clusterIdx] = 10.0f * log10f(4.0f * M_PIf * (pow(abs(AU), 2) + pow(abs(AR), 2))); + clusterRcsHost->at(clusterIdx) = 10.0f * log10f(4.0f * M_PIf * (pow(abs(AU), 2) + pow(abs(AR), 2))); + // clusterRcsHost->at(clusterIdx) /= 3.0f; // TODO auto distance = (cluster.minMaxDistance[0] + cluster.minMaxDistance[1]) / 2.0f; - auto sphereArea = 4.0f * std::numbers::pi_v * pow(distance, 2.0f); + auto sphereArea = 4.0f * std::numbers::pi_v * powf(distance, 2.0f); // https://en.wikipedia.org/wiki/Radar_cross_section#Formulation - clusterPowerHost[clusterIdx] = POWER_TRANSMITTER * ANTENNA_GAIN * ANTENNA_EFFECTIVE_AREA * clusterRcsHost[clusterIdx] / - pow(sphereArea, 2.0f); - clusterNoiseHost[clusterIdx] = 1.0f; - clusterSnrHost[clusterIdx] = clusterPowerHost[clusterIdx] - clusterNoiseHost[clusterIdx]; + clusterPowerHost->at(clusterIdx) = 10 * std::log10(ANTENNA_GAIN * ANTENNA_EFFECTIVE_AREA * + powf(10, clusterRcsHost->at(clusterIdx) / 10.0f) / + powf(sphereArea, 2.0f)); + clusterNoiseHost->at(clusterIdx) = 1.0f; + clusterSnrHost->at(clusterIdx) = clusterPowerHost->at(clusterIdx) - clusterNoiseHost->at(clusterIdx); } + clusterPowerDev->copyFrom(clusterPowerHost); + clusterRcsDev->copyFrom(clusterRcsHost); + clusterNoiseDev->copyFrom(clusterNoiseHost); + clusterSnrDev->copyFrom(clusterSnrHost); // getFieldData may be called in client's thread from rgl_graph_get_result_data // Doing job there would be: @@ -185,6 +191,19 @@ IAnyArray::ConstPtr RadarPostprocessPointsNode::getFieldData(rgl_field_t field) { std::lock_guard lock{getFieldDataMutex}; + if (field == POWER_F32) { + return clusterPowerDev->asAny(); + } + if (field == RCS_F32) { + return clusterRcsDev->asAny(); + } + if (field == NOISE_F32) { + return clusterNoiseDev->asAny(); + } + if (field == SNR_F32) { + return clusterSnrDev->asAny(); + } + if (!cacheManager.contains(field)) { auto fieldData = createArray(field, arrayMgr); fieldData->resize(filteredIndices->getCount(), false, false); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index f2fc131f9..776984634 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -55,7 +55,7 @@ if (RGL_BUILD_ROS2_EXTENSION) src/graph/Ros2NodesTest.cpp src/graph/nodes/Ros2PublishRadarScanNodeTest.cpp src/graph/nodes/Ros2PublishPointsNodeTest.cpp - src/scene/rcsAngleDistributionTest.cpp + src/scene/radarTest.cpp ) endif() @@ -74,10 +74,12 @@ if ((NOT WIN32) AND (NOT RGL_AUTO_TAPE_PATH)) ) endif() -file(COPY ${CMAKE_SOURCE_DIR}/test/data/ DESTINATION ${CMAKE_BINARY_DIR}/data/) - add_executable(RobotecGPULidar_test ${RGL_TEST_FILES}) +target_compile_definitions(RobotecGPULidar_test PRIVATE + RGL_TEST_DATA_DIR="${CMAKE_CURRENT_LIST_DIR}/data" +) + target_link_libraries(RobotecGPULidar_test PRIVATE gtest_main gmock_main diff --git a/test/include/helpers/sceneHelpers.hpp b/test/include/helpers/sceneHelpers.hpp index 6267ff1d8..e613f3d54 100644 --- a/test/include/helpers/sceneHelpers.hpp +++ b/test/include/helpers/sceneHelpers.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include @@ -54,13 +55,13 @@ static inline void setupBoxesAlongAxes() } } -static inline rgl_mesh_t loadFromSTL(const char* path) +static inline rgl_mesh_t loadFromSTL(std::filesystem::path path) { std::vector vertices, _normals; std::vector indices, _solids; std::vector rgl_vertices; std::vector rgl_indices; - stl_reader::ReadStlFile(path, vertices, _normals, indices, _solids); + stl_reader::ReadStlFile(path.c_str(), vertices, _normals, indices, _solids); assert(vertices.size() % 3 == 0); assert(indices.size() % 3 == 0); for (size_t i = 0; i < vertices.size(); i += 3) { diff --git a/test/src/scene/radarTest.cpp b/test/src/scene/radarTest.cpp new file mode 100644 index 000000000..3fafd31e6 --- /dev/null +++ b/test/src/scene/radarTest.cpp @@ -0,0 +1,117 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include "helpers/testPointCloud.hpp" + +struct RadarTest : RGLTest +{}; + +const auto minAzimuth = -65.0f; +const auto maxAzimuth = 65.0f; +const auto minElevation = -7.5f; +const auto maxElevation = 7.5f; +const auto azimuthStep = 0.49f; +const auto elevationStep = 0.49f; + +std::vector genRadarRays() +{ + std::vector rays; + for (auto a = minAzimuth; a <= maxAzimuth; a += azimuthStep) { + for (auto e = minElevation; e <= maxElevation; e += elevationStep) { + // By default, the rays are directed along the Z-axis + // So first, we rotate them around the Y-axis to point towards the X-axis (to be RVIZ2 compatible) + // Then, rotation around Z is azimuth, around Y is elevation + auto ray = Mat3x4f::rotationDeg(0, e + 90, -a); + rays.emplace_back(ray.toRGL()); + + // The above will have to be modified again - we assume that target is farther in X axis when in fact + // we use Z as RGL LiDAR front. Remember to update. + } + } + + return rays; +} + +namespace fs = std::filesystem; +using namespace std::chrono_literals; + +TEST_F(RadarTest, rotating_reflector_2d) +{ + // GTEST_SKIP(); + + // Setup sensor and graph + std::vector raysData = genRadarRays(); + rgl_radar_scope_t radarScope{ + .begin_distance = 1.3f, + .end_distance = 19.0f, + .distance_separation_threshold = 0.3f, + .radial_speed_separation_threshold = 0.3f, + .azimuth_separation_threshold = 8.0f * (std::numbers::pi_v / 180.0f), + }; + std::vector fields = {XYZ_VEC3_F32, DISTANCE_F32, RCS_F32, POWER_F32, NOISE_F32, SNR_F32}; + + // LiDAR + rgl_node_t rays = nullptr, noise = nullptr, raytrace = nullptr, compact = nullptr, lidarFormat = nullptr, + lidarPublish = nullptr; + EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&rays, raysData.data(), raysData.size())); + EXPECT_RGL_SUCCESS(rgl_node_gaussian_noise_angular_ray(&noise, 0, 2, RGL_AXIS_Z)); + EXPECT_RGL_SUCCESS(rgl_node_raytrace(&raytrace, nullptr)); + EXPECT_RGL_SUCCESS(rgl_node_points_compact_by_field(&compact, RGL_FIELD_IS_HIT_I32)); + EXPECT_RGL_SUCCESS(rgl_node_points_format(&lidarFormat, fields.data(), 1)); // Publish only XYZ + EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&lidarPublish, "rgl_lidar", "world")); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(rays, noise)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(noise, raytrace)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(raytrace, compact)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(compact, lidarFormat)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(lidarFormat, lidarPublish)); + + // Radar postprocessing and publishing + rgl_node_t radarPostProcess = nullptr, radarFormat = nullptr, radarPublish = nullptr; + EXPECT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarPostProcess, &radarScope, 1, azimuthStep, elevationStep, 79E9f)); + EXPECT_RGL_SUCCESS(rgl_node_points_format(&radarFormat, fields.data(), fields.size())); + EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&radarPublish, "rgl_radar", "world")); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(compact, radarPostProcess)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(radarPostProcess, radarFormat)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(radarFormat, radarPublish)); + + // Setup scene + std::vector reflectors2D; + reflectors2D.resize(3, nullptr); + rgl_mesh_t reflector2dMesh = loadFromSTL(fs::path(RGL_TEST_DATA_DIR) / "reflector2d.stl"); + for (auto&& reflector2D : reflectors2D) { + EXPECT_RGL_SUCCESS(rgl_entity_create(&reflector2D, nullptr, reflector2dMesh)); + } + + uint64_t time = 0; + for (float angle = -45; angle <= 45; angle += 0.1f) { + EXPECT_RGL_SUCCESS(rgl_scene_set_time(nullptr, time)); + auto position = Vec3f{5, 0, 0}; + auto rotation = Vec3f{0, 0, angle}; + auto scale = Vec3f{1, 1, 1}; + + auto offset = Vec3f{0, -3, 0}; + for (auto&& reflector2D : reflectors2D) { + rgl_mat3x4f reflectorPose = Mat3x4f::TRS(position + offset, rotation, scale).toRGL(); + EXPECT_RGL_SUCCESS(rgl_entity_set_pose(reflector2D, &reflectorPose)); + offset += Vec3f{0, 3, 0}; + } + + fmt::print("Angle: {}\n", angle); + EXPECT_RGL_SUCCESS(rgl_graph_run(rays)); + std::this_thread::sleep_for(100ms); + time += 10'000'000; // 10 ms + } +} From c4e2fa218a8bb78c2488cd7822341864451803b7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Liberadzki?= Date: Mon, 11 Mar 2024 16:50:08 +0100 Subject: [PATCH 04/16] Make updates to radar power calculation. Make minor fixes. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Liberadzki --- src/graph/RadarPostprocessPointsNode.cpp | 31 +++++++++++++++++------- 1 file changed, 22 insertions(+), 9 deletions(-) diff --git a/src/graph/RadarPostprocessPointsNode.cpp b/src/graph/RadarPostprocessPointsNode.cpp index 3a5709354..fb9f1fdbf 100644 --- a/src/graph/RadarPostprocessPointsNode.cpp +++ b/src/graph/RadarPostprocessPointsNode.cpp @@ -19,7 +19,7 @@ #include // For now, values are hardcoded for the target radar. Everything in SI units. -constexpr auto POWER_TRANSMITTER = std::numbers::pi_v; +constexpr auto POWER_TRANSMITTED = std::numbers::pi_v; constexpr auto ANTENNA_GAIN = 27.0f; constexpr auto ANTENNA_EFFECTIVE_AREA = std::numbers::e_v / 10'000; // A few cm2 @@ -133,6 +133,13 @@ void RadarPostprocessPointsNode::enqueueExecImpl() filteredIndices->copyFromExternal(filteredIndicesHost.data(), filteredIndicesHost.size()); + // Become radar parameters + const auto powerTransmittedDbm = 31.0f; // According to manual. + const auto antennaGainDbi = 27.0f; // both transmitter and receiver antenna. + + const auto lambda = 0.00379484; // TODO: Calculate from frequency. + const auto lambdaSqrtDbsm = 10.0f * log10f(lambda * lambda); + // Compute per-cluster properties clusterRcsHost->resize(filteredIndicesHost.size(), false, false); clusterPowerHost->resize(filteredIndicesHost.size(), false, false); @@ -142,7 +149,8 @@ void RadarPostprocessPointsNode::enqueueExecImpl() std::complex AU = 0; std::complex AR = 0; auto&& cluster = clusters[clusterIdx]; - for (int pointInCluster = 0; pointInCluster < outBUBRFactorHost->getCount(); ++pointInCluster) { + + for (const auto pointInCluster : cluster.indices) { std::complex BU = {outBUBRFactorHost->at(pointInCluster)[0].real(), outBUBRFactorHost->at(pointInCluster)[0].imag()}; std::complex BR = {outBUBRFactorHost->at(pointInCluster)[1].real(), @@ -152,14 +160,19 @@ void RadarPostprocessPointsNode::enqueueExecImpl() AU += BU * factor; AR += BR * factor; } - clusterRcsHost->at(clusterIdx) = 10.0f * log10f(4.0f * M_PIf * (pow(abs(AU), 2) + pow(abs(AR), 2))); - // clusterRcsHost->at(clusterIdx) /= 3.0f; // TODO - auto distance = (cluster.minMaxDistance[0] + cluster.minMaxDistance[1]) / 2.0f; - auto sphereArea = 4.0f * std::numbers::pi_v * powf(distance, 2.0f); + // https://en.wikipedia.org/wiki/Radar_cross_section#Formulation - clusterPowerHost->at(clusterIdx) = 10 * std::log10(ANTENNA_GAIN * ANTENNA_EFFECTIVE_AREA * - powf(10, clusterRcsHost->at(clusterIdx) / 10.0f) / - powf(sphereArea, 2.0f)); + + // TODO: Handle nans in RCS. + const auto rcsDbsm = 10.0f * log10f(4.0f * M_PIf * (pow(abs(AU), 2) + pow(abs(AR), 2))); + clusterRcsHost->at(clusterIdx) = rcsDbsm; + + const auto distance = distanceInputHost->at(filteredIndicesHost.at(clusterIdx)); + const auto multiplier = 10.0f * log10f(powf(4 * std::numbers::pi_v, 3)) + 10.0f * log10f(powf(distance, 4)); + + const auto outputPower = powerTransmittedDbm + antennaGainDbi + antennaGainDbi + rcsDbsm + lambdaSqrtDbsm - multiplier; + + clusterPowerHost->at(clusterIdx) = outputPower; clusterNoiseHost->at(clusterIdx) = 1.0f; clusterSnrHost->at(clusterIdx) = clusterPowerHost->at(clusterIdx) - clusterNoiseHost->at(clusterIdx); } From 6f825166fac0d8d5d24953844648d6e604a81e04 Mon Sep 17 00:00:00 2001 From: Piotr Rybicki Date: Mon, 11 Mar 2024 17:17:36 +0100 Subject: [PATCH 05/16] Add additional radar parameters --- include/rgl/api/core.h | 5 ++++- src/api/apiCore.cpp | 17 ++++++++++------ src/graph/NodesCore.hpp | 4 +++- src/graph/RadarPostprocessPointsNode.cpp | 20 +++++++------------ test/src/TapeTest.cpp | 2 +- .../nodes/RadarPostprocessPointsNodeTest.cpp | 3 ++- test/src/scene/radarTest.cpp | 3 ++- 7 files changed, 30 insertions(+), 24 deletions(-) diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h index c337cc678..2bb35b7ee 100644 --- a/include/rgl/api/core.h +++ b/include/rgl/api/core.h @@ -770,10 +770,13 @@ RGL_API rgl_status_t rgl_node_points_from_array(rgl_node_t* node, const void* po * @param ray_azimuth_step The azimuth step between rays (in radians). * @param ray_elevation_step The elevation step between rays (in radians). * @param frequency The frequency of the radar (in Hz). + * @param powerTransmittedDbm The power transmitted by the radar (in dBm). + * @param antennaGainDbi The gain of the radar's antenna (in dBi). */ RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, const rgl_radar_scope_t* radar_scopes, int32_t radar_scopes_count, float ray_azimuth_step, - float ray_elevation_step, float frequency); + float ray_elevation_step, float frequency, float powerTransmittedDbm, + float antennaGainDbi); /** * Creates or modifies FilterGroundPointsNode. diff --git a/src/api/apiCore.cpp b/src/api/apiCore.cpp index c4f27cba7..908c39303 100644 --- a/src/api/apiCore.cpp +++ b/src/api/apiCore.cpp @@ -1053,17 +1053,21 @@ void TapeCore::tape_node_points_from_array(const YAML::Node& yamlNode, PlaybackS RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, const rgl_radar_scope_t* radar_scopes, int32_t radar_scopes_count, float ray_azimuth_step, - float ray_elevation_step, float frequency) + float ray_elevation_step, float frequency, float powerTransmittedDbm, + float antennaGainDbi) { auto status = rglSafeCall([&]() { RGL_API_LOG("rgl_node_points_radar_postprocess(node={}, radar_scopes={}, ray_azimuth_step={}, ray_elevation_step={}, " - "frequency={})", - repr(node), repr(radar_scopes, radar_scopes_count), ray_azimuth_step, ray_elevation_step, frequency); + "frequency={}, powerTransmittedDbm={}, antennaGainDbi={})", + repr(node), repr(radar_scopes, radar_scopes_count), ray_azimuth_step, ray_elevation_step, frequency, + powerTransmittedDbm, antennaGainDbi); CHECK_ARG(radar_scopes != nullptr); CHECK_ARG(radar_scopes_count > 0); CHECK_ARG(ray_azimuth_step > 0); CHECK_ARG(ray_elevation_step > 0); CHECK_ARG(frequency > 0); + CHECK_ARG(powerTransmittedDbm > 0); + CHECK_ARG(antennaGainDbi > 0); for (int i = 0; i < radar_scopes_count; ++i) { CHECK_ARG(radar_scopes[i].begin_distance >= 0); @@ -1076,10 +1080,10 @@ RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, const r createOrUpdateNode( node, std::vector{radar_scopes, radar_scopes + radar_scopes_count}, ray_azimuth_step, - ray_elevation_step, frequency); + ray_elevation_step, frequency, powerTransmittedDbm, antennaGainDbi); }); TAPE_HOOK(node, TAPE_ARRAY(radar_scopes, radar_scopes_count), radar_scopes_count, ray_azimuth_step, ray_elevation_step, - frequency); + frequency, powerTransmittedDbm, antennaGainDbi); return status; } @@ -1088,7 +1092,8 @@ void TapeCore::tape_node_points_radar_postprocess(const YAML::Node& yamlNode, Pl auto nodeId = yamlNode[0].as(); rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; rgl_node_points_radar_postprocess(&node, state.getPtr(yamlNode[1]), yamlNode[2].as(), - yamlNode[3].as(), yamlNode[4].as(), yamlNode[5].as()); + yamlNode[3].as(), yamlNode[4].as(), yamlNode[5].as(), + yamlNode[6].as(), yamlNode[7].as()); state.nodes.insert({nodeId, node}); } diff --git a/src/graph/NodesCore.hpp b/src/graph/NodesCore.hpp index a4d5c798c..52f7b1aef 100644 --- a/src/graph/NodesCore.hpp +++ b/src/graph/NodesCore.hpp @@ -476,7 +476,7 @@ struct RadarPostprocessPointsNode : IPointsNodeSingleInput using Ptr = std::shared_ptr; void setParameters(const std::vector& radarScopes, float rayAzimuthStepRad, float rayElevationStepRad, - float frequency); + float frequency, float powerTransmittedDbm, float antennaGainDbi); // Node void validateImpl() override; @@ -520,6 +520,8 @@ struct RadarPostprocessPointsNode : IPointsNodeSingleInput float rayAzimuthStepRad; float rayElevationStepRad; float frequency; + float powerTransmittedDbm; + float antennaGainDbi; std::vector radarScopes; diff --git a/src/graph/RadarPostprocessPointsNode.cpp b/src/graph/RadarPostprocessPointsNode.cpp index fb9f1fdbf..6e62f83dc 100644 --- a/src/graph/RadarPostprocessPointsNode.cpp +++ b/src/graph/RadarPostprocessPointsNode.cpp @@ -18,11 +18,6 @@ #include #include -// For now, values are hardcoded for the target radar. Everything in SI units. -constexpr auto POWER_TRANSMITTED = std::numbers::pi_v; -constexpr auto ANTENNA_GAIN = 27.0f; -constexpr auto ANTENNA_EFFECTIVE_AREA = std::numbers::e_v / 10'000; // A few cm2 - inline static std::optional getRadarScopeWithinDistance(const std::vector& radarScopes, Field::type distance) { @@ -35,12 +30,15 @@ inline static std::optional getRadarScopeWithinDistance(const } void RadarPostprocessPointsNode::setParameters(const std::vector& radarScopes, float rayAzimuthStepRad, - float rayElevationStepRad, float frequency) + float rayElevationStepRad, float frequency, float powerTransmittedDbm, + float antennaGainDbi) { this->rayAzimuthStepRad = rayAzimuthStepRad; this->rayElevationStepRad = rayElevationStepRad; this->frequency = frequency; this->radarScopes = radarScopes; + this->powerTransmittedDbm = powerTransmittedDbm; + this->antennaGainDbi = antennaGainDbi; } void RadarPostprocessPointsNode::validateImpl() @@ -133,11 +131,7 @@ void RadarPostprocessPointsNode::enqueueExecImpl() filteredIndices->copyFromExternal(filteredIndicesHost.data(), filteredIndicesHost.size()); - // Become radar parameters - const auto powerTransmittedDbm = 31.0f; // According to manual. - const auto antennaGainDbi = 27.0f; // both transmitter and receiver antenna. - - const auto lambda = 0.00379484; // TODO: Calculate from frequency. + const auto lambda = 299'792'458.0f / frequency; const auto lambdaSqrtDbsm = 10.0f * log10f(lambda * lambda); // Compute per-cluster properties @@ -164,11 +158,11 @@ void RadarPostprocessPointsNode::enqueueExecImpl() // https://en.wikipedia.org/wiki/Radar_cross_section#Formulation // TODO: Handle nans in RCS. - const auto rcsDbsm = 10.0f * log10f(4.0f * M_PIf * (pow(abs(AU), 2) + pow(abs(AR), 2))); + const auto rcsDbsm = 10.0f * log10f(4.0f * M_PIf * (pow(abs(AU), 2) + pow(abs(AR), 2))); clusterRcsHost->at(clusterIdx) = rcsDbsm; const auto distance = distanceInputHost->at(filteredIndicesHost.at(clusterIdx)); - const auto multiplier = 10.0f * log10f(powf(4 * std::numbers::pi_v, 3)) + 10.0f * log10f(powf(distance, 4)); + const auto multiplier = 10.0f * log10f(powf(4 * std::numbers::pi_v, 3)) + 10.0f * log10f(powf(distance, 4)); const auto outputPower = powerTransmittedDbm + antennaGainDbi + antennaGainDbi + rcsDbsm + lambdaSqrtDbsm - multiplier; diff --git a/test/src/TapeTest.cpp b/test/src/TapeTest.cpp index f4b27ccd0..9a6ea38b5 100644 --- a/test/src/TapeTest.cpp +++ b/test/src/TapeTest.cpp @@ -265,7 +265,7 @@ TEST_F(TapeTest, RecordPlayAllCalls) rgl_node_t radarPostprocess = nullptr; rgl_radar_scope_t radarScope{0.0f, 1.0f, 1.0f, 1.0f, 1.0f}; - EXPECT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarPostprocess, &radarScope, 1, 1.0f, 1.0f, 79E9f)); + EXPECT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarPostprocess, &radarScope, 1, 1.0f, 1.0f, 79E9f, 31.0f, 27.0f)); rgl_node_t filterGround = nullptr; rgl_vec3f sensorUpVector = {0.0f, 1.0f, 0.0f}; diff --git a/test/src/graph/nodes/RadarPostprocessPointsNodeTest.cpp b/test/src/graph/nodes/RadarPostprocessPointsNodeTest.cpp index cd699ecc6..5181c9d97 100644 --- a/test/src/graph/nodes/RadarPostprocessPointsNodeTest.cpp +++ b/test/src/graph/nodes/RadarPostprocessPointsNodeTest.cpp @@ -26,7 +26,8 @@ TEST_F(RadarPostprocessPointsNodeTest, clustering_test) .radial_speed_separation_threshold = 1.0f, .azimuth_separation_threshold = 1.0f}, }; - ASSERT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarNode, radarScopes.data(), radarScopes.size(), 1.0f, 1.0f, 1.0f)); + ASSERT_RGL_SUCCESS( + rgl_node_points_radar_postprocess(&radarNode, radarScopes.data(), radarScopes.size(), 1.0f, 1.0f, 1.0f, 1.0f, 1.0f)); auto runAndReturnNumberOfClusters = [&radarNode](const TestPointCloud& inPointCloud) -> int32_t { rgl_node_t usePointsNode = inPointCloud.createUsePointsNode(); diff --git a/test/src/scene/radarTest.cpp b/test/src/scene/radarTest.cpp index 3fafd31e6..4f417d39b 100644 --- a/test/src/scene/radarTest.cpp +++ b/test/src/scene/radarTest.cpp @@ -80,7 +80,8 @@ TEST_F(RadarTest, rotating_reflector_2d) // Radar postprocessing and publishing rgl_node_t radarPostProcess = nullptr, radarFormat = nullptr, radarPublish = nullptr; - EXPECT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarPostProcess, &radarScope, 1, azimuthStep, elevationStep, 79E9f)); + EXPECT_RGL_SUCCESS( + rgl_node_points_radar_postprocess(&radarPostProcess, &radarScope, 1, azimuthStep, elevationStep, 79E9f, 31.0f, 27.0f)); EXPECT_RGL_SUCCESS(rgl_node_points_format(&radarFormat, fields.data(), fields.size())); EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&radarPublish, "rgl_radar", "world")); EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(compact, radarPostProcess)); From d4d46221a52a15c526bb639803eb0253b42d0ff8 Mon Sep 17 00:00:00 2001 From: Piotr Rybicki Date: Tue, 12 Mar 2024 11:51:45 +0100 Subject: [PATCH 06/16] Add noise params and RCS NaN check --- include/rgl/api/core.h | 5 +++- src/api/apiCore.cpp | 23 +++++++++++-------- src/graph/NodesCore.hpp | 7 +++++- src/graph/RadarPostprocessPointsNode.cpp | 14 +++++++---- test/src/TapeTest.cpp | 3 ++- .../nodes/RadarPostprocessPointsNodeTest.cpp | 4 ++-- test/src/scene/radarTest.cpp | 6 ++--- 7 files changed, 39 insertions(+), 23 deletions(-) diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h index 2bb35b7ee..bd0207122 100644 --- a/include/rgl/api/core.h +++ b/include/rgl/api/core.h @@ -772,11 +772,14 @@ RGL_API rgl_status_t rgl_node_points_from_array(rgl_node_t* node, const void* po * @param frequency The frequency of the radar (in Hz). * @param powerTransmittedDbm The power transmitted by the radar (in dBm). * @param antennaGainDbi The gain of the radar's antenna (in dBi). + * @param received_noise_mean_db The mean of the received noise (in dB). + * @param received_noise_std_dev_db The standard deviation of the received noise (in dB). */ RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, const rgl_radar_scope_t* radar_scopes, int32_t radar_scopes_count, float ray_azimuth_step, float ray_elevation_step, float frequency, float powerTransmittedDbm, - float antennaGainDbi); + float antennaGainDbi, float received_noise_mean_dbm, + float received_noise_st_dev_dbm); /** * Creates or modifies FilterGroundPointsNode. diff --git a/src/api/apiCore.cpp b/src/api/apiCore.cpp index 908c39303..fd8eb573d 100644 --- a/src/api/apiCore.cpp +++ b/src/api/apiCore.cpp @@ -1053,21 +1053,24 @@ void TapeCore::tape_node_points_from_array(const YAML::Node& yamlNode, PlaybackS RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, const rgl_radar_scope_t* radar_scopes, int32_t radar_scopes_count, float ray_azimuth_step, - float ray_elevation_step, float frequency, float powerTransmittedDbm, - float antennaGainDbi) + float ray_elevation_step, float frequency, float power_transmitted_dbm, + float antenna_gain_dbi, float received_noise_mean_dbm, + float received_noise_st_dev_dbm) { auto status = rglSafeCall([&]() { RGL_API_LOG("rgl_node_points_radar_postprocess(node={}, radar_scopes={}, ray_azimuth_step={}, ray_elevation_step={}, " - "frequency={}, powerTransmittedDbm={}, antennaGainDbi={})", + "frequency={}, power_transmitted_dbm={}, antenna_gain_dbi={}, received_noise_mean_db={}, " + "received_noise_std_dev_db={})", repr(node), repr(radar_scopes, radar_scopes_count), ray_azimuth_step, ray_elevation_step, frequency, - powerTransmittedDbm, antennaGainDbi); + power_transmitted_dbm, antenna_gain_dbi, received_noise_mean_dbm, received_noise_st_dev_dbm); CHECK_ARG(radar_scopes != nullptr); CHECK_ARG(radar_scopes_count > 0); CHECK_ARG(ray_azimuth_step > 0); CHECK_ARG(ray_elevation_step > 0); CHECK_ARG(frequency > 0); - CHECK_ARG(powerTransmittedDbm > 0); - CHECK_ARG(antennaGainDbi > 0); + CHECK_ARG(power_transmitted_dbm > 0); + CHECK_ARG(antenna_gain_dbi > 0); + CHECK_ARG(received_noise_st_dev_dbm > 0); for (int i = 0; i < radar_scopes_count; ++i) { CHECK_ARG(radar_scopes[i].begin_distance >= 0); @@ -1080,10 +1083,11 @@ RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, const r createOrUpdateNode( node, std::vector{radar_scopes, radar_scopes + radar_scopes_count}, ray_azimuth_step, - ray_elevation_step, frequency, powerTransmittedDbm, antennaGainDbi); + ray_elevation_step, frequency, power_transmitted_dbm, antenna_gain_dbi, received_noise_mean_dbm, + received_noise_st_dev_dbm); }); TAPE_HOOK(node, TAPE_ARRAY(radar_scopes, radar_scopes_count), radar_scopes_count, ray_azimuth_step, ray_elevation_step, - frequency, powerTransmittedDbm, antennaGainDbi); + frequency, power_transmitted_dbm, antenna_gain_dbi, received_noise_mean_dbm, received_noise_st_dev_dbm); return status; } @@ -1093,7 +1097,8 @@ void TapeCore::tape_node_points_radar_postprocess(const YAML::Node& yamlNode, Pl rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr; rgl_node_points_radar_postprocess(&node, state.getPtr(yamlNode[1]), yamlNode[2].as(), yamlNode[3].as(), yamlNode[4].as(), yamlNode[5].as(), - yamlNode[6].as(), yamlNode[7].as()); + yamlNode[6].as(), yamlNode[7].as(), yamlNode[8].as(), + yamlNode[9].as()); state.nodes.insert({nodeId, node}); } diff --git a/src/graph/NodesCore.hpp b/src/graph/NodesCore.hpp index 52f7b1aef..25717df3f 100644 --- a/src/graph/NodesCore.hpp +++ b/src/graph/NodesCore.hpp @@ -476,7 +476,8 @@ struct RadarPostprocessPointsNode : IPointsNodeSingleInput using Ptr = std::shared_ptr; void setParameters(const std::vector& radarScopes, float rayAzimuthStepRad, float rayElevationStepRad, - float frequency, float powerTransmittedDbm, float antennaGainDbi); + float frequency, float powerTransmittedDbm, float antennaGainDbi, float receivedNoiseMean, + float receivedNoiseStdDev); // Node void validateImpl() override; @@ -522,9 +523,13 @@ struct RadarPostprocessPointsNode : IPointsNodeSingleInput float frequency; float powerTransmittedDbm; float antennaGainDbi; + float receivedNoiseMeanDb; + float receivedNoiseStDevDb; std::vector radarScopes; + std::random_device randomDevice; + // RGL related members std::mutex getFieldDataMutex; mutable CacheManager cacheManager; diff --git a/src/graph/RadarPostprocessPointsNode.cpp b/src/graph/RadarPostprocessPointsNode.cpp index 6e62f83dc..3d2d91319 100644 --- a/src/graph/RadarPostprocessPointsNode.cpp +++ b/src/graph/RadarPostprocessPointsNode.cpp @@ -31,7 +31,7 @@ inline static std::optional getRadarScopeWithinDistance(const void RadarPostprocessPointsNode::setParameters(const std::vector& radarScopes, float rayAzimuthStepRad, float rayElevationStepRad, float frequency, float powerTransmittedDbm, - float antennaGainDbi) + float antennaGainDbi, float receivedNoiseMean, float receivedNoiseStdDev) { this->rayAzimuthStepRad = rayAzimuthStepRad; this->rayElevationStepRad = rayElevationStepRad; @@ -39,6 +39,8 @@ void RadarPostprocessPointsNode::setParameters(const std::vectorradarScopes = radarScopes; this->powerTransmittedDbm = powerTransmittedDbm; this->antennaGainDbi = antennaGainDbi; + this->receivedNoiseMeanDb = receivedNoiseMean; + this->receivedNoiseStDevDb = receivedNoiseStdDev; } void RadarPostprocessPointsNode::validateImpl() @@ -139,6 +141,7 @@ void RadarPostprocessPointsNode::enqueueExecImpl() clusterPowerHost->resize(filteredIndicesHost.size(), false, false); clusterNoiseHost->resize(filteredIndicesHost.size(), false, false); clusterSnrHost->resize(filteredIndicesHost.size(), false, false); + std::normal_distribution gaussianNoise(receivedNoiseMeanDb, receivedNoiseStDevDb); for (int clusterIdx = 0; clusterIdx < filteredIndicesHost.size(); ++clusterIdx) { std::complex AU = 0; std::complex AR = 0; @@ -159,15 +162,16 @@ void RadarPostprocessPointsNode::enqueueExecImpl() // TODO: Handle nans in RCS. const auto rcsDbsm = 10.0f * log10f(4.0f * M_PIf * (pow(abs(AU), 2) + pow(abs(AR), 2))); - clusterRcsHost->at(clusterIdx) = rcsDbsm; - + if (std::isnan(rcsDbsm)) { + throw InvalidPipeline("RCS is NaN"); + } const auto distance = distanceInputHost->at(filteredIndicesHost.at(clusterIdx)); const auto multiplier = 10.0f * log10f(powf(4 * std::numbers::pi_v, 3)) + 10.0f * log10f(powf(distance, 4)); - const auto outputPower = powerTransmittedDbm + antennaGainDbi + antennaGainDbi + rcsDbsm + lambdaSqrtDbsm - multiplier; + clusterRcsHost->at(clusterIdx) = rcsDbsm; clusterPowerHost->at(clusterIdx) = outputPower; - clusterNoiseHost->at(clusterIdx) = 1.0f; + clusterNoiseHost->at(clusterIdx) = gaussianNoise(randomDevice); clusterSnrHost->at(clusterIdx) = clusterPowerHost->at(clusterIdx) - clusterNoiseHost->at(clusterIdx); } clusterPowerDev->copyFrom(clusterPowerHost); diff --git a/test/src/TapeTest.cpp b/test/src/TapeTest.cpp index 9a6ea38b5..81b0abc35 100644 --- a/test/src/TapeTest.cpp +++ b/test/src/TapeTest.cpp @@ -265,7 +265,8 @@ TEST_F(TapeTest, RecordPlayAllCalls) rgl_node_t radarPostprocess = nullptr; rgl_radar_scope_t radarScope{0.0f, 1.0f, 1.0f, 1.0f, 1.0f}; - EXPECT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarPostprocess, &radarScope, 1, 1.0f, 1.0f, 79E9f, 31.0f, 27.0f)); + EXPECT_RGL_SUCCESS( + rgl_node_points_radar_postprocess(&radarPostprocess, &radarScope, 1, 1.0f, 1.0f, 79E9f, 31.0f, 27.0f, 60.0f, 1.0f)); rgl_node_t filterGround = nullptr; rgl_vec3f sensorUpVector = {0.0f, 1.0f, 0.0f}; diff --git a/test/src/graph/nodes/RadarPostprocessPointsNodeTest.cpp b/test/src/graph/nodes/RadarPostprocessPointsNodeTest.cpp index 5181c9d97..3e75a1781 100644 --- a/test/src/graph/nodes/RadarPostprocessPointsNodeTest.cpp +++ b/test/src/graph/nodes/RadarPostprocessPointsNodeTest.cpp @@ -26,8 +26,8 @@ TEST_F(RadarPostprocessPointsNodeTest, clustering_test) .radial_speed_separation_threshold = 1.0f, .azimuth_separation_threshold = 1.0f}, }; - ASSERT_RGL_SUCCESS( - rgl_node_points_radar_postprocess(&radarNode, radarScopes.data(), radarScopes.size(), 1.0f, 1.0f, 1.0f, 1.0f, 1.0f)); + ASSERT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarNode, radarScopes.data(), radarScopes.size(), 1.0f, 1.0f, 1.0f, + 1.0f, 1.0f, 1.0f, 1.0f)); auto runAndReturnNumberOfClusters = [&radarNode](const TestPointCloud& inPointCloud) -> int32_t { rgl_node_t usePointsNode = inPointCloud.createUsePointsNode(); diff --git a/test/src/scene/radarTest.cpp b/test/src/scene/radarTest.cpp index 4f417d39b..fa7a2e81c 100644 --- a/test/src/scene/radarTest.cpp +++ b/test/src/scene/radarTest.cpp @@ -80,8 +80,8 @@ TEST_F(RadarTest, rotating_reflector_2d) // Radar postprocessing and publishing rgl_node_t radarPostProcess = nullptr, radarFormat = nullptr, radarPublish = nullptr; - EXPECT_RGL_SUCCESS( - rgl_node_points_radar_postprocess(&radarPostProcess, &radarScope, 1, azimuthStep, elevationStep, 79E9f, 31.0f, 27.0f)); + EXPECT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarPostProcess, &radarScope, 1, azimuthStep, elevationStep, 79E9f, + 31.0f, 27.0f, 60.0f, 1.0f)); EXPECT_RGL_SUCCESS(rgl_node_points_format(&radarFormat, fields.data(), fields.size())); EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&radarPublish, "rgl_radar", "world")); EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(compact, radarPostProcess)); @@ -110,9 +110,7 @@ TEST_F(RadarTest, rotating_reflector_2d) offset += Vec3f{0, 3, 0}; } - fmt::print("Angle: {}\n", angle); EXPECT_RGL_SUCCESS(rgl_graph_run(rays)); - std::this_thread::sleep_for(100ms); time += 10'000'000; // 10 ms } } From 1365d6788a40988b87e8b7885b9e93d47c6701d9 Mon Sep 17 00:00:00 2001 From: Piotr Rybicki Date: Wed, 13 Mar 2024 11:16:45 +0100 Subject: [PATCH 07/16] Fix clustering test --- test/include/helpers/fieldGenerators.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/test/include/helpers/fieldGenerators.hpp b/test/include/helpers/fieldGenerators.hpp index 93a89b54f..8f0040dd0 100644 --- a/test/include/helpers/fieldGenerators.hpp +++ b/test/include/helpers/fieldGenerators.hpp @@ -76,8 +76,10 @@ static std::function::type(int)> genIncidentAngle = [] return std::uniform_real_distribution(0, std::numbers::pi / 2.0f)(randomGenerator); }; static std::function::type(int)> genNormal = [](int i) { - return Vec3f(static_cast(i) / (static_cast(i) + 1), static_cast(i) / (static_cast(i) + 2), - static_cast(i) / (static_cast(i) + 3)); + std::mt19937 gen(i); + std::uniform_real_distribution<> dis(0, 2 * M_PI); + double theta = dis(gen), phi = dis(gen); + return Vec3f{std::sin(theta) * std::cos(phi), std::sin(theta) * std::sin(phi), std::cos(theta)}; }; static std::function::type(int)> genRayPose = [](int i) { std::uniform_real_distribution tr(-1, 1); @@ -96,5 +98,4 @@ static std::function::type(int)> genAllGround = [](int i) { static std::function::type(int)> genAllNonGround = [](int i) { return 1; }; static std::function::type(int)> genRandGround = [](int i) { return std::uniform_int_distribution(0, 1)(randomGenerator); - }; \ No newline at end of file From 0ad79bfd1eb4b8073f45df774108a491388127b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Liberadzki?= Date: Thu, 14 Mar 2024 13:20:33 +0100 Subject: [PATCH 08/16] Fix reflecting polarization. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Liberadzki --- src/gpu/nodeKernels.cu | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gpu/nodeKernels.cu b/src/gpu/nodeKernels.cu index e4332cbed..883fadb55 100644 --- a/src/gpu/nodeKernels.cu +++ b/src/gpu/nodeKernels.cu @@ -87,7 +87,7 @@ __device__ Vec3f reflectPolarization(const Vec3f& pol, const Vec3f& hitNormal, c const auto refDir = (rayDir - hitNormal * (2 * rayDir.dot(hitNormal))).normalized(); const auto refPolU = -1.0f * polU; - const auto refPolR = rayDir.cross(refPolU); + const auto refPolR = refDir.cross(refPolU); const auto polCompU = pol.dot(polU); const auto polCompR = pol.dot(polR); From 5bc019923cc5029d506913669a1e5ab70e95ecce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Liberadzki?= Date: Thu, 14 Mar 2024 13:39:18 +0100 Subject: [PATCH 09/16] Change not necessary complex to scalar. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Liberadzki --- src/gpu/nodeKernels.cu | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/gpu/nodeKernels.cu b/src/gpu/nodeKernels.cu index 883fadb55..c1104d57e 100644 --- a/src/gpu/nodeKernels.cu +++ b/src/gpu/nodeKernels.cu @@ -125,7 +125,7 @@ __global__ void kRadarComputeEnergy(size_t count, float rayAzimuthStepRad, float const Vec3f dirP = {-sp, cp, 0}; const Vec3f dirT = {cp * ct, sp * ct, -st}; - const thrust::complex kr = {waveNum * hitDist[tid], 0.0f}; + const float kr = waveNum * hitDist[tid]; const Vec3f rayDir = rayDirCts.normalized(); const Vec3f rayPol = rayPose[tid] * Vec3f{-1, 0, 0}; // UP, perpendicular to ray From fc5df8f4f5dd02c4f8248947c68c3509cb506fde Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Liberadzki?= Date: Thu, 14 Mar 2024 13:41:28 +0100 Subject: [PATCH 10/16] Fix ray dir (to reflected dir) for scattered field calculation. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Liberadzki --- src/gpu/nodeKernels.cu | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/gpu/nodeKernels.cu b/src/gpu/nodeKernels.cu index c1104d57e..dcb5cf6ae 100644 --- a/src/gpu/nodeKernels.cu +++ b/src/gpu/nodeKernels.cu @@ -130,19 +130,21 @@ __global__ void kRadarComputeEnergy(size_t count, float rayAzimuthStepRad, float const Vec3f rayDir = rayDirCts.normalized(); const Vec3f rayPol = rayPose[tid] * Vec3f{-1, 0, 0}; // UP, perpendicular to ray const Vec3f reflectedPol = reflectPolarization(rayPol, hitNorm[tid], rayDir); + const Vec3f reflectedDir = (rayDir - hitNorm[tid] * (2 * rayDir.dot(hitNorm[tid]))).normalized(); const Vector<3, thrust::complex> rayPolCplx = {reflectedPol.x(), reflectedPol.y(), reflectedPol.z()}; const Vector<3, thrust::complex> apE = reflectionCoef * exp(i * kr) * rayPolCplx; - const Vector<3, thrust::complex> apH = -apE.cross(rayDir); + const Vector<3, thrust::complex> apH = -apE.cross(reflectedDir); const Vec3f vecK = waveNum * ((dirX * cp + dirY * sp) * st + dirZ * ct); - const float rayArea = hitDist[tid] * hitDist[tid] * std::sin(rayElevationStepRad) * rayAzimuthStepRad; + const float rayArea = hitDist[tid] * hitDist[tid] * sinf(rayElevationStepRad) * rayAzimuthStepRad; + //printf("ele rad: %.6f azi rad: %.6f area: %.6f distance: %0.2f\n", rayElevationStepRad, rayAzimuthStepRad, rayArea, hitDist[tid]); - thrust::complex BU = (-(apE.cross(-dirP) + apH.cross(dirT))).dot(rayDir); - thrust::complex BR = (-(apE.cross(dirT) + apH.cross(dirP))).dot(rayDir); - thrust::complex factor = thrust::complex(0.0, ((waveNum * rayArea) / (4.0f * static_cast(M_PI)))) * + thrust::complex BU = (-(apE.cross(-dirP) + apH.cross(dirT))).dot(reflectedDir); + thrust::complex BR = (-(apE.cross(dirT) + apH.cross(dirP))).dot(reflectedDir); + thrust::complex factor = thrust::complex(0.0, ((waveNum * rayArea) / (4.0f * M_PIf))) * exp(-i * vecK.dot(hitPos[tid])); // printf("GPU: point=%d ray=??: dist=%f, pos=(%.2f, %.2f, %.2f), norm=(%.2f, %.2f, %.2f), BU=(%.2f+%.2fi), BR=(%.2f+%.2fi), factor=(%.2f+%.2fi)\n", tid, hitDist[tid], From 01e33334c49c71f75149abc4359a8b12a2e8d1ab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Liberadzki?= Date: Thu, 14 Mar 2024 13:44:16 +0100 Subject: [PATCH 11/16] Update azimuth and elevation angles in ray generation in RCS radar test. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Paweł Liberadzki --- test/src/scene/radarTest.cpp | 22 +++++++++++++++++----- 1 file changed, 17 insertions(+), 5 deletions(-) diff --git a/test/src/scene/radarTest.cpp b/test/src/scene/radarTest.cpp index fa7a2e81c..02022cfef 100644 --- a/test/src/scene/radarTest.cpp +++ b/test/src/scene/radarTest.cpp @@ -34,7 +34,7 @@ std::vector genRadarRays() // By default, the rays are directed along the Z-axis // So first, we rotate them around the Y-axis to point towards the X-axis (to be RVIZ2 compatible) // Then, rotation around Z is azimuth, around Y is elevation - auto ray = Mat3x4f::rotationDeg(0, e + 90, -a); + const auto ray = Mat3x4f::rotationDeg(a, e, 0); rays.emplace_back(ray.toRGL()); // The above will have to be modified again - we assume that target is farther in X axis when in fact @@ -66,22 +66,34 @@ TEST_F(RadarTest, rotating_reflector_2d) // LiDAR rgl_node_t rays = nullptr, noise = nullptr, raytrace = nullptr, compact = nullptr, lidarFormat = nullptr, lidarPublish = nullptr; + + rgl_node_t raysTransform = nullptr; + const auto raycasterTransform = Mat3x4f::rotationDeg(0.0f, 90.0f, 0.0f).toRGL(); + EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&rays, raysData.data(), raysData.size())); + EXPECT_RGL_SUCCESS(rgl_node_rays_transform(&raysTransform, &raycasterTransform)); EXPECT_RGL_SUCCESS(rgl_node_gaussian_noise_angular_ray(&noise, 0, 2, RGL_AXIS_Z)); EXPECT_RGL_SUCCESS(rgl_node_raytrace(&raytrace, nullptr)); EXPECT_RGL_SUCCESS(rgl_node_points_compact_by_field(&compact, RGL_FIELD_IS_HIT_I32)); EXPECT_RGL_SUCCESS(rgl_node_points_format(&lidarFormat, fields.data(), 1)); // Publish only XYZ EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&lidarPublish, "rgl_lidar", "world")); - EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(rays, noise)); - EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(noise, raytrace)); + + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(rays, raysTransform)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(raysTransform, raytrace)); + //EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(noise, raytrace)); EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(raytrace, compact)); EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(compact, lidarFormat)); EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(lidarFormat, lidarPublish)); + const float azimuthStepRad = azimuthStep * (std::numbers::pi_v / 180.0f); + const float elevationStepRad = elevationStep * (std::numbers::pi_v / 180.0f); + const float powerTransmittedDdm = 31.0f; + const float antennaGainDbi = 27.0f; + // Radar postprocessing and publishing rgl_node_t radarPostProcess = nullptr, radarFormat = nullptr, radarPublish = nullptr; - EXPECT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarPostProcess, &radarScope, 1, azimuthStep, elevationStep, 79E9f, - 31.0f, 27.0f, 60.0f, 1.0f)); + EXPECT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarPostProcess, &radarScope, 1, azimuthStepRad, elevationStepRad, 79E9f, + powerTransmittedDdm, antennaGainDbi, 60.0f, 1.0f)); EXPECT_RGL_SUCCESS(rgl_node_points_format(&radarFormat, fields.data(), fields.size())); EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&radarPublish, "rgl_radar", "world")); EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(compact, radarPostProcess)); From df81a7235086ac2ffdacea6ca020bd9fc722e837 Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski <112629916+msz-rai@users.noreply.github.com> Date: Tue, 26 Mar 2024 08:25:29 +0100 Subject: [PATCH 12/16] Improve rcs (#273) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add debug development chnages. Signed-off-by: Paweł Liberadzki * Add fixes for RCS calculation. Signed-off-by: Paweł Liberadzki * Reduce mess in RCS calculation kernel. Signed-off-by: Paweł Liberadzki * Remove not used variable. Signed-off-by: Paweł Liberadzki * Remove not necessary dependency on hit position. Signed-off-by: Paweł Liberadzki * Compute radar energy locally to be independent on lidar pose * Make update to lower RCS value near 90 and -90 degrees. Signed-off-by: Paweł Liberadzki * Cleanup radar postprocessing node (revert clustering changes) * Make log constexpr (maybe perf optimization) * Get rid of M_PIf * Remove calculateMat function --------- Signed-off-by: Paweł Liberadzki Co-authored-by: Paweł Liberadzki --- src/gpu/nodeKernels.cu | 140 +++++++++++++++-------- src/gpu/nodeKernels.hpp | 10 +- src/graph/RadarPostprocessPointsNode.cpp | 13 ++- test/src/scene/radarTest.cpp | 127 ++++++++++++++++---- 4 files changed, 211 insertions(+), 79 deletions(-) diff --git a/src/gpu/nodeKernels.cu b/src/gpu/nodeKernels.cu index dcb5cf6ae..63a2e98b0 100644 --- a/src/gpu/nodeKernels.cu +++ b/src/gpu/nodeKernels.cu @@ -81,6 +81,11 @@ __global__ void kFilter(size_t count, const Field::type* indices, c __device__ Vec3f reflectPolarization(const Vec3f& pol, const Vec3f& hitNormal, const Vec3f& rayDir) { + // Normal incidence + if (abs(rayDir.dot(hitNormal)) == 1) { + return -pol; + } + const auto diffCrossNormal = rayDir.cross(hitNormal); const auto polU = diffCrossNormal.normalized(); const auto polR = rayDir.cross(polU).normalized(); @@ -96,12 +101,14 @@ __device__ Vec3f reflectPolarization(const Vec3f& pol, const Vec3f& hitNormal, c } __global__ void kRadarComputeEnergy(size_t count, float rayAzimuthStepRad, float rayElevationStepRad, float freq, - const Field::type* rayPose, const Field::type* hitDist, - const Field::type* hitNorm, const Field::type* hitPos, - Vector<3, thrust::complex>* outBUBRFactor) + Mat3x4f lookAtOriginTransform, const Field::type* rayPose, + const Field::type* hitDist, const Field::type* hitNorm, + const Field::type* hitPos, Vector<3, thrust::complex>* outBUBRFactor) { LIMIT(count); + constexpr bool log = false; + constexpr float c0 = 299792458.0f; constexpr float reflectionCoef = 1.0f; // TODO const float waveLen = c0 / freq; @@ -111,10 +118,20 @@ __global__ void kRadarComputeEnergy(size_t count, float rayAzimuthStepRad, float const Vec3f dirY = {0, 1, 0}; const Vec3f dirZ = {0, 0, 1}; - const Vec3f rayDirCts = rayPose[tid] * Vec3f{0, 0, 1}; - const Vec3f rayDirSph = rayDirCts.toSpherical(); - const float phi = rayDirSph[1]; // azimuth, 0 = X-axis, positive = CCW - const float the = rayDirSph[2]; // elevation, 0 = Z-axis, 90 = XY-plane, -180 = negative Z-axis + const Mat3x4f rayPoseLocal = lookAtOriginTransform * rayPose[tid]; + // const Vec3f hitPosLocal = lookAtOriginTransform * hitPos[tid]; + const Vec3f rayDir = rayPoseLocal * Vec3f{0, 0, 1}; + const Vec3f rayPol = rayPoseLocal * Vec3f{1, 0, 0}; // UP, perpendicular to ray + const Vec3f hitNormalLocal = lookAtOriginTransform.rotation() * hitNorm[tid]; + const float hitDistance = hitDist[tid]; + const float rayArea = hitDistance * hitDistance * sinf(rayElevationStepRad) * rayAzimuthStepRad; + + if (log) + printf("rayDir: (%.4f %.4f %.4f) rayPol: (%.4f %.4f %.4f) hitNormal: (%.4f %.4f %.4f)\n", rayDir.x(), rayDir.y(), + rayDir.z(), rayPol.x(), rayPol.y(), rayPol.z(), hitNormalLocal.x(), hitNormalLocal.y(), hitNormalLocal.z()); + + const float phi = atan2(rayDir[1], rayDir[2]); + const float the = acos(rayDir[0] / rayDir.length()); // Consider unit vector of the ray direction, these are its projections: const float cp = cosf(phi); // X-dir component @@ -122,42 +139,74 @@ __global__ void kRadarComputeEnergy(size_t count, float rayAzimuthStepRad, float const float ct = cosf(the); // Z-dir component const float st = sinf(the); // XY-plane component - const Vec3f dirP = {-sp, cp, 0}; - const Vec3f dirT = {cp * ct, sp * ct, -st}; + const Vec3f dirP = {0, cp, -sp}; + const Vec3f dirT = {-st, sp * ct, cp * ct}; + const Vec3f vecK = waveNum * ((dirZ * cp + dirY * sp) * st + dirX * ct); - const float kr = waveNum * hitDist[tid]; + if (log) + printf("phi: %.2f [dirP: (%.4f %.4f %.4f)] theta: %.2f [dirT: (%.4f %.4f %.4f)] vecK=(%.2f, %.2f, %.2f)\n", phi, + dirP.x(), dirP.y(), dirP.z(), the, dirT.x(), dirT.y(), dirT.z(), vecK.x(), vecK.y(), vecK.z()); - const Vec3f rayDir = rayDirCts.normalized(); - const Vec3f rayPol = rayPose[tid] * Vec3f{-1, 0, 0}; // UP, perpendicular to ray - const Vec3f reflectedPol = reflectPolarization(rayPol, hitNorm[tid], rayDir); - const Vec3f reflectedDir = (rayDir - hitNorm[tid] * (2 * rayDir.dot(hitNorm[tid]))).normalized(); + const Vec3f reflectedDir = (rayDir - hitNormalLocal * (2 * rayDir.dot(hitNormalLocal))).normalized(); + const Vec3f reflectedPol = reflectPolarization(rayPol, hitNormalLocal, rayDir); + const Vector<3, thrust::complex> reflectedPolCplx = {reflectedPol.x(), reflectedPol.y(), reflectedPol.z()}; + const float kr = waveNum * hitDistance; - const Vector<3, thrust::complex> rayPolCplx = {reflectedPol.x(), reflectedPol.y(), reflectedPol.z()}; + if (log) + printf("reflectedDir: (%.4f %.4f %.4f) reflectedPol: (%.4f %.4f %.4f)\n", reflectedDir.x(), reflectedDir.y(), + reflectedDir.z(), reflectedPol.x(), reflectedPol.y(), reflectedPol.z()); - const Vector<3, thrust::complex> apE = reflectionCoef * exp(i * kr) * rayPolCplx; + const Vector<3, thrust::complex> apE = reflectionCoef * exp(i * kr) * reflectedPolCplx; const Vector<3, thrust::complex> apH = -apE.cross(reflectedDir); - const Vec3f vecK = waveNum * ((dirX * cp + dirY * sp) * st + dirZ * ct); - - const float rayArea = hitDist[tid] * hitDist[tid] * sinf(rayElevationStepRad) * rayAzimuthStepRad; - //printf("ele rad: %.6f azi rad: %.6f area: %.6f distance: %0.2f\n", rayElevationStepRad, rayAzimuthStepRad, rayArea, hitDist[tid]); - - thrust::complex BU = (-(apE.cross(-dirP) + apH.cross(dirT))).dot(reflectedDir); - thrust::complex BR = (-(apE.cross(dirT) + apH.cross(dirP))).dot(reflectedDir); - thrust::complex factor = thrust::complex(0.0, ((waveNum * rayArea) / (4.0f * M_PIf))) * - exp(-i * vecK.dot(hitPos[tid])); - - // printf("GPU: point=%d ray=??: dist=%f, pos=(%.2f, %.2f, %.2f), norm=(%.2f, %.2f, %.2f), BU=(%.2f+%.2fi), BR=(%.2f+%.2fi), factor=(%.2f+%.2fi)\n", tid, hitDist[tid], - // hitPos[tid].x(), hitPos[tid].y(), hitPos[tid].z(), hitNorm[tid].x(), hitNorm[tid].y(), hitNorm[tid].z(), - // BU.real(), BU.imag(), BR.real(), BR.imag(), factor.real(), factor.imag()); - // Print variables: - // printf("GPU: point=%d ray=??: rayDirCts=(%.2f, %.2f, %.2f), rayDirSph=(%.2f, %.2f, %.2f), phi=%.2f, the=%.2f, cp=%.2f, " - // "sp=%.2f, ct=%.2f, st=%.2f, dirP=(%.2f, %.2f, %.2f), dirT=(%.2f, %.2f, %.2f), kr=(%.2f+%.2fi), rayDir=(%.2f, " - // "%.2f, %.2f), rayPol=(%.2f, %.2f, %.2f), reflectedPol=(%.2f, %.2f, %.2f)\n", - // tid, rayDirCts.x(), rayDirCts.y(), rayDirCts.z(), rayDirSph.x(), rayDirSph.y(), - // rayDirSph.z(), phi, the, cp, sp, ct, st, dirP.x(), dirP.y(), dirP.z(), dirT.x(), dirT.y(), dirT.z(), kr.real(), - // kr.imag(), rayDir.x(), rayDir.y(), rayDir.z(), rayPol.x(), rayPol.y(), rayPol.z(), reflectedPol.x(), - // reflectedPol.y(), reflectedPol.z()); + if (log) + printf("apE: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n", apE.x().real(), apE.x().imag(), apE.y().real(), + apE.y().imag(), apE.z().real(), apE.z().imag()); + if (log) + printf("apH: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n", apH.x().real(), apH.x().imag(), apH.y().real(), + apH.y().imag(), apH.z().real(), apH.z().imag()); + + const Vector<3, thrust::complex> BU1 = apE.cross(-dirP); + const Vector<3, thrust::complex> BU2 = apH.cross(dirT); + const Vector<3, thrust::complex> refField1 = (-(BU1 + BU2)); + + if (log) + printf("BU1: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n" + "BU2: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n" + "refField1: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n", + BU1.x().real(), BU1.x().imag(), BU1.y().real(), BU1.y().imag(), BU1.z().real(), BU1.z().imag(), BU2.x().real(), + BU2.x().imag(), BU2.y().real(), BU2.y().imag(), BU2.z().real(), BU2.z().imag(), refField1.x().real(), + refField1.x().imag(), refField1.y().real(), refField1.y().imag(), refField1.z().real(), refField1.z().imag()); + + const Vector<3, thrust::complex> BR1 = apE.cross(dirT); + const Vector<3, thrust::complex> BR2 = apH.cross(dirP); + const Vector<3, thrust::complex> refField2 = (-(BR1 + BR2)); + + if (log) + printf("BR1: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n" + "BR2: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n" + "refField2: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n", + BR1.x().real(), BR1.x().imag(), BR1.y().real(), BR1.y().imag(), BR1.z().real(), BR1.z().imag(), BR2.x().real(), + BR2.x().imag(), BR2.y().real(), BR2.y().imag(), BR2.z().real(), BR2.z().imag(), refField2.x().real(), + refField2.x().imag(), refField2.y().real(), refField2.y().imag(), refField2.z().real(), refField2.z().imag()); + + const thrust::complex BU = refField1.dot(reflectedDir); + const thrust::complex BR = refField2.dot(reflectedDir); + // const thrust::complex factor = thrust::complex(0.0, ((waveNum * rayArea) / (4.0f * static_cast(M_PI)))) * + // exp(-i * waveNum * hitDistance); + const thrust::complex factor = thrust::complex(0.0, ((waveNum * rayArea * reflectedDir.dot(hitNormalLocal)) / + (4.0f * static_cast(M_PI)))) * + exp(-i * waveNum * hitDistance); + // const thrust::complex factor = thrust::complex(0.0, ((waveNum * rayArea) / (4.0f * static_cast(M_PI)))) * + // exp(-i * vecK.dot(hitPosLocal)); + + const auto BUf = BU * factor; + const auto BRf = BR * factor; + + if (log) + printf("BU: (%.2f + %.2fi) BR: (%.2f + %.2fi) factor: (%.2f + %.2fi) [BUf: (%.2f + %.2fi) BRf: %.2f + %.2fi]\n", + BU.real(), BU.imag(), BR.real(), BR.imag(), factor.real(), factor.imag(), BUf.real(), BUf.imag(), BRf.real(), + BRf.imag()); outBUBRFactor[tid] = {BU, BR, factor}; } @@ -230,20 +279,19 @@ void gpuFilter(cudaStream_t stream, size_t count, const Field::type run(kFilter, stream, count, indices, dst, src, fieldSize); } -void gpuFilterGroundPoints(cudaStream_t stream, size_t pointCount, const Vec3f sensor_up_vector, - float ground_angle_threshold, const Field::type* inPoints, - const Field::type* inNormalsPtr, Field::type* outNonGround, - Mat3x4f lidarTransform) +void gpuFilterGroundPoints(cudaStream_t stream, size_t pointCount, const Vec3f sensor_up_vector, float ground_angle_threshold, + const Field::type* inPoints, const Field::type* inNormalsPtr, + Field::type* outNonGround, Mat3x4f lidarTransform) { run(kFilterGroundPoints, stream, pointCount, sensor_up_vector, ground_angle_threshold, inPoints, inNormalsPtr, outNonGround, lidarTransform); } void gpuRadarComputeEnergy(cudaStream_t stream, size_t count, float rayAzimuthStepRad, float rayElevationStepRad, float freq, - const Field::type* rayPose, const Field::type* hitDist, - const Field::type* hitNorm, const Field::type* hitPos, - Vector<3, thrust::complex>* outBUBRFactor) + Mat3x4f lookAtOriginTransform, const Field::type* rayPose, + const Field::type* hitDist, const Field::type* hitNorm, + const Field::type* hitPos, Vector<3, thrust::complex>* outBUBRFactor) { - run(kRadarComputeEnergy, stream, count, rayAzimuthStepRad, rayElevationStepRad, freq, rayPose, hitDist, hitNorm, hitPos, - outBUBRFactor); + run(kRadarComputeEnergy, stream, count, rayAzimuthStepRad, rayElevationStepRad, freq, lookAtOriginTransform, rayPose, + hitDist, hitNorm, hitPos, outBUBRFactor); } diff --git a/src/gpu/nodeKernels.hpp b/src/gpu/nodeKernels.hpp index c82685191..f05cfd602 100644 --- a/src/gpu/nodeKernels.hpp +++ b/src/gpu/nodeKernels.hpp @@ -30,8 +30,8 @@ // This could be defined in CompactNode, however such include here causes mess because nvcc does not support C++20. using CompactionIndexType = int32_t; -void gpuFindCompaction(cudaStream_t, size_t pointCount, const int32_t* shouldCompact, - CompactionIndexType* hitCountInclusive, size_t* outHitCount); +void gpuFindCompaction(cudaStream_t, size_t pointCount, const int32_t* shouldCompact, CompactionIndexType* hitCountInclusive, + size_t* outHitCount); void gpuFormatSoaToAos(cudaStream_t, size_t pointCount, size_t pointSize, size_t fieldCount, const GPUFieldDesc* soaInData, char* aosOutData); void gpuFormatAosToSoa(cudaStream_t, size_t pointCount, size_t pointSize, size_t fieldCount, const char* aosInData, @@ -48,6 +48,6 @@ void gpuFilterGroundPoints(cudaStream_t stream, size_t pointCount, const Vec3f s const Field::type* inPoints, const Field::type* inNormalsPtr, Field::type* outNonGround, Mat3x4f lidarTransform); void gpuRadarComputeEnergy(cudaStream_t stream, size_t count, float rayAzimuthStepRad, float rayElevationStepRad, float freq, - const Field::type* rayPose, const Field::type* hitDist, - const Field::type* hitNorm, const Field::type* hitPos, - Vector<3, thrust::complex>* outBUBRFactor); \ No newline at end of file + Mat3x4f lookAtOriginTransform, const Field::type* rayPose, + const Field::type* hitDist, const Field::type* hitNorm, + const Field::type* hitPos, Vector<3, thrust::complex>* outBUBRFactor); \ No newline at end of file diff --git a/src/graph/RadarPostprocessPointsNode.cpp b/src/graph/RadarPostprocessPointsNode.cpp index 3d2d91319..4d59507c2 100644 --- a/src/graph/RadarPostprocessPointsNode.cpp +++ b/src/graph/RadarPostprocessPointsNode.cpp @@ -66,8 +66,9 @@ void RadarPostprocessPointsNode::enqueueExecImpl() auto normalPtr = input->getFieldDataTyped()->asSubclass()->getReadPtr(); auto xyzPtr = input->getFieldDataTyped()->asSubclass()->getReadPtr(); outBUBRFactorDev->resize(input->getPointCount(), false, false); - gpuRadarComputeEnergy(getStreamHandle(), input->getPointCount(), rayAzimuthStepRad, rayElevationStepRad, frequency, raysPtr, - distancePtr, normalPtr, xyzPtr, outBUBRFactorDev->getWritePtr()); + gpuRadarComputeEnergy(getStreamHandle(), input->getPointCount(), rayAzimuthStepRad, rayElevationStepRad, frequency, + input->getLookAtOriginTransform(), raysPtr, distancePtr, normalPtr, xyzPtr, + outBUBRFactorDev->getWritePtr()); outBUBRFactorHost->copyFrom(outBUBRFactorDev); CHECK_CUDA(cudaStreamSynchronize(getStreamHandle())); @@ -159,17 +160,23 @@ void RadarPostprocessPointsNode::enqueueExecImpl() } // https://en.wikipedia.org/wiki/Radar_cross_section#Formulation + const auto rcsDbsm = 10.0f * log10f(4.0f * std::numbers::pi_v * (pow(abs(AU), 2) + pow(abs(AR), 2))); // TODO: Handle nans in RCS. - const auto rcsDbsm = 10.0f * log10f(4.0f * M_PIf * (pow(abs(AU), 2) + pow(abs(AR), 2))); if (std::isnan(rcsDbsm)) { throw InvalidPipeline("RCS is NaN"); } + const auto distance = distanceInputHost->at(filteredIndicesHost.at(clusterIdx)); const auto multiplier = 10.0f * log10f(powf(4 * std::numbers::pi_v, 3)) + 10.0f * log10f(powf(distance, 4)); + + //TODO(Pawel): Power transmitted is in dBm, which is here summed up with dB - this is rather incorrect, because dB and dBm are different. const auto outputPower = powerTransmittedDbm + antennaGainDbi + antennaGainDbi + rcsDbsm + lambdaSqrtDbsm - multiplier; clusterRcsHost->at(clusterIdx) = rcsDbsm; + + //TODO(Pawel): SmartMicro driver calculates SNR as Power - Noise. This means that their power contains this noise. Thus, it should + // be outputPower + noise below. clusterPowerHost->at(clusterIdx) = outputPower; clusterNoiseHost->at(clusterIdx) = gaussianNoise(randomDevice); clusterSnrHost->at(clusterIdx) = clusterPowerHost->at(clusterIdx) - clusterNoiseHost->at(clusterIdx); diff --git a/test/src/scene/radarTest.cpp b/test/src/scene/radarTest.cpp index 02022cfef..565e584b2 100644 --- a/test/src/scene/radarTest.cpp +++ b/test/src/scene/radarTest.cpp @@ -16,6 +16,13 @@ #include #include "helpers/testPointCloud.hpp" +#include +struct numpunct : std::numpunct +{ +protected: + char do_decimal_point() const override { return ','; } +}; + struct RadarTest : RGLTest {}; @@ -26,6 +33,32 @@ const auto maxElevation = 7.5f; const auto azimuthStep = 0.49f; const auto elevationStep = 0.49f; +rgl_mesh_t getFlatPlate(float dim) +{ + dim *= 0.5f; + std::vector rgl_vertices = { + {0.0f, dim, -dim}, + {0.0f, dim, dim}, + {0.0f, -dim, dim}, + {0.0f, -dim, -dim} + }; + std::vector rgl_indices = { + {0, 3, 1}, + {1, 3, 2} + }; + + rgl_mesh_t outMesh = nullptr; + rgl_status_t status = rgl_mesh_create(&outMesh, rgl_vertices.data(), rgl_vertices.size(), rgl_indices.data(), + rgl_indices.size()); + + if (status != RGL_SUCCESS) { + const char* errorString = nullptr; + rgl_get_last_error_string(&errorString); + throw std::runtime_error(fmt::format("rgl_mesh_create: {}", errorString)); + } + return outMesh; +} + std::vector genRadarRays() { std::vector rays; @@ -39,6 +72,9 @@ std::vector genRadarRays() // The above will have to be modified again - we assume that target is farther in X axis when in fact // we use Z as RGL LiDAR front. Remember to update. + + const auto rayDir = ray * Vec3f{0, 0, 1}; + //printf("rayDir: %.2f %.2f %.2f\n", rayDir.x(), rayDir.y(), rayDir.z()); } } @@ -54,6 +90,13 @@ TEST_F(RadarTest, rotating_reflector_2d) // Setup sensor and graph std::vector raysData = genRadarRays(); + // std::vector raysData = { + // Mat3x4f::rotationDeg(0, 0, 0).toRGL(), + //// Mat3x4f::rotationDeg(2.5, 0, 0).toRGL(), + //// Mat3x4f::rotationDeg(5, 0, 0).toRGL(), + //// Mat3x4f::rotationDeg(-2.5, 0, 0).toRGL(), + //// Mat3x4f::rotationDeg(-5, 0, 0).toRGL(), + // }; rgl_radar_scope_t radarScope{ .begin_distance = 1.3f, .end_distance = 19.0f, @@ -68,7 +111,7 @@ TEST_F(RadarTest, rotating_reflector_2d) lidarPublish = nullptr; rgl_node_t raysTransform = nullptr; - const auto raycasterTransform = Mat3x4f::rotationDeg(0.0f, 90.0f, 0.0f).toRGL(); + auto raycasterTransform = Mat3x4f::rotationDeg(0.0f, 90.0f, 0.0f).toRGL(); EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&rays, raysData.data(), raysData.size())); EXPECT_RGL_SUCCESS(rgl_node_rays_transform(&raysTransform, &raycasterTransform)); @@ -78,8 +121,8 @@ TEST_F(RadarTest, rotating_reflector_2d) EXPECT_RGL_SUCCESS(rgl_node_points_format(&lidarFormat, fields.data(), 1)); // Publish only XYZ EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&lidarPublish, "rgl_lidar", "world")); - EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(rays, raysTransform)); - EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(raysTransform, raytrace)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(rays, raytrace)); + //EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(raysTransform, raytrace)); //EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(noise, raytrace)); EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(raytrace, compact)); EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(compact, lidarFormat)); @@ -92,37 +135,71 @@ TEST_F(RadarTest, rotating_reflector_2d) // Radar postprocessing and publishing rgl_node_t radarPostProcess = nullptr, radarFormat = nullptr, radarPublish = nullptr; - EXPECT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarPostProcess, &radarScope, 1, azimuthStepRad, elevationStepRad, 79E9f, - powerTransmittedDdm, antennaGainDbi, 60.0f, 1.0f)); + EXPECT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarPostProcess, &radarScope, 1, azimuthStepRad, elevationStepRad, + 79E9f, powerTransmittedDdm, antennaGainDbi, 60.0f, 1.0f)); EXPECT_RGL_SUCCESS(rgl_node_points_format(&radarFormat, fields.data(), fields.size())); EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&radarPublish, "rgl_radar", "world")); EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(compact, radarPostProcess)); EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(radarPostProcess, radarFormat)); EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(radarFormat, radarPublish)); - // Setup scene - std::vector reflectors2D; - reflectors2D.resize(3, nullptr); - rgl_mesh_t reflector2dMesh = loadFromSTL(fs::path(RGL_TEST_DATA_DIR) / "reflector2d.stl"); - for (auto&& reflector2D : reflectors2D) { - EXPECT_RGL_SUCCESS(rgl_entity_create(&reflector2D, nullptr, reflector2dMesh)); - } - - uint64_t time = 0; - for (float angle = -45; angle <= 45; angle += 0.1f) { - EXPECT_RGL_SUCCESS(rgl_scene_set_time(nullptr, time)); - auto position = Vec3f{5, 0, 0}; - auto rotation = Vec3f{0, 0, angle}; + rgl_entity_t reflector2D = nullptr; + //rgl_mesh_t reflector2dMesh = loadFromSTL(fs::path(RGL_TEST_DATA_DIR) / "reflector2d.stl"); + rgl_mesh_t reflector2dMesh = getFlatPlate(1.0f); + //rgl_mesh_t reflector2dMesh = loadFromSTL("/home/pawel/Pawel/Documentation/RGL/2024Q1/Lexus.stl"); + EXPECT_RGL_SUCCESS(rgl_entity_create(&reflector2D, nullptr, reflector2dMesh)); + + const float initialAngle = -80.0f; + const float finalAngle = 80.0f; + float angle = initialAngle; + for (int index = 0; index <= 1000; ++index, angle = initialAngle + 0.001f * index * (finalAngle - initialAngle)) { + //auto position = Vec3f{15 + angle / 5, 0, 0}; + auto position = Vec3f{0, 0, 5}; + auto rotation = Vec3f{0, -90, 0}; auto scale = Vec3f{1, 1, 1}; + rgl_mat3x4f reflectorPose = + (Mat3x4f::TRS(position, {angle, 0, 0}, scale) * Mat3x4f::TRS({0, 0, 0}, rotation, scale)).toRGL(); + EXPECT_RGL_SUCCESS(rgl_entity_set_pose(reflector2D, &reflectorPose)); - auto offset = Vec3f{0, -3, 0}; - for (auto&& reflector2D : reflectors2D) { - rgl_mat3x4f reflectorPose = Mat3x4f::TRS(position + offset, rotation, scale).toRGL(); - EXPECT_RGL_SUCCESS(rgl_entity_set_pose(reflector2D, &reflectorPose)); - offset += Vec3f{0, 3, 0}; - } + // std::locale loc(std::locale(), new numpunct()); + // std::cout << fmt::format(loc, "{0:L} ", angle); + //printf("%.2f;", angle); EXPECT_RGL_SUCCESS(rgl_graph_run(rays)); - time += 10'000'000; // 10 ms + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + + // for (float azi = -15.0f; azi <= 15.0f; azi += 15.0f) { + // //for (float ele = -15.0f; ele <= 15.0f; ele += 15.0f) { + // raycasterTransform = Mat3x4f::rotationDeg(0.0f, 90.0f, -azi).toRGL(); + // EXPECT_RGL_SUCCESS(rgl_node_rays_transform(&raysTransform, &raycasterTransform)); + // EXPECT_RGL_SUCCESS(rgl_graph_run(rays)); + // //} + // } } + + // Setup scene + // std::vector reflectors2D; + // reflectors2D.resize(3, nullptr); + // rgl_mesh_t reflector2dMesh = loadFromSTL(fs::path(RGL_TEST_DATA_DIR) / "reflector2d.stl"); + // for (auto&& reflector2D : reflectors2D) { + // EXPECT_RGL_SUCCESS(rgl_entity_create(&reflector2D, nullptr, reflector2dMesh)); + // } + // + // uint64_t time = 0; + // for (float angle = -45; angle <= 45; angle += 0.1f) { + // EXPECT_RGL_SUCCESS(rgl_scene_set_time(nullptr, time)); + // auto position = Vec3f{5, 0, 0}; + // auto rotation = Vec3f{0, 0, angle}; + // auto scale = Vec3f{1, 1, 1}; + // + // auto offset = Vec3f{0, -3, 0}; + // for (auto&& reflector2D : reflectors2D) { + // rgl_mat3x4f reflectorPose = Mat3x4f::TRS(position + offset, rotation, scale).toRGL(); + // EXPECT_RGL_SUCCESS(rgl_entity_set_pose(reflector2D, &reflectorPose)); + // offset += Vec3f{0, 3, 0}; + // } + // + // EXPECT_RGL_SUCCESS(rgl_graph_run(rays)); + // time += 10'000'000; // 10 ms + // } } From 71dd3dbcc28d28d997476b31d4b68ef3ee5bed21 Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski Date: Tue, 26 Mar 2024 14:02:53 +0100 Subject: [PATCH 13/16] Radar power as power received + noise --- src/graph/RadarPostprocessPointsNode.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/graph/RadarPostprocessPointsNode.cpp b/src/graph/RadarPostprocessPointsNode.cpp index 4d59507c2..c94ad504c 100644 --- a/src/graph/RadarPostprocessPointsNode.cpp +++ b/src/graph/RadarPostprocessPointsNode.cpp @@ -143,7 +143,7 @@ void RadarPostprocessPointsNode::enqueueExecImpl() clusterNoiseHost->resize(filteredIndicesHost.size(), false, false); clusterSnrHost->resize(filteredIndicesHost.size(), false, false); std::normal_distribution gaussianNoise(receivedNoiseMeanDb, receivedNoiseStDevDb); - for (int clusterIdx = 0; clusterIdx < filteredIndicesHost.size(); ++clusterIdx) { + for (int clusterIdx = 0; clusterIdx < clusters.size(); ++clusterIdx) { std::complex AU = 0; std::complex AR = 0; auto&& cluster = clusters[clusterIdx]; @@ -160,7 +160,7 @@ void RadarPostprocessPointsNode::enqueueExecImpl() } // https://en.wikipedia.org/wiki/Radar_cross_section#Formulation - const auto rcsDbsm = 10.0f * log10f(4.0f * std::numbers::pi_v * (pow(abs(AU), 2) + pow(abs(AR), 2))); + const auto rcsDbsm = 10.0f * log10f(4.0f * std::numbers::pi_v * (powf(abs(AU), 2) + powf(abs(AR), 2))); // TODO: Handle nans in RCS. if (std::isnan(rcsDbsm)) { @@ -170,17 +170,15 @@ void RadarPostprocessPointsNode::enqueueExecImpl() const auto distance = distanceInputHost->at(filteredIndicesHost.at(clusterIdx)); const auto multiplier = 10.0f * log10f(powf(4 * std::numbers::pi_v, 3)) + 10.0f * log10f(powf(distance, 4)); - //TODO(Pawel): Power transmitted is in dBm, which is here summed up with dB - this is rather incorrect, because dB and dBm are different. - const auto outputPower = powerTransmittedDbm + antennaGainDbi + antennaGainDbi + rcsDbsm + lambdaSqrtDbsm - multiplier; + const auto powerReceived = powerTransmittedDbm + antennaGainDbi + antennaGainDbi + rcsDbsm + lambdaSqrtDbsm - + multiplier; clusterRcsHost->at(clusterIdx) = rcsDbsm; - - //TODO(Pawel): SmartMicro driver calculates SNR as Power - Noise. This means that their power contains this noise. Thus, it should - // be outputPower + noise below. - clusterPowerHost->at(clusterIdx) = outputPower; clusterNoiseHost->at(clusterIdx) = gaussianNoise(randomDevice); + clusterPowerHost->at(clusterIdx) = powerReceived + clusterNoiseHost->at(clusterIdx); // power received + noise clusterSnrHost->at(clusterIdx) = clusterPowerHost->at(clusterIdx) - clusterNoiseHost->at(clusterIdx); } + clusterPowerDev->copyFrom(clusterPowerHost); clusterRcsDev->copyFrom(clusterRcsHost); clusterNoiseDev->copyFrom(clusterNoiseHost); From 4d1380c7ee5c426f6566c02fb98409a5ab3b9b44 Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski Date: Tue, 26 Mar 2024 14:29:48 +0100 Subject: [PATCH 14/16] Cleanup parameters --- include/rgl/api/core.h | 16 ++++++------ src/api/apiCore.cpp | 31 ++++++++++++------------ src/graph/NodesCore.hpp | 8 +++--- src/graph/RadarPostprocessPointsNode.cpp | 20 +++++++-------- test/CMakeLists.txt | 2 +- test/src/scene/radarTest.cpp | 2 +- 6 files changed, 39 insertions(+), 40 deletions(-) diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h index bd0207122..fdc62a595 100644 --- a/include/rgl/api/core.h +++ b/include/rgl/api/core.h @@ -769,17 +769,17 @@ RGL_API rgl_status_t rgl_node_points_from_array(rgl_node_t* node, const void* po * @param radar_scopes_count Number of elements in the `radar_scopes` array. * @param ray_azimuth_step The azimuth step between rays (in radians). * @param ray_elevation_step The elevation step between rays (in radians). - * @param frequency The frequency of the radar (in Hz). - * @param powerTransmittedDbm The power transmitted by the radar (in dBm). - * @param antennaGainDbi The gain of the radar's antenna (in dBi). - * @param received_noise_mean_db The mean of the received noise (in dB). - * @param received_noise_std_dev_db The standard deviation of the received noise (in dB). + * @param frequency The operating frequency of the radar (in Hz). + * @param power_transmitted The power transmitted by the radar (in dBm). + * @param cumulative_device_gain The gain of the radar's antennas and any other gains of the device (in dBi). + * @param received_noise_mean The mean of the received noise (in dB). + * @param received_noise_st_dev The standard deviation of the received noise (in dB). */ RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, const rgl_radar_scope_t* radar_scopes, int32_t radar_scopes_count, float ray_azimuth_step, - float ray_elevation_step, float frequency, float powerTransmittedDbm, - float antennaGainDbi, float received_noise_mean_dbm, - float received_noise_st_dev_dbm); + float ray_elevation_step, float frequency, float power_transmitted, + float cumulative_device_gain, float received_noise_mean, + float received_noise_st_dev); /** * Creates or modifies FilterGroundPointsNode. diff --git a/src/api/apiCore.cpp b/src/api/apiCore.cpp index fd8eb573d..d3f217200 100644 --- a/src/api/apiCore.cpp +++ b/src/api/apiCore.cpp @@ -869,12 +869,11 @@ void TapeCore::tape_node_raytrace_configure_distortion(const YAML::Node& yamlNod rgl_node_raytrace_configure_distortion(node, yamlNode[1].as()); } -RGL_API rgl_status_t rgl_node_raytrace_configure_non_hits(rgl_node_t node, float nearDistance, - float farDistance) +RGL_API rgl_status_t rgl_node_raytrace_configure_non_hits(rgl_node_t node, float nearDistance, float farDistance) { auto status = rglSafeCall([&]() { - RGL_API_LOG("rgl_node_raytrace_configure_non_hits(node={}, nearDistance={}, farDistance={})", - repr(node), nearDistance, farDistance); + RGL_API_LOG("rgl_node_raytrace_configure_non_hits(node={}, nearDistance={}, farDistance={})", repr(node), nearDistance, + farDistance); CHECK_ARG(node != nullptr); RaytraceNode::Ptr raytraceNode = Node::validatePtr(node); raytraceNode->setNonHitDistanceValues(nearDistance, farDistance); @@ -1053,24 +1052,24 @@ void TapeCore::tape_node_points_from_array(const YAML::Node& yamlNode, PlaybackS RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, const rgl_radar_scope_t* radar_scopes, int32_t radar_scopes_count, float ray_azimuth_step, - float ray_elevation_step, float frequency, float power_transmitted_dbm, - float antenna_gain_dbi, float received_noise_mean_dbm, - float received_noise_st_dev_dbm) + float ray_elevation_step, float frequency, float power_transmitted, + float cumulative_device_gain, float received_noise_mean, + float received_noise_st_dev) { auto status = rglSafeCall([&]() { RGL_API_LOG("rgl_node_points_radar_postprocess(node={}, radar_scopes={}, ray_azimuth_step={}, ray_elevation_step={}, " - "frequency={}, power_transmitted_dbm={}, antenna_gain_dbi={}, received_noise_mean_db={}, " - "received_noise_std_dev_db={})", + "frequency={}, power_transmitted={}, cumulative_device_gain={}, received_noise_mean={}, " + "received_noise_st_dev={})", repr(node), repr(radar_scopes, radar_scopes_count), ray_azimuth_step, ray_elevation_step, frequency, - power_transmitted_dbm, antenna_gain_dbi, received_noise_mean_dbm, received_noise_st_dev_dbm); + power_transmitted, cumulative_device_gain, received_noise_mean, received_noise_st_dev); CHECK_ARG(radar_scopes != nullptr); CHECK_ARG(radar_scopes_count > 0); CHECK_ARG(ray_azimuth_step > 0); CHECK_ARG(ray_elevation_step > 0); CHECK_ARG(frequency > 0); - CHECK_ARG(power_transmitted_dbm > 0); - CHECK_ARG(antenna_gain_dbi > 0); - CHECK_ARG(received_noise_st_dev_dbm > 0); + CHECK_ARG(power_transmitted > 0); + CHECK_ARG(cumulative_device_gain > 0); + CHECK_ARG(received_noise_st_dev > 0); for (int i = 0; i < radar_scopes_count; ++i) { CHECK_ARG(radar_scopes[i].begin_distance >= 0); @@ -1083,11 +1082,11 @@ RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, const r createOrUpdateNode( node, std::vector{radar_scopes, radar_scopes + radar_scopes_count}, ray_azimuth_step, - ray_elevation_step, frequency, power_transmitted_dbm, antenna_gain_dbi, received_noise_mean_dbm, - received_noise_st_dev_dbm); + ray_elevation_step, frequency, power_transmitted, cumulative_device_gain, received_noise_mean, + received_noise_st_dev); }); TAPE_HOOK(node, TAPE_ARRAY(radar_scopes, radar_scopes_count), radar_scopes_count, ray_azimuth_step, ray_elevation_step, - frequency, power_transmitted_dbm, antenna_gain_dbi, received_noise_mean_dbm, received_noise_st_dev_dbm); + frequency, power_transmitted, cumulative_device_gain, received_noise_mean, received_noise_st_dev); return status; } diff --git a/src/graph/NodesCore.hpp b/src/graph/NodesCore.hpp index 25717df3f..22e59c1fb 100644 --- a/src/graph/NodesCore.hpp +++ b/src/graph/NodesCore.hpp @@ -476,8 +476,8 @@ struct RadarPostprocessPointsNode : IPointsNodeSingleInput using Ptr = std::shared_ptr; void setParameters(const std::vector& radarScopes, float rayAzimuthStepRad, float rayElevationStepRad, - float frequency, float powerTransmittedDbm, float antennaGainDbi, float receivedNoiseMean, - float receivedNoiseStdDev); + float frequency, float powerTransmitted, float cumulativeDeviceGain, float receivedNoiseMean, + float receivedNoiseStDev); // Node void validateImpl() override; @@ -520,9 +520,9 @@ struct RadarPostprocessPointsNode : IPointsNodeSingleInput float rayAzimuthStepRad; float rayElevationStepRad; - float frequency; + float frequencyHz; float powerTransmittedDbm; - float antennaGainDbi; + float cumulativeDeviceGainDbi; float receivedNoiseMeanDb; float receivedNoiseStDevDb; diff --git a/src/graph/RadarPostprocessPointsNode.cpp b/src/graph/RadarPostprocessPointsNode.cpp index c94ad504c..131152422 100644 --- a/src/graph/RadarPostprocessPointsNode.cpp +++ b/src/graph/RadarPostprocessPointsNode.cpp @@ -30,17 +30,17 @@ inline static std::optional getRadarScopeWithinDistance(const } void RadarPostprocessPointsNode::setParameters(const std::vector& radarScopes, float rayAzimuthStepRad, - float rayElevationStepRad, float frequency, float powerTransmittedDbm, - float antennaGainDbi, float receivedNoiseMean, float receivedNoiseStdDev) + float rayElevationStepRad, float frequency, float powerTransmitted, + float cumulativeDeviceGain, float receivedNoiseMean, float receivedNoiseStDev) { this->rayAzimuthStepRad = rayAzimuthStepRad; this->rayElevationStepRad = rayElevationStepRad; - this->frequency = frequency; + this->frequencyHz = frequency; this->radarScopes = radarScopes; - this->powerTransmittedDbm = powerTransmittedDbm; - this->antennaGainDbi = antennaGainDbi; + this->powerTransmittedDbm = powerTransmitted; + this->cumulativeDeviceGainDbi = cumulativeDeviceGain; this->receivedNoiseMeanDb = receivedNoiseMean; - this->receivedNoiseStDevDb = receivedNoiseStdDev; + this->receivedNoiseStDevDb = receivedNoiseStDev; } void RadarPostprocessPointsNode::validateImpl() @@ -66,7 +66,7 @@ void RadarPostprocessPointsNode::enqueueExecImpl() auto normalPtr = input->getFieldDataTyped()->asSubclass()->getReadPtr(); auto xyzPtr = input->getFieldDataTyped()->asSubclass()->getReadPtr(); outBUBRFactorDev->resize(input->getPointCount(), false, false); - gpuRadarComputeEnergy(getStreamHandle(), input->getPointCount(), rayAzimuthStepRad, rayElevationStepRad, frequency, + gpuRadarComputeEnergy(getStreamHandle(), input->getPointCount(), rayAzimuthStepRad, rayElevationStepRad, frequencyHz, input->getLookAtOriginTransform(), raysPtr, distancePtr, normalPtr, xyzPtr, outBUBRFactorDev->getWritePtr()); outBUBRFactorHost->copyFrom(outBUBRFactorDev); @@ -134,7 +134,7 @@ void RadarPostprocessPointsNode::enqueueExecImpl() filteredIndices->copyFromExternal(filteredIndicesHost.data(), filteredIndicesHost.size()); - const auto lambda = 299'792'458.0f / frequency; + const auto lambda = 299'792'458.0f / frequencyHz; const auto lambdaSqrtDbsm = 10.0f * log10f(lambda * lambda); // Compute per-cluster properties @@ -170,8 +170,8 @@ void RadarPostprocessPointsNode::enqueueExecImpl() const auto distance = distanceInputHost->at(filteredIndicesHost.at(clusterIdx)); const auto multiplier = 10.0f * log10f(powf(4 * std::numbers::pi_v, 3)) + 10.0f * log10f(powf(distance, 4)); - const auto powerReceived = powerTransmittedDbm + antennaGainDbi + antennaGainDbi + rcsDbsm + lambdaSqrtDbsm - - multiplier; + const auto powerReceived = powerTransmittedDbm + cumulativeDeviceGainDbi + cumulativeDeviceGainDbi + rcsDbsm + + lambdaSqrtDbsm - multiplier; clusterRcsHost->at(clusterIdx) = rcsDbsm; clusterNoiseHost->at(clusterIdx) = gaussianNoise(randomDevice); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 776984634..3c0096ec7 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -55,7 +55,7 @@ if (RGL_BUILD_ROS2_EXTENSION) src/graph/Ros2NodesTest.cpp src/graph/nodes/Ros2PublishRadarScanNodeTest.cpp src/graph/nodes/Ros2PublishPointsNodeTest.cpp - src/scene/radarTest.cpp + src/scene/radarTest.cpp ) endif() diff --git a/test/src/scene/radarTest.cpp b/test/src/scene/radarTest.cpp index 565e584b2..5c0119d4f 100644 --- a/test/src/scene/radarTest.cpp +++ b/test/src/scene/radarTest.cpp @@ -86,7 +86,7 @@ using namespace std::chrono_literals; TEST_F(RadarTest, rotating_reflector_2d) { - // GTEST_SKIP(); + GTEST_SKIP(); // Setup sensor and graph std::vector raysData = genRadarRays(); From 306eab961669aa9bc970da399647814131a7693f Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski Date: Tue, 26 Mar 2024 14:30:33 +0100 Subject: [PATCH 15/16] Remove rcsAngleDistributionTest --- test/src/scene/rcsAngleDistributionTest.cpp | 92 --------------------- 1 file changed, 92 deletions(-) delete mode 100644 test/src/scene/rcsAngleDistributionTest.cpp diff --git a/test/src/scene/rcsAngleDistributionTest.cpp b/test/src/scene/rcsAngleDistributionTest.cpp deleted file mode 100644 index 113184e06..000000000 --- a/test/src/scene/rcsAngleDistributionTest.cpp +++ /dev/null @@ -1,92 +0,0 @@ -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include "helpers/testPointCloud.hpp" - -struct RcsAngleDistributionTest : RGLTest -{}; - -const auto minAzimuth = -65.0f; -const auto maxAzimuth = 65.0f; -const auto minElevation = -7.5f; -const auto maxElevation = 7.5f; -const auto azimuthStep = 0.05f; -const auto elevationStep = 0.05f; -const auto azimuthRad = (maxAzimuth - minAzimuth) * std::numbers::pi_v / 180.0f; -const auto elevationRad = (maxElevation - minElevation) * std::numbers::pi_v / 180.0f; - -std::vector genRadarRays() -{ - std::vector rays; - for (auto a = minAzimuth; a <= maxAzimuth; a += azimuthStep) { - for (auto e = minElevation; e <= maxElevation; e += elevationStep) { - // By default, the rays are directed along the Z-axis - // So first, we rotate them around the Y-axis to point towards the X-axis (to be RVIZ2 compatible) - // Then, rotation around Z is azimuth, around Y is elevation - auto ray = Mat3x4f::rotationDeg(0, e + 90, -a); - rays.emplace_back(ray.toRGL()); - - // The above will have to be modified again - we assume that target is farther in X axis when in fact - // we use Z as RGL LiDAR front. Remember to update. - } - } - - return rays; -} - -TEST_F(RcsAngleDistributionTest, rotating_reflector_2d) -{ - GTEST_SKIP(); - // Load mesh - rgl_mesh_t reflector2dMesh = loadFromSTL("../../test/data/reflector2d.stl"); - - // Setup scene - rgl_entity_t reflector2d = nullptr; - EXPECT_RGL_SUCCESS(rgl_entity_create(&reflector2d, nullptr, reflector2dMesh)); - - // Setup sensor and graph - std::vector fields = {XYZ_VEC3_F32, DISTANCE_F32, NORMAL_VEC3_F32, RAY_IDX_U32, AZIMUTH_F32, ELEVATION_F32}; - std::vector rays = genRadarRays(); - rgl_node_t raysNode = nullptr, raytraceNode = nullptr, compactNode = nullptr, formatNode = nullptr, ros2Node = nullptr; - rgl_node_t radarNode = nullptr, yieldNode = nullptr; - EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&raysNode, rays.data(), rays.size())); - EXPECT_RGL_SUCCESS(rgl_node_raytrace(&raytraceNode, nullptr)); - EXPECT_RGL_SUCCESS(rgl_node_points_compact_by_field(&compactNode, RGL_FIELD_IS_HIT_I32)); - rgl_radar_scope_t radarScope{0.0f, 100.0f, 0.1f, 0.1f, 0.1f}; - EXPECT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarNode, &radarScope, 1, azimuthStep, elevationStep, 79E9f)); - EXPECT_RGL_SUCCESS(rgl_node_points_format(&formatNode, fields.data(), fields.size())); - EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&ros2Node, "rgl_test_topic", "rgl_test_frame_id")); - EXPECT_RGL_SUCCESS(rgl_node_points_yield(&yieldNode, fields.data(), fields.size())); - - EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(raysNode, raytraceNode)); - EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(raytraceNode, compactNode)); - EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(compactNode, radarNode)); - EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(radarNode, formatNode)); - - EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(formatNode, ros2Node)); - - for (float angle = -45; angle <= 45; angle += 0.1f) { - auto position = Vec3f{5, 0, 0}; - auto rotation = Vec3f{0, 0, angle}; - auto scale = Vec3f{1, 1, 1}; - - rgl_mat3x4f reflectorPose = Mat3x4f::TRS(position, rotation, scale).toRGL(); - EXPECT_RGL_SUCCESS(rgl_entity_set_pose(reflector2d, &reflectorPose)); - - EXPECT_RGL_SUCCESS(rgl_graph_run(raysNode)); - int32_t pointCount = 0, pointSize = 0; - EXPECT_RGL_SUCCESS(rgl_graph_get_result_size(radarNode, XYZ_VEC3_F32, &pointCount, &pointSize)); - } -} From c2838da8b9480e2bd1614ced86255b3bf6d4dd9f Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski Date: Tue, 26 Mar 2024 15:21:10 +0100 Subject: [PATCH 16/16] Review changes --- src/gpu/optixPrograms.cu | 2 +- test/src/scene/radarTest.cpp | 26 -------------------------- 2 files changed, 1 insertion(+), 27 deletions(-) diff --git a/src/gpu/optixPrograms.cu b/src/gpu/optixPrograms.cu index c775b0b05..ba248a71a 100644 --- a/src/gpu/optixPrograms.cu +++ b/src/gpu/optixPrograms.cu @@ -228,7 +228,7 @@ extern "C" __global__ void __closesthit__() Vec3f absPointVelocity{NAN}; Vec3f relPointVelocity{NAN}; - float radialSpeed{0.014}; + float radialSpeed{NAN}; bool isVelocityRequested = ctx.pointAbsVelocity != nullptr || ctx.pointRelVelocity != nullptr || ctx.radialSpeed != nullptr; if (ctx.sceneDeltaTime > 0 && isVelocityRequested) { Vec3f displacementFromTransformChange = {0, 0, 0}; diff --git a/test/src/scene/radarTest.cpp b/test/src/scene/radarTest.cpp index 5c0119d4f..89aba4282 100644 --- a/test/src/scene/radarTest.cpp +++ b/test/src/scene/radarTest.cpp @@ -176,30 +176,4 @@ TEST_F(RadarTest, rotating_reflector_2d) // //} // } } - - // Setup scene - // std::vector reflectors2D; - // reflectors2D.resize(3, nullptr); - // rgl_mesh_t reflector2dMesh = loadFromSTL(fs::path(RGL_TEST_DATA_DIR) / "reflector2d.stl"); - // for (auto&& reflector2D : reflectors2D) { - // EXPECT_RGL_SUCCESS(rgl_entity_create(&reflector2D, nullptr, reflector2dMesh)); - // } - // - // uint64_t time = 0; - // for (float angle = -45; angle <= 45; angle += 0.1f) { - // EXPECT_RGL_SUCCESS(rgl_scene_set_time(nullptr, time)); - // auto position = Vec3f{5, 0, 0}; - // auto rotation = Vec3f{0, 0, angle}; - // auto scale = Vec3f{1, 1, 1}; - // - // auto offset = Vec3f{0, -3, 0}; - // for (auto&& reflector2D : reflectors2D) { - // rgl_mat3x4f reflectorPose = Mat3x4f::TRS(position + offset, rotation, scale).toRGL(); - // EXPECT_RGL_SUCCESS(rgl_entity_set_pose(reflector2D, &reflectorPose)); - // offset += Vec3f{0, 3, 0}; - // } - // - // EXPECT_RGL_SUCCESS(rgl_graph_run(rays)); - // time += 10'000'000; // 10 ms - // } }