diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h index 868ffad1b..fdc62a595 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 /** @@ -765,11 +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 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 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 c4f27cba7..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,17 +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 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={})", - repr(node), repr(radar_scopes, radar_scopes_count), ray_azimuth_step, ray_elevation_step, frequency); + "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, 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 > 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); @@ -1076,10 +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); + 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); + frequency, power_transmitted, cumulative_device_gain, received_noise_mean, received_noise_st_dev); return status; } @@ -1088,7 +1095,9 @@ 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(), yamlNode[8].as(), + yamlNode[9].as()); state.nodes.insert({nodeId, node}); } diff --git a/src/gpu/nodeKernels.cu b/src/gpu/nodeKernels.cu index e4332cbed..63a2e98b0 100644 --- a/src/gpu/nodeKernels.cu +++ b/src/gpu/nodeKernels.cu @@ -81,13 +81,18 @@ __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(); 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); @@ -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,40 +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 thrust::complex kr = {waveNum * hitDist[tid], 0.0f}; - - 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 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 Vec3f vecK = waveNum * ((dirX * cp + dirY * sp) * st + dirZ * ct); - - const float rayArea = hitDist[tid] * hitDist[tid] * std::sin(rayElevationStepRad) * rayAzimuthStepRad; - - 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)))) * - 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()); + 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); + + 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 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; + + 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) * reflectedPolCplx; + const Vector<3, thrust::complex> apH = -apE.cross(reflectedDir); + + 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}; } @@ -228,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/gpu/optixPrograms.cu b/src/gpu/optixPrograms.cu index fad61c9ff..ba248a71a 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__() diff --git a/src/graph/NodesCore.hpp b/src/graph/NodesCore.hpp index 4ad7c0c2f..22e59c1fb 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 frequency, float powerTransmitted, float cumulativeDeviceGain, float receivedNoiseMean, + float receivedNoiseStDev); // Node void validateImpl() override; @@ -507,12 +508,28 @@ struct RadarPostprocessPointsNode : IPointsNodeSingleInput HostPinnedArray>>::Ptr outBUBRFactorHost = HostPinnedArray>>::create(); + 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; - float frequency; + float frequencyHz; + float powerTransmittedDbm; + float cumulativeDeviceGainDbi; + float receivedNoiseMeanDb; + float receivedNoiseStDevDb; std::vector radarScopes; + std::random_device randomDevice; + // RGL related members std::mutex getFieldDataMutex; mutable CacheManager cacheManager; @@ -530,7 +547,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..131152422 100644 --- a/src/graph/RadarPostprocessPointsNode.cpp +++ b/src/graph/RadarPostprocessPointsNode.cpp @@ -30,12 +30,17 @@ inline static std::optional getRadarScopeWithinDistance(const } void RadarPostprocessPointsNode::setParameters(const std::vector& radarScopes, float rayAzimuthStepRad, - float rayElevationStepRad, float frequency) + 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 = powerTransmitted; + this->cumulativeDeviceGainDbi = cumulativeDeviceGain; + this->receivedNoiseMeanDb = receivedNoiseMean; + this->receivedNoiseStDevDb = receivedNoiseStDev; } void RadarPostprocessPointsNode::validateImpl() @@ -61,23 +66,12 @@ 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, frequencyHz, + input->getLookAtOriginTransform(), raysPtr, distancePtr, normalPtr, xyzPtr, + outBUBRFactorDev->getWritePtr()); 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 +134,56 @@ void RadarPostprocessPointsNode::enqueueExecImpl() filteredIndices->copyFromExternal(filteredIndicesHost.data(), filteredIndicesHost.size()); + const auto lambda = 299'792'458.0f / frequencyHz; + const auto lambdaSqrtDbsm = 10.0f * log10f(lambda * lambda); + + // Compute per-cluster properties + clusterRcsHost->resize(filteredIndicesHost.size(), false, false); + 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 < clusters.size(); ++clusterIdx) { + std::complex AU = 0; + std::complex AR = 0; + auto&& cluster = clusters[clusterIdx]; + + 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(), + outBUBRFactorHost->at(pointInCluster)[1].imag()}; + std::complex factor = {outBUBRFactorHost->at(pointInCluster)[2].real(), + outBUBRFactorHost->at(pointInCluster)[2].imag()}; + AU += BU * factor; + AR += BR * factor; + } + + // https://en.wikipedia.org/wiki/Radar_cross_section#Formulation + 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)) { + 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 powerReceived = powerTransmittedDbm + cumulativeDeviceGainDbi + cumulativeDeviceGainDbi + rcsDbsm + + lambdaSqrtDbsm - multiplier; + + clusterRcsHost->at(clusterIdx) = rcsDbsm; + 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); + clusterSnrDev->copyFrom(clusterSnrHost); + // 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) @@ -163,6 +207,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..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/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/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 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/TapeTest.cpp b/test/src/TapeTest.cpp index f4b27ccd0..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)); + 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 cd699ecc6..3e75a1781 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, 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 new file mode 100644 index 000000000..89aba4282 --- /dev/null +++ b/test/src/scene/radarTest.cpp @@ -0,0 +1,179 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include "helpers/testPointCloud.hpp" + +#include +struct numpunct : std::numpunct +{ +protected: + char do_decimal_point() const override { return ','; } +}; + +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; + +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; + 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 + 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 + // 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()); + } + } + + 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(); + // 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, + .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; + + rgl_node_t raysTransform = nullptr; + 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, 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)); + 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, 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)); + + 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)); + + // 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)); + 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)); + // //} + // } + } +} 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)); - } -}