Review fixes

RobotecAI · Mar 4, 2024 · 9221bd8 · 9221bd8
1 parent 2265928
commit 9221bd8
Show file tree

Hide file tree

Showing 9 changed files with 97 additions and 153 deletions.
diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h
@@ -299,7 +299,7 @@ typedef enum : int32_t
 	RGL_FIELD_INCIDENT_ANGLE_F32,
 
 	/**
-	 * 3x4 matrix describing pose of the ray in the TODO coordinate system.
+	 * 3x4 matrix describing pose of the ray in the world coordinate system.
 	 */
 	RGL_FIELD_RAY_POSE_MAT3x4_F32,
 
@@ -719,9 +719,10 @@ RGL_API rgl_status_t rgl_node_points_from_array(rgl_node_t* node, const void* po
  * @param azimuth_separation The maximum azimuth difference to create a new radar cluster (in radians).
  * @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).
  */
 RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, float distance_separation, float azimuth_separation,
-                                                       float ray_azimuth_step, float ray_elevation_step);
+                                                       float ray_azimuth_step, float ray_elevation_step, float frequency);
 
 /**
  * Creates or modifies FilterGroundPointsNode.

diff --git a/src/api/apiCore.cpp b/src/api/apiCore.cpp
@@ -1033,7 +1033,7 @@ 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, float distance_separation, float azimuth_separation,
-                                                       float ray_azimuth_step, float ray_elevation_step)
+                                                       float ray_azimuth_step, float ray_elevation_step, float frequency)
 {
 	auto status = rglSafeCall([&]() {
 		RGL_API_LOG("rgl_node_points_radar_postprocess(node={}, distance_separation={}, azimuth_separation={})", repr(node),
@@ -1045,9 +1045,9 @@ RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, float d
 		CHECK_ARG(ray_elevation_step > 0);
 
 		createOrUpdateNode<RadarPostprocessPointsNode>(node, distance_separation, azimuth_separation, ray_azimuth_step,
-		                                               ray_elevation_step);
+		                                               ray_elevation_step, frequency);
 	});
-	TAPE_HOOK(node, distance_separation, azimuth_separation, ray_azimuth_step, ray_elevation_step);
+	TAPE_HOOK(node, distance_separation, azimuth_separation, ray_azimuth_step, ray_elevation_step, frequency);
 	return status;
 }
 
@@ -1056,7 +1056,7 @@ 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, yamlNode[1].as<float>(), yamlNode[2].as<float>(), yamlNode[3].as<float>(),
-	                                  yamlNode[4].as<float>());
+	                                  yamlNode[4].as<float>(), yamlNode[5].as<float>());
 	state.nodes.insert({nodeId, node});
 }
 

diff --git a/src/gpu/nodeKernels.cu b/src/gpu/nodeKernels.cu
@@ -95,59 +95,69 @@ __device__ Vec3f reflectPolarization(const Vec3f& pol, const Vec3f& hitNormal, c
 	return -polCompR * refPolR + polCompU * refPolU;
 }
 
-
-__global__ void kRadarComputeEnergy(size_t count, float rayAzimuthStepRad, float rayElevationStepRad,
+__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,
-                                    thrust::complex<float>* outBR, thrust::complex<float>* outBU,
-                                    thrust::complex<float>* outFactor)
+                                    Vector<3, thrust::complex<float>>* outBUBRFactor)
 {
 	LIMIT(count);
 
 	constexpr float c0 = 299792458.0f;
-	constexpr float freq = 79E9f;
-	constexpr float waveLen = c0 / freq;
-	constexpr float waveNum = 2.0f * M_PIf / waveLen;
 	constexpr float reflectionCoef = 1.0f; // TODO
+	const float waveLen = c0 / freq;
+	const float waveNum = 2.0f * M_PIf / waveLen;
 	const thrust::complex<float> i = {0, 1.0};
 	const Vec3f dirX = {1, 0, 0};
 	const Vec3f dirY = {0, 1, 0};
 	const Vec3f dirZ = {0, 0, 1};
 
-	Vec3f rayDirCts = rayPose[tid] * Vec3f{0, 0, 1};
-	Vec3f rayDirSph = {rayDirCts.length(), rayDirCts[0] == 0 && rayDirCts[1] == 0 ? 0 : atan2(rayDirCts.y(), rayDirCts.x()),
-	                   std::acos(rayDirCts.z() / rayDirCts.length())};
-	float phi = rayDirSph[1]; // azimuth, 0 = X-axis, positive = CCW
-	float the = rayDirSph[2]; // elevation, 0 = Z-axis, 90 = XY-plane, -180 = negative Z-axis
+	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
 
 	// Consider unit vector of the ray direction, these are its projections:
-	float cp = cosf(phi); // X-dir component
-	float sp = sinf(phi); // Y-dir component
-	float ct = cosf(the); // Z-dir component
-	float st = sinf(the); // XY-plane component
+	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};
 
-	Vec3f dirP = {-sp, cp, 0};
-	Vec3f dirT = {cp * ct, sp * ct, -st};
+	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);
 
-	thrust::complex<float> kr = {waveNum * hitDist[tid], 0.0f};
+	const Vector<3, thrust::complex<float>> rayPolCplx = {reflectedPol.x(), reflectedPol.y(), reflectedPol.z()};
 
-	Vec3f rayDir = rayDirCts.normalized();
-	Vec3f rayPol = rayPose[tid] * Vec3f{-1, 0, 0}; // UP, perpendicular to ray
-	Vec3f reflectedPol = reflectPolarization(rayPol, hitNorm[tid], rayDir);
+	const Vector<3, thrust::complex<float>> apE = reflectionCoef * exp(i * kr) * rayPolCplx;
+	const Vector<3, thrust::complex<float>> apH = -apE.cross(rayDir);
 
-	Vector<3, thrust::complex<float>> rayPolCplx = {reflectedPol.x(), reflectedPol.y(), reflectedPol.z()};
+	const Vec3f vecK = waveNum * ((dirX * cp + dirY * sp) * st + dirZ * ct);
 
-	Vector<3, thrust::complex<float>> apE = reflectionCoef * exp(i * kr) * rayPolCplx;
-	Vector<3, thrust::complex<float>> apH = -apE.cross(rayDir);
+	const float rayArea = hitDist[tid] * hitDist[tid] * std::sin(rayElevationStepRad) * rayAzimuthStepRad;
 
-	Vec3f vecK = waveNum * ((dirX * cp + dirY * sp) * st + dirZ * ct);
+	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 * M_PIf))) *
+	                                exp(-i * vecK.dot(hitPos[tid]));
 
-	float rayArea = hitDist[tid] * hitDist[tid] * std::sin(rayElevationStepRad) * rayAzimuthStepRad;
-	Vector<3, thrust::complex<float>> outBUBRFactor = {0, 0, 0};
+	//	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());
 
-	outBUBRFactor[0] = (-(apE.cross(-dirP) + apH.cross(dirT))).dot(rayDir);
-	outBUBRFactor[1] = (-(apE.cross(dirT) + apH.cross(dirP))).dot(rayDir);
-	outBUBRFactor[2] = thrust::complex<float>(0.0, ((waveNum * rayArea) / (4.0f * M_PIf))) * exp(-i * vecK.dot(hitPos[tid]));
+	outBUBRFactor[tid] = {BU, BR, factor};
 }
 
 __global__ void kFilterGroundPoints(size_t pointCount, const Vec3f sensor_up_vector, float ground_angle_threshold,
@@ -227,11 +237,11 @@ void gpuFilterGroundPoints(cudaStream_t stream, size_t pointCount, const Vec3f s
 	    lidarTransform);
 }
 
-void gpuRadarComputeEnergy(cudaStream_t stream, size_t count, float rayAzimuthStepRad, float rayElevationStepRad,
+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)
 {
-	run(kRadarComputeEnergy, stream, count, rayAzimuthStepRad, rayElevationStepRad, rayPose, hitDist, hitNorm, hitPos,
+	run(kRadarComputeEnergy, stream, count, rayAzimuthStepRad, rayElevationStepRad, freq, rayPose, hitDist, hitNorm, hitPos,
 	    outBUBRFactor);
 }
diff --git a/src/gpu/nodeKernels.hpp b/src/gpu/nodeKernels.hpp
@@ -47,7 +47,7 @@ void gpuFilter(cudaStream_t, size_t count, const Field<RAY_IDX_U32>::type* indic
 void gpuFilterGroundPoints(cudaStream_t stream, size_t pointCount, const Vec3f sensor_up_axis, 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 gpuRadarComputeEnergy(cudaStream_t stream, size_t count, float rayAzimuthStepRad, float rayElevationStepRad,
+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);
diff --git a/src/graph/NodesCore.hpp b/src/graph/NodesCore.hpp
@@ -116,6 +116,9 @@ struct RaytraceNode : IPointsNode
 	// Data getters
 	IAnyArray::ConstPtr getFieldData(rgl_field_t field) override
 	{
+		if (field == RAY_POSE_MAT3x4_F32) {
+			return raysNode->getRays();
+		}
 		return std::const_pointer_cast<const IAnyArray>(fieldData.at(field));
 	}
 
@@ -463,7 +466,8 @@ struct GaussianNoiseDistanceNode : IPointsNodeSingleInput
 struct RadarPostprocessPointsNode : IPointsNodeSingleInput
 {
 	using Ptr = std::shared_ptr<RadarPostprocessPointsNode>;
-	void setParameters(float distanceSeparation, float azimuthSeparation, float rayAzimuthStepRad, float rayElevationStepRad);
+	void setParameters(float distanceSeparation, float azimuthSeparation, float rayAzimuthStepRad, float rayElevationStepRad,
+	                   float frequency);
 
 	// Node
 	void validateImpl() override;
@@ -496,6 +500,7 @@ struct RadarPostprocessPointsNode : IPointsNodeSingleInput
 	float azimuthSeparation;
 	float rayAzimuthStepRad;
 	float rayElevationStepRad;
+	float frequency;
 
 
 	// RGL related members

diff --git a/src/graph/RadarPostprocessPointsNode.cpp b/src/graph/RadarPostprocessPointsNode.cpp
@@ -18,12 +18,13 @@
 #include <gpu/nodeKernels.hpp>
 
 void RadarPostprocessPointsNode::setParameters(float distanceSeparation, float azimuthSeparation, float rayAzimuthStepRad,
-                                               float rayElevationStepRad)
+                                               float rayElevationStepRad, float frequency)
 {
 	this->distanceSeparation = distanceSeparation;
 	this->azimuthSeparation = azimuthSeparation;
 	this->rayAzimuthStepRad = rayAzimuthStepRad;
 	this->rayElevationStepRad = rayElevationStepRad;
+	this->frequency = frequency;
 }
 
 void RadarPostprocessPointsNode::validateImpl()
@@ -49,11 +50,23 @@ void RadarPostprocessPointsNode::enqueueExecImpl()
 	auto normal = input->getFieldDataTyped<NORMAL_VEC3_F32>()->asSubclass<DeviceAsyncArray>()->getReadPtr();
 	auto xyz = input->getFieldDataTyped<XYZ_VEC3_F32>()->asSubclass<DeviceAsyncArray>()->getReadPtr();
 	outBUBRFactorDev->resize(input->getPointCount(), false, false);
-	gpuRadarComputeEnergy(getStreamHandle(), input->getPointCount(), rayAzimuthStepRad, rayElevationStepRad, rays, distance,
-	                      normal, xyz, outBUBRFactorDev->getWritePtr());
+	gpuRadarComputeEnergy(getStreamHandle(), input->getPointCount(), rayAzimuthStepRad, rayElevationStepRad, frequency, rays,
+	                      distance, normal, xyz, outBUBRFactorDev->getWritePtr());
 	outBUBRFactorHost->copyFrom(outBUBRFactorDev);
 	CHECK_CUDA(cudaStreamSynchronize(getStreamHandle()));
 
+	// Computing RCS (example for all points)
+	//	std::complex<float> AU = 0;
+	//	std::complex<float> AR = 0;
+	//	for (int hitIdx = 0; hitIdx < outBUBRFactorHost->getCount(); ++hitIdx) {
+	//		std::complex<float> BU = {outBUBRFactorHost->at(hitIdx)[0].real(), outBUBRFactorHost->at(hitIdx)[0].imag()};
+	//		std::complex<float> BR = {outBUBRFactorHost->at(hitIdx)[1].real(), outBUBRFactorHost->at(hitIdx)[1].imag()};
+	//		std::complex<float> 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;

diff --git a/src/math/Vector.hpp b/src/math/Vector.hpp
@@ -126,7 +126,7 @@ struct Vector
 	PIECEWISE_OPERATOR(/, /=)
 #undef PIECEWISE_OPERATOR
 
-	HostDevFn V operator-()
+	HostDevFn V operator-() const
 	{
 		V v;
 		for (int i = 0; i < dim; ++i) {
@@ -188,6 +188,12 @@ struct Vector
 		return value;
 	}
 
+	template<int D = dim, typename std::enable_if_t<D == 3, int> = 0>
+	HostDevFn V toSpherical() const
+	{
+		return {length(), row[0] == 0 && row[1] == 0 ? 0 : atan2(row[1], row[0]), std::acos(row[2] / length())};
+	}
+
 private:
 	T row[dim];
 };

diff --git a/test/src/TapeTest.cpp b/test/src/TapeTest.cpp
@@ -262,7 +262,7 @@ TEST_F(TapeTest, RecordPlayAllCalls)
 	                                              usePointsFields.data(), usePointsFields.size()));
 
 	rgl_node_t radarPostprocess = nullptr;
-	EXPECT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarPostprocess, 1.0f, 0.5f, 1.0f, 1.0f));
+	EXPECT_RGL_SUCCESS(rgl_node_points_radar_postprocess(&radarPostprocess, 1.0f, 0.5f, 1.0f, 1.0f, 79E9f));
 
 	rgl_node_t filterGround = nullptr;
 	rgl_vec3f sensorUpVector = {0.0f, 1.0f, 0.0f};