Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Compute radar power & noise levels, improve rcs #264

Merged
merged 16 commits into from
Mar 26, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 20 additions & 10 deletions include/rgl/api/core.h
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,10 @@
#include <stddef.h>
#include <stdint.h>

#ifdef __cplusplus
#include <type_traits>
#endif

#ifdef __cplusplus
#define NO_MANGLING extern "C"
#else // NOT __cplusplus
Expand Down Expand Up @@ -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<rgl_vec2f>);
static_assert(std::is_standard_layout_v<rgl_vec2f>);
Expand All @@ -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<rgl_vec3f>);
static_assert(std::is_standard_layout_v<rgl_vec3f>);
Expand All @@ -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<rgl_vec3i>);
static_assert(std::is_standard_layout_v<rgl_vec3i>);
Expand All @@ -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<rgl_mat3x4f>);
static_assert(std::is_standard_layout_v<rgl_mat3x4f>);
Expand Down Expand Up @@ -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<rgl_radar_separations_t>);
static_assert(std::is_standard_layout_v<rgl_radar_separations_t>);
#ifdef __cplusplus
static_assert(sizeof(rgl_radar_scope_t) == 5 * sizeof(float));
static_assert(std::is_trivial_v<rgl_radar_scope_t>);
static_assert(std::is_standard_layout_v<rgl_radar_scope_t>);
#endif

/**
Expand Down Expand Up @@ -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.
Expand Down
29 changes: 19 additions & 10 deletions src/api/apiCore.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -869,12 +869,11 @@ void TapeCore::tape_node_raytrace_configure_distortion(const YAML::Node& yamlNod
rgl_node_raytrace_configure_distortion(node, yamlNode[1].as<bool>());
}

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<RaytraceNode>(node);
raytraceNode->setNonHitDistanceValues(nearDistance, farDistance);
Expand Down Expand Up @@ -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);
Expand All @@ -1076,10 +1082,11 @@ RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, const r

createOrUpdateNode<RadarPostprocessPointsNode>(
node, std::vector<rgl_radar_scope_t>{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;
}

Expand All @@ -1088,7 +1095,9 @@ void TapeCore::tape_node_points_radar_postprocess(const YAML::Node& yamlNode, Pl
auto nodeId = yamlNode[0].as<TapeAPIObjectID>();
rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr;
rgl_node_points_radar_postprocess(&node, state.getPtr<const rgl_radar_scope_t>(yamlNode[1]), yamlNode[2].as<int32_t>(),
yamlNode[3].as<float>(), yamlNode[4].as<float>(), yamlNode[5].as<float>());
yamlNode[3].as<float>(), yamlNode[4].as<float>(), yamlNode[5].as<float>(),
yamlNode[6].as<float>(), yamlNode[7].as<float>(), yamlNode[8].as<float>(),
yamlNode[9].as<float>());
state.nodes.insert({nodeId, node});
}

Expand Down
152 changes: 101 additions & 51 deletions src/gpu/nodeKernels.cu
Original file line number Diff line number Diff line change
Expand Up @@ -81,13 +81,18 @@ __global__ void kFilter(size_t count, const Field<RAY_IDX_U32>::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);
Expand All @@ -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<RAY_POSE_MAT3x4_F32>::type* rayPose, const Field<DISTANCE_F32>::type* hitDist,
const Field<NORMAL_VEC3_F32>::type* hitNorm, const Field<XYZ_VEC3_F32>::type* hitPos,
Vector<3, thrust::complex<float>>* outBUBRFactor)
Mat3x4f lookAtOriginTransform, const Field<RAY_POSE_MAT3x4_F32>::type* rayPose,
const Field<DISTANCE_F32>::type* hitDist, const Field<NORMAL_VEC3_F32>::type* hitNorm,
const Field<XYZ_VEC3_F32>::type* hitPos, Vector<3, thrust::complex<float>>* outBUBRFactor)
{
LIMIT(count);

constexpr bool log = false;

constexpr float c0 = 299792458.0f;
constexpr float reflectionCoef = 1.0f; // TODO
const float waveLen = c0 / freq;
Expand All @@ -111,51 +118,95 @@ __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
const float sp = sinf(phi); // Y-dir component
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<float> 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<float>> rayPolCplx = {reflectedPol.x(), reflectedPol.y(), reflectedPol.z()};

const Vector<3, thrust::complex<float>> apE = reflectionCoef * exp(i * kr) * rayPolCplx;
const Vector<3, thrust::complex<float>> 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<float> BU = (-(apE.cross(-dirP) + apH.cross(dirT))).dot(rayDir);
thrust::complex<float> BR = (-(apE.cross(dirT) + apH.cross(dirP))).dot(rayDir);
thrust::complex<float> factor = thrust::complex<float>(0.0, ((waveNum * rayArea) / (4.0f * static_cast<float>(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<float>> 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<float>> apE = reflectionCoef * exp(i * kr) * reflectedPolCplx;
const Vector<3, thrust::complex<float>> 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<float>> BU1 = apE.cross(-dirP);
const Vector<3, thrust::complex<float>> BU2 = apH.cross(dirT);
const Vector<3, thrust::complex<float>> 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<float>> BR1 = apE.cross(dirT);
const Vector<3, thrust::complex<float>> BR2 = apH.cross(dirP);
const Vector<3, thrust::complex<float>> 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<float> BU = refField1.dot(reflectedDir);
const thrust::complex<float> BR = refField2.dot(reflectedDir);
// const thrust::complex<float> factor = thrust::complex<float>(0.0, ((waveNum * rayArea) / (4.0f * static_cast<float>(M_PI)))) *
// exp(-i * waveNum * hitDistance);
const thrust::complex<float> factor = thrust::complex<float>(0.0, ((waveNum * rayArea * reflectedDir.dot(hitNormalLocal)) /
(4.0f * static_cast<float>(M_PI)))) *
exp(-i * waveNum * hitDistance);
// const thrust::complex<float> factor = thrust::complex<float>(0.0, ((waveNum * rayArea) / (4.0f * static_cast<float>(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};
}
Expand Down Expand Up @@ -228,20 +279,19 @@ void gpuFilter(cudaStream_t stream, size_t count, const Field<RAY_IDX_U32>::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<XYZ_VEC3_F32>::type* inPoints,
const Field<NORMAL_VEC3_F32>::type* inNormalsPtr, Field<IS_GROUND_I32>::type* outNonGround,
Mat3x4f lidarTransform)
void gpuFilterGroundPoints(cudaStream_t stream, size_t pointCount, const Vec3f sensor_up_vector, float ground_angle_threshold,
const Field<XYZ_VEC3_F32>::type* inPoints, const Field<NORMAL_VEC3_F32>::type* inNormalsPtr,
Field<IS_GROUND_I32>::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<RAY_POSE_MAT3x4_F32>::type* rayPose, const Field<DISTANCE_F32>::type* hitDist,
const Field<NORMAL_VEC3_F32>::type* hitNorm, const Field<XYZ_VEC3_F32>::type* hitPos,
Vector<3, thrust::complex<float>>* outBUBRFactor)
Mat3x4f lookAtOriginTransform, const Field<RAY_POSE_MAT3x4_F32>::type* rayPose,
msz-rai marked this conversation as resolved.
Show resolved Hide resolved
const Field<DISTANCE_F32>::type* hitDist, const Field<NORMAL_VEC3_F32>::type* hitNorm,
const Field<XYZ_VEC3_F32>::type* hitPos, Vector<3, thrust::complex<float>>* 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);
}
Loading
Loading