Improve radar clustering with variable separation on distance ranges (still CPU only) (#260)

* Add ranged separation for distance and radial speed

* Add missing static_asset for structs

* Introduce rgl_radar_separations_t

* Follow math convention (a < b < c)

Co-authored-by: Piotr Rybicki <[email protected]>

* Review changes

* Add validation to the radarScopes

* Fix clusters merging check

* Fix iteration and required field list

* Add clustering test

* Add more test cases

* Review changes

---------

Co-authored-by: Piotr Rybicki <[email protected]>

Author: msz-rai

include/rgl/api/core.h

@@ -61,6 +61,12 @@ typedef struct
 	float value[2];
 } rgl_vec2f;
 
+#ifndef __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>);
+#endif
+
 /**
  * Three consecutive 32-bit floats.
  */
@@ -71,6 +77,8 @@ typedef struct
 
 #ifndef __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>);
 #endif
 
 /**
@@ -81,6 +89,12 @@ typedef struct
 	int32_t value[3];
 } rgl_vec3i;
 
+#ifndef __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>);
+#endif
+
 /**
  * Row-major matrix with 3 rows and 4 columns of 32-bit floats.
  * Right-handed coordinate system.
@@ -90,6 +104,45 @@ typedef struct
 	float value[3][4];
 } rgl_mat3x4f;
 
+#ifndef __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>);
+#endif
+
+/**
+ * Radar parameters applied at a given distance range.
+ */
+typedef struct
+{
+	/**
+	 * The beginning distance range for the parameters.
+	 */
+	float begin_distance;
+	/**
+	 * The end range distance for the parameters.
+	 */
+	float end_distance;
+	/**
+	 * The maximum distance difference to create a new radar detection (in simulation units).
+	 */
+	float distance_separation_threshold;
+	/**
+	 * The maximum radial speed difference to create a new radar detection (in simulation units)
+	 */
+	float radial_speed_separation_threshold;
+	/**
+	 * The maximum azimuth difference to create a new radar detection (in radians).
+	 */
+	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>);
+#endif
+
 /**
  * Represents on-GPU Mesh that can be referenced by Entities on the Scene.
  * Each Mesh can be referenced by any number of Entities on different Scenes.
@@ -699,19 +752,24 @@ RGL_API rgl_status_t rgl_node_points_from_array(rgl_node_t* node, const void* po
 /**
  * Creates or modifies RadarPostprocessPointsNode.
  * The Node processes point cloud to create radar-like output.
- * The point cloud is reduced by clustering input based on hit-point distance and hit-point azimuth.
+ * The point cloud is reduced by clustering input based on hit-point attributes: distance, radial speed and azimuth.
+ * Some radar parameters may vary for different distance ranges, as radars may employ multiple frequency bands.
+ * For this reason, the configuration allows the definition of multiple radar scopes of parameters on the assumption that:
+ *   - in case of scopes that overlap, the first matching one will be used
+ *   - if the point is not within any of the radar scopes, it will be rejected
  * The output consists of the collection of one point per cluster (the closest to the azimuth and elevation center).
  * Graph input: point cloud
  * Graph output: point cloud
  * @param node If (*node) == nullptr, a new Node will be created. Otherwise, (*node) will be modified.
- * @param distance_separation The maximum distance difference to create a new radar cluster (in simulation units).
- * @param azimuth_separation The maximum azimuth difference to create a new radar cluster (in radians).
+ * @param radar_scopes Array of radar scopes of parameters. See `rgl_radar_scope_t` for more details.
+ * @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).
  */
