Skip to content

Commit

Permalink
Improve rcs (#273)
Browse files Browse the repository at this point in the history 
* Add debug development chnages.

Signed-off-by: Paweł Liberadzki <[email protected]>

* Add fixes for RCS calculation.

Signed-off-by: Paweł Liberadzki <[email protected]>

* Reduce mess in RCS calculation kernel.

Signed-off-by: Paweł Liberadzki <[email protected]>

* Remove not used variable.

Signed-off-by: Paweł Liberadzki <[email protected]>

* Remove not necessary dependency on hit position.

Signed-off-by: Paweł Liberadzki <[email protected]>

* 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 <[email protected]>

* 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 <[email protected]>
Co-authored-by: Paweł Liberadzki <[email protected]>
  • Loading branch information
@msz-rai @PawelLiberadzki
msz-rai and PawelLiberadzki committed Mar 26, 2024
1 parent 01e3333 commit df81a72
Show file tree
Hide file tree
Showing 4 changed files with 211 additions and 79 deletions.
140 changes: 94 additions & 46 deletions src/gpu/nodeKernels.cu
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,11 @@ __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();
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,53 +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 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<float>> reflectedPolCplx = {reflectedPol.x(), reflectedPol.y(), reflectedPol.z()};
const float kr = waveNum * hitDistance;

const Vector<3, thrust::complex<float>> 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<float>> apE = reflectionCoef * exp(i * kr) * rayPolCplx;
const Vector<3, thrust::complex<float>> apE = reflectionCoef * exp(i * kr) * reflectedPolCplx;
const Vector<3, thrust::complex<float>> 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<float> BU = (-(apE.cross(-dirP) + apH.cross(dirT))).dot(reflectedDir);
thrust::complex<float> BR = (-(apE.cross(dirT) + apH.cross(dirP))).dot(reflectedDir);
thrust::complex<float> factor = thrust::complex<float>(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<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 @@ -230,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,
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);
}
10 changes: 5 additions & 5 deletions src/gpu/nodeKernels.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -48,6 +48,6 @@ void gpuFilterGroundPoints(cudaStream_t stream, size_t pointCount, const Vec3f s
const Field<XYZ_VEC3_F32>::type* inPoints, const Field<NORMAL_VEC3_F32>::type* inNormalsPtr,
Field<IS_GROUND_I32>::type* outNonGround, Mat3x4f 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,
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);
13 changes: 10 additions & 3 deletions src/graph/RadarPostprocessPointsNode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,8 +66,9 @@ void RadarPostprocessPointsNode::enqueueExecImpl()
auto normalPtr = input->getFieldDataTyped<NORMAL_VEC3_F32>()->asSubclass<DeviceAsyncArray>()->getReadPtr();
auto xyzPtr = input->getFieldDataTyped<XYZ_VEC3_F32>()->asSubclass<DeviceAsyncArray>()->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()));

Expand Down Expand Up @@ -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<float> * (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<float>, 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);
Expand Down
Loading

0 comments on commit df81a72

Please sign in to comment.