2020
2121#include < thrust/device_ptr.h>
2222#include < thrust/scan.h>
23+ #include < thrust/complex.h>
2324
2425__global__ void kFormatSoaToAos (size_t pointCount, size_t pointSize, size_t fieldCount, const GPUFieldDesc* soaInData,
2526 char * aosOutData)
@@ -78,6 +79,87 @@ __global__ void kFilter(size_t count, const Field<RAY_IDX_U32>::type* indices, c
7879 memcpy (dst + tid * fieldSize, src + indices[tid] * fieldSize, fieldSize);
7980}
8081
82+ __device__ Vec3f reflectPolarization (const Vec3f& pol, const Vec3f& hitNormal, const Vec3f& rayDir)
83+ {
84+ const auto diffCrossNormal = rayDir.cross (hitNormal);
85+ const auto polU = diffCrossNormal.normalized ();
86+ const auto polR = rayDir.cross (polU).normalized ();
87+
88+ const auto refDir = (rayDir - hitNormal * (2 * rayDir.dot (hitNormal))).normalized ();
89+ const auto refPolU = -1 .0f * polU;
90+ const auto refPolR = rayDir.cross (refPolU);
91+
92+ const auto polCompU = pol.dot (polU);
93+ const auto polCompR = pol.dot (polR);
94+
95+ return -polCompR * refPolR + polCompU * refPolU;
96+ }
97+
98+ __global__ void kRadarComputeEnergy (size_t count, float rayAzimuthStepRad, float rayElevationStepRad, float freq,
99+ const Field<RAY_POSE_MAT3x4_F32>::type* rayPose, const Field<DISTANCE_F32>::type* hitDist,
100+ const Field<NORMAL_VEC3_F32>::type* hitNorm, const Field<XYZ_VEC3_F32>::type* hitPos,
101+ Vector<3 , thrust::complex <float >>* outBUBRFactor)
102+ {
103+ LIMIT (count);
104+
105+ constexpr float c0 = 299792458 .0f ;
106+ constexpr float reflectionCoef = 1 .0f ; // TODO
107+ const float waveLen = c0 / freq;
108+ const float waveNum = 2 .0f * M_PIf / waveLen;
109+ const thrust::complex <float > i = {0 , 1.0 };
110+ const Vec3f dirX = {1 , 0 , 0 };
111+ const Vec3f dirY = {0 , 1 , 0 };
112+ const Vec3f dirZ = {0 , 0 , 1 };
113+
114+ const Vec3f rayDirCts = rayPose[tid] * Vec3f{0 , 0 , 1 };
115+ const Vec3f rayDirSph = rayDirCts.toSpherical ();
116+ const float phi = rayDirSph[1 ]; // azimuth, 0 = X-axis, positive = CCW
117+ const float the = rayDirSph[2 ]; // elevation, 0 = Z-axis, 90 = XY-plane, -180 = negative Z-axis
118+
119+ // Consider unit vector of the ray direction, these are its projections:
120+ const float cp = cosf (phi); // X-dir component
121+ const float sp = sinf (phi); // Y-dir component
122+ const float ct = cosf (the); // Z-dir component
123+ const float st = sinf (the); // XY-plane component
124+
125+ const Vec3f dirP = {-sp, cp, 0 };
126+ const Vec3f dirT = {cp * ct, sp * ct, -st};
127+
128+ const thrust::complex <float > kr = {waveNum * hitDist[tid], 0 .0f };
129+
130+ const Vec3f rayDir = rayDirCts.normalized ();
131+ const Vec3f rayPol = rayPose[tid] * Vec3f{-1 , 0 , 0 }; // UP, perpendicular to ray
132+ const Vec3f reflectedPol = reflectPolarization (rayPol, hitNorm[tid], rayDir);
133+
134+ const Vector<3 , thrust::complex <float >> rayPolCplx = {reflectedPol.x (), reflectedPol.y (), reflectedPol.z ()};
135+
136+ const Vector<3 , thrust::complex <float >> apE = reflectionCoef * exp (i * kr) * rayPolCplx;
137+ const Vector<3 , thrust::complex <float >> apH = -apE.cross (rayDir);
138+
139+ const Vec3f vecK = waveNum * ((dirX * cp + dirY * sp) * st + dirZ * ct);
140+
141+ const float rayArea = hitDist[tid] * hitDist[tid] * std::sin (rayElevationStepRad) * rayAzimuthStepRad;
142+
143+ thrust::complex <float > BU = (-(apE.cross (-dirP) + apH.cross (dirT))).dot (rayDir);
144+ thrust::complex <float > BR = (-(apE.cross (dirT) + apH.cross (dirP))).dot (rayDir);
145+ thrust::complex <float > factor = thrust::complex <float >(0.0 , ((waveNum * rayArea) / (4 .0f * M_PIf))) *
146+ exp (-i * vecK.dot (hitPos[tid]));
147+
148+ // 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],
149+ // hitPos[tid].x(), hitPos[tid].y(), hitPos[tid].z(), hitNorm[tid].x(), hitNorm[tid].y(), hitNorm[tid].z(),
150+ // BU.real(), BU.imag(), BR.real(), BR.imag(), factor.real(), factor.imag());
151+ // Print variables:
152+ // printf("GPU: point=%d ray=??: rayDirCts=(%.2f, %.2f, %.2f), rayDirSph=(%.2f, %.2f, %.2f), phi=%.2f, the=%.2f, cp=%.2f, "
153+ // "sp=%.2f, ct=%.2f, st=%.2f, dirP=(%.2f, %.2f, %.2f), dirT=(%.2f, %.2f, %.2f), kr=(%.2f+%.2fi), rayDir=(%.2f, "
154+ // "%.2f, %.2f), rayPol=(%.2f, %.2f, %.2f), reflectedPol=(%.2f, %.2f, %.2f)\n",
155+ // tid, rayDirCts.x(), rayDirCts.y(), rayDirCts.z(), rayDirSph.x(), rayDirSph.y(),
156+ // rayDirSph.z(), phi, the, cp, sp, ct, st, dirP.x(), dirP.y(), dirP.z(), dirT.x(), dirT.y(), dirT.z(), kr.real(),
157+ // kr.imag(), rayDir.x(), rayDir.y(), rayDir.z(), rayPol.x(), rayPol.y(), rayPol.z(), reflectedPol.x(),
158+ // reflectedPol.y(), reflectedPol.z());
159+
160+ outBUBRFactor[tid] = {BU, BR, factor};
161+ }
162+
81163__global__ void kFilterGroundPoints (size_t pointCount, const Vec3f sensor_up_vector, float ground_angle_threshold,
82164 const Field<XYZ_VEC3_F32>::type* inPoints, const Field<NORMAL_VEC3_F32>::type* inNormalsPtr,
83165 Field<IS_GROUND_I32>::type* outNonGround, Mat3x4f lidarTransform)
@@ -90,6 +172,7 @@ __global__ void kFilterGroundPoints(size_t pointCount, const Vec3f sensor_up_vec
90172 outNonGround[tid] = normalUpAngle > ground_angle_threshold;
91173}
92174
175+
93176void gpuFindCompaction (cudaStream_t stream, size_t pointCount, const int32_t * shouldCompact,
94177 CompactionIndexType* hitCountInclusive, size_t * outHitCount)
95178{
@@ -153,3 +236,12 @@ void gpuFilterGroundPoints(cudaStream_t stream, size_t pointCount, const Vec3f s
153236 run (kFilterGroundPoints , stream, pointCount, sensor_up_vector, ground_angle_threshold, inPoints, inNormalsPtr, outNonGround,
154237 lidarTransform);
155238}
239+
240+ void gpuRadarComputeEnergy (cudaStream_t stream, size_t count, float rayAzimuthStepRad, float rayElevationStepRad, float freq,
241+ const Field<RAY_POSE_MAT3x4_F32>::type* rayPose, const Field<DISTANCE_F32>::type* hitDist,
242+ const Field<NORMAL_VEC3_F32>::type* hitNorm, const Field<XYZ_VEC3_F32>::type* hitPos,
243+ Vector<3 , thrust::complex <float >>* outBUBRFactor)
244+ {
245+ run (kRadarComputeEnergy , stream, count, rayAzimuthStepRad, rayElevationStepRad, freq, rayPose, hitDist, hitNorm, hitPos,
246+ outBUBRFactor);
247+ }
0 commit comments