-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 frequency);
+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);
 
 /**
  * Creates or modifies FilterGroundPointsNode.

src/api/apiCore.cpp

@@ -1049,31 +1049,44 @@ void TapeCore::tape_node_points_from_array(const YAML::Node& yamlNode, PlaybackS
 	state.nodes.insert({nodeId, node});
 }
 
-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 frequency)
+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)
 {
 	auto status = rglSafeCall([&]() {
-		RGL_API_LOG("rgl_node_points_radar_postprocess(node={}, distance_separation={}, azimuth_separation={})", repr(node),
-		            distance_separation, azimuth_separation);
-
-		CHECK_ARG(distance_separation > 0);
-		CHECK_ARG(azimuth_separation > 0);
+		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);
+		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);
+
+		for (int i = 0; i < radar_scopes_count; ++i) {
+			CHECK_ARG(radar_scopes[i].begin_distance >= 0);
+			CHECK_ARG(radar_scopes[i].end_distance >= 0);
+			CHECK_ARG(radar_scopes[i].distance_separation_threshold >= 0);
+			CHECK_ARG(radar_scopes[i].radial_speed_separation_threshold >= 0);
+			CHECK_ARG(radar_scopes[i].azimuth_separation_threshold >= 0);
+			CHECK_ARG(radar_scopes[i].end_distance >= radar_scopes[i].begin_distance);
+		}
 
-		createOrUpdateNode<RadarPostprocessPointsNode>(node, distance_separation, azimuth_separation, ray_azimuth_step,
-		                                               ray_elevation_step, frequency);
+		createOrUpdateNode<RadarPostprocessPointsNode>(
+		    node, std::vector<rgl_radar_scope_t>{radar_scopes, radar_scopes + radar_scopes_count}, ray_azimuth_step,
+		    ray_elevation_step, frequency);
 	});
-	TAPE_HOOK(node, distance_separation, azimuth_separation, ray_azimuth_step, ray_elevation_step, frequency);
+	TAPE_HOOK(node, TAPE_ARRAY(radar_scopes, radar_scopes_count), radar_scopes_count, ray_azimuth_step, ray_elevation_step,
+	          frequency);
 	return status;
 }
 
 void TapeCore::tape_node_points_radar_postprocess(const YAML::Node& yamlNode, PlaybackState& state)
 {
 	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[5].as<float>());
+	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>());
 	state.nodes.insert({nodeId, node});
 }
 

src/graph/NodesCore.hpp

@@ -369,7 +369,11 @@ struct FromArrayPointsNode : IPointsNode, INoInputNode
 	void enqueueExecImpl() override {}
 
 	// Point cloud description
-	bool isDense() const override { return false; }
+	bool isDense() const override
+	{
+		// If point cloud doesn't contain IS_HIT field we assume all points are hits.
+		return !fieldData.contains(RGL_FIELD_IS_HIT_I32);
+	}
 	bool hasField(rgl_field_t field) const override { return fieldData.contains(field); }
 	size_t getWidth() const override { return width; }
 	size_t getHeight() const override { return 1; }
@@ -470,7 +474,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(const std::vector<rgl_radar_scope_t>& radarScopes, float rayAzimuthStepRad, float rayElevationStepRad,
 	                   float frequency);
 
 	// Node
@@ -494,32 +499,33 @@ struct RadarPostprocessPointsNode : IPointsNodeSingleInput
 	    arrayMgr);
 	HostPinnedArray<Field<DISTANCE_F32>::type>::Ptr distanceInputHost = HostPinnedArray<Field<DISTANCE_F32>::type>::create();
 	HostPinnedArray<Field<AZIMUTH_F32>::type>::Ptr azimuthInputHost = HostPinnedArray<Field<AZIMUTH_F32>::type>::create();
+	HostPinnedArray<Field<RADIAL_SPEED_F32>::type>::Ptr radialSpeedInputHost =
+	    HostPinnedArray<Field<RADIAL_SPEED_F32>::type>::create();
 	HostPinnedArray<Field<ELEVATION_F32>::type>::Ptr elevationInputHost = HostPinnedArray<Field<ELEVATION_F32>::type>::create();
 	DeviceAsyncArray<Vector<3, thrust::complex<float>>>::Ptr outBUBRFactorDev =
 	    DeviceAsyncArray<Vector<3, thrust::complex<float>>>::create(arrayMgr);
 	HostPinnedArray<Vector<3, thrust::complex<float>>>::Ptr outBUBRFactorHost =
 	    HostPinnedArray<Vector<3, thrust::complex<float>>>::create();
 
-	float distanceSeparation;
-	float azimuthSeparation;
 	float rayAzimuthStepRad;
 	float rayElevationStepRad;
 	float frequency;
 
+	std::vector<rgl_radar_scope_t> radarScopes;
 
 	// RGL related members
 	std::mutex getFieldDataMutex;
 	mutable CacheManager<rgl_field_t, IAnyArray::Ptr> cacheManager;
 
 	struct RadarCluster
 	{
-		RadarCluster(Field<RAY_IDX_U32>::type index, float distance, float azimuth, float elevation);
+		RadarCluster(Field<RAY_IDX_U32>::type index, float distance, float azimuth, float radialSpeed, float elevation);
 		RadarCluster(RadarCluster&& other) noexcept = default;
 		RadarCluster& operator=(RadarCluster&& other) noexcept = default;
 
-		void addPoint(Field<RAY_IDX_U32>::type index, float distance, float azimuth, float elevation);
-		inline bool isCandidate(float distance, float azimuth, float distanceSeparation, float azimuthSeparation) const;
-		inline bool canMergeWith(const RadarCluster& other, float distanceSeparation, float azimuthSeparation) const;
+		void addPoint(Field<RAY_IDX_U32>::type index, float distance, float azimuth, float radialSpeed, float elevation);
+		inline bool isCandidate(float distance, float azimuth, float radialSpeed, const rgl_radar_scope_t& separations) const;
+		inline bool canMergeWith(const RadarCluster& other, const std::vector<rgl_radar_scope_t>& radarScopes) const;
 		void takeIndicesFrom(RadarCluster&& other);
 		Field<RAY_IDX_U32>::type findDirectionalCenterIndex(const Field<AZIMUTH_F32>::type* azimuths,
 		                                                    const Field<ELEVATION_F32>::type* elevations) const;
@@ -528,6 +534,7 @@ struct RadarPostprocessPointsNode : IPointsNodeSingleInput
 		std::vector<Field<RAY_IDX_U32>::type> indices;
 		Vector<2, Field<DISTANCE_F32>::type> minMaxDistance;
 		Vector<2, Field<AZIMUTH_F32>::type> minMaxAzimuth;
+		Vector<2, Field<RADIAL_SPEED_F32>::type> minMaxRadialSpeed;
 		Vector<2, Field<ELEVATION_F32>::type> minMaxElevation; // For finding directional center only
 	};
 };