@@ -81,13 +81,18 @@ __global__ void kFilter(size_t count, const Field<RAY_IDX_U32>::type* indices, c
8181
8282__device__ Vec3f reflectPolarization (const Vec3f& pol, const Vec3f& hitNormal, const Vec3f& rayDir)
8383{
84+ // Normal incidence
85+ if (abs (rayDir.dot (hitNormal)) == 1 ) {
86+ return -pol;
87+ }
88+
8489 const auto diffCrossNormal = rayDir.cross (hitNormal);
8590 const auto polU = diffCrossNormal.normalized ();
8691 const auto polR = rayDir.cross (polU).normalized ();
8792
8893 const auto refDir = (rayDir - hitNormal * (2 * rayDir.dot (hitNormal))).normalized ();
8994 const auto refPolU = -1 .0f * polU;
90- const auto refPolR = rayDir .cross (refPolU);
95+ const auto refPolR = refDir .cross (refPolU);
9196
9297 const auto polCompU = pol.dot (polU);
9398 const auto polCompR = pol.dot (polR);
@@ -96,12 +101,14 @@ __device__ Vec3f reflectPolarization(const Vec3f& pol, const Vec3f& hitNormal, c
96101}
97102
98103__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)
104+ Mat3x4f lookAtOriginTransform , const Field<RAY_POSE_MAT3x4_F32 >::type* rayPose ,
105+ const Field<DISTANCE_F32 >::type* hitDist , const Field<NORMAL_VEC3_F32 >::type* hitNorm ,
106+ const Field<XYZ_VEC3_F32>::type* hitPos, Vector<3 , thrust::complex <float >>* outBUBRFactor)
102107{
103108 LIMIT (count);
104109
110+ constexpr bool log = false ;
111+
105112 constexpr float c0 = 299792458 .0f ;
106113 constexpr float reflectionCoef = 1 .0f ; // TODO
107114 const float waveLen = c0 / freq;
@@ -111,51 +118,95 @@ __global__ void kRadarComputeEnergy(size_t count, float rayAzimuthStepRad, float
111118 const Vec3f dirY = {0 , 1 , 0 };
112119 const Vec3f dirZ = {0 , 0 , 1 };
113120
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
121+ const Mat3x4f rayPoseLocal = lookAtOriginTransform * rayPose[tid];
122+ // const Vec3f hitPosLocal = lookAtOriginTransform * hitPos[tid];
123+ const Vec3f rayDir = rayPoseLocal * Vec3f{0 , 0 , 1 };
124+ const Vec3f rayPol = rayPoseLocal * Vec3f{1 , 0 , 0 }; // UP, perpendicular to ray
125+ const Vec3f hitNormalLocal = lookAtOriginTransform.rotation () * hitNorm[tid];
126+ const float hitDistance = hitDist[tid];
127+ const float rayArea = hitDistance * hitDistance * sinf (rayElevationStepRad) * rayAzimuthStepRad;
128+
129+ if (log)
130+ printf (" rayDir: (%.4f %.4f %.4f) rayPol: (%.4f %.4f %.4f) hitNormal: (%.4f %.4f %.4f)\n " x (), rayDir.y (),
131+ rayDir.z (), rayPol.x (), rayPol.y (), rayPol.z (), hitNormalLocal.x (), hitNormalLocal.y (), hitNormalLocal.z ());
132+
133+ const float phi = atan2 (rayDir[1 ], rayDir[2 ]);
134+ const float the = acos (rayDir[0 ] / rayDir.length ());
118135
119136 // Consider unit vector of the ray direction, these are its projections:
120137 const float cp = cosf (phi); // X-dir component
121138 const float sp = sinf (phi); // Y-dir component
122139 const float ct = cosf (the); // Z-dir component
123140 const float st = sinf (the); // XY-plane component
124141
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 * static_cast <float >(M_PI)))) *
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());
142+ const Vec3f dirP = {0 , cp, -sp};
143+ const Vec3f dirT = {-st, sp * ct, cp * ct};
144+ const Vec3f vecK = waveNum * ((dirZ * cp + dirY * sp) * st + dirX * ct);
145+
146+ if (log)
147+ printf (" phi: %.2f [dirP: (%.4f %.4f %.4f)] theta: %.2f [dirT: (%.4f %.4f %.4f)] vecK=(%.2f, %.2f, %.2f)\n "
148+ dirP.x (), dirP.y (), dirP.z (), the, dirT.x (), dirT.y (), dirT.z (), vecK.x (), vecK.y (), vecK.z ());
149+
150+ const Vec3f reflectedDir = (rayDir - hitNormalLocal * (2 * rayDir.dot (hitNormalLocal))).normalized ();
151+ const Vec3f reflectedPol = reflectPolarization (rayPol, hitNormalLocal, rayDir);
152+ const Vector<3 , thrust::complex <float >> reflectedPolCplx = {reflectedPol.x (), reflectedPol.y (), reflectedPol.z ()};
153+ const float kr = waveNum * hitDistance;
154+
155+ if (log)
156+ printf (" reflectedDir: (%.4f %.4f %.4f) reflectedPol: (%.4f %.4f %.4f)\n " x (), reflectedDir.y (),
157+ reflectedDir.z (), reflectedPol.x (), reflectedPol.y (), reflectedPol.z ());
158+
159+ const Vector<3 , thrust::complex <float >> apE = reflectionCoef * exp (i * kr) * reflectedPolCplx;
160+ const Vector<3 , thrust::complex <float >> apH = -apE.cross (reflectedDir);
161+
162+ if (log)
163+ printf (" apE: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n " x ().real (), apE.x ().imag (), apE.y ().real (),
164+ apE.y ().imag (), apE.z ().real (), apE.z ().imag ());
165+ if (log)
166+ printf (" apH: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n " x ().real (), apH.x ().imag (), apH.y ().real (),
167+ apH.y ().imag (), apH.z ().real (), apH.z ().imag ());
168+
169+ const Vector<3 , thrust::complex <float >> BU1 = apE.cross (-dirP);
170+ const Vector<3 , thrust::complex <float >> BU2 = apH.cross (dirT);
171+ const Vector<3 , thrust::complex <float >> refField1 = (-(BU1 + BU2));
172+
173+ if (log)
174+ printf (" BU1: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n "
175+ " BU2: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n "
176+ " refField1: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n "
177+ BU1.x ().real (), BU1.x ().imag (), BU1.y ().real (), BU1.y ().imag (), BU1.z ().real (), BU1.z ().imag (), BU2.x ().real (),
178+ BU2.x ().imag (), BU2.y ().real (), BU2.y ().imag (), BU2.z ().real (), BU2.z ().imag (), refField1.x ().real (),
179+ refField1.x ().imag (), refField1.y ().real (), refField1.y ().imag (), refField1.z ().real (), refField1.z ().imag ());
180+
181+ const Vector<3 , thrust::complex <float >> BR1 = apE.cross (dirT);
182+ const Vector<3 , thrust::complex <float >> BR2 = apH.cross (dirP);
183+ const Vector<3 , thrust::complex <float >> refField2 = (-(BR1 + BR2));
184+
185+ if (log)
186+ printf (" BR1: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n "
187+ " BR2: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n "
188+ " refField2: [(%.2f + %.2fi) (%.2f + %.2fi) (%.2f + %.2fi)]\n "
189+ BR1.x ().real (), BR1.x ().imag (), BR1.y ().real (), BR1.y ().imag (), BR1.z ().real (), BR1.z ().imag (), BR2.x ().real (),
190+ BR2.x ().imag (), BR2.y ().real (), BR2.y ().imag (), BR2.z ().real (), BR2.z ().imag (), refField2.x ().real (),
191+ refField2.x ().imag (), refField2.y ().real (), refField2.y ().imag (), refField2.z ().real (), refField2.z ().imag ());
192+
193+ const thrust::complex <float > BU = refField1.dot (reflectedDir);
194+ const thrust::complex <float > BR = refField2.dot (reflectedDir);
195+ // const thrust::complex<float> factor = thrust::complex<float>(0.0, ((waveNum * rayArea) / (4.0f * static_cast<float>(M_PI)))) *
196+ // exp(-i * waveNum * hitDistance);
197+ const thrust::complex <float > factor = thrust::complex <float >(0.0 , ((waveNum * rayArea * reflectedDir.dot (hitNormalLocal)) /
198+ (4 .0f * static_cast <float >(M_PI)))) *
199+ exp (-i * waveNum * hitDistance);
200+ // const thrust::complex<float> factor = thrust::complex<float>(0.0, ((waveNum * rayArea) / (4.0f * static_cast<float>(M_PI)))) *
201+ // exp(-i * vecK.dot(hitPosLocal));
202+
203+ const auto BUf = BU * factor;
204+ const auto BRf = BR * factor;
205+
206+ if (log)
207+ printf (" BU: (%.2f + %.2fi) BR: (%.2f + %.2fi) factor: (%.2f + %.2fi) [BUf: (%.2f + %.2fi) BRf: %.2f + %.2fi]\n "
208+ BU.real (), BU.imag (), BR.real (), BR.imag (), factor.real (), factor.imag (), BUf.real (), BUf.imag (), BRf.real (),
209+ BRf.imag ());
159210
160211 outBUBRFactor[tid] = {BU, BR, factor};
161212}
@@ -228,20 +279,19 @@ void gpuFilter(cudaStream_t stream, size_t count, const Field<RAY_IDX_U32>::type
228279 run (kFilter , stream, count, indices, dst, src, fieldSize);
229280}
230281
231- void gpuFilterGroundPoints (cudaStream_t stream, size_t pointCount, const Vec3f sensor_up_vector,
232- float ground_angle_threshold, const Field<XYZ_VEC3_F32>::type* inPoints,
233- const Field<NORMAL_VEC3_F32>::type* inNormalsPtr, Field<IS_GROUND_I32>::type* outNonGround,
234- Mat3x4f lidarTransform)
282+ void gpuFilterGroundPoints (cudaStream_t stream, size_t pointCount, const Vec3f sensor_up_vector, float ground_angle_threshold,
283+ const Field<XYZ_VEC3_F32>::type* inPoints, const Field<NORMAL_VEC3_F32>::type* inNormalsPtr,
284+ Field<IS_GROUND_I32>::type* outNonGround, Mat3x4f lidarTransform)
235285{
236286 run (kFilterGroundPoints , stream, pointCount, sensor_up_vector, ground_angle_threshold, inPoints, inNormalsPtr, outNonGround,
237287 lidarTransform);
238288}
239289
240290void 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)
291+ Mat3x4f lookAtOriginTransform , const Field<RAY_POSE_MAT3x4_F32 >::type* rayPose ,
292+ const Field<DISTANCE_F32 >::type* hitDist , const Field<NORMAL_VEC3_F32 >::type* hitNorm ,
293+ const Field<XYZ_VEC3_F32>::type* hitPos, Vector<3 , thrust::complex <float >>* outBUBRFactor)
244294{
245- run (kRadarComputeEnergy , stream, count, rayAzimuthStepRad, rayElevationStepRad, freq, rayPose, hitDist, hitNorm, hitPos ,
246- outBUBRFactor);
295+ run (kRadarComputeEnergy , stream, count, rayAzimuthStepRad, rayElevationStepRad, freq, lookAtOriginTransform, rayPose ,
296+ hitDist, hitNorm, hitPos, outBUBRFactor);
247297}
0 commit comments