Track object with radar postprocess input test

CMakeLists.txt

@@ -123,6 +123,7 @@ set(RGL_SOURCE_FILES
     src/graph/FromMat3x4fRaysNode.cpp
     src/graph/FilterGroundPointsNode.cpp
     src/graph/RadarPostprocessPointsNode.cpp
+    src/graph/RadarTrackObjectsNode.cpp
     src/graph/SetRangeRaysNode.cpp
     src/graph/SetRaysRingIdsRaysNode.cpp
     src/graph/SetTimeOffsetsRaysNode.cpp

extensions.repos

@@ -2,7 +2,7 @@ repositories:
   extensions/udp:
     type: git
     url: [email protected]:RobotecAI/RGL-extension-udp.git
-    version: develop
+    version: feature/q2-features
 
   extensions/snow:
     type: git

include/rgl/api/core.h

@@ -151,6 +151,22 @@ static_assert(std::is_trivial_v<rgl_radar_scope_t>);
 static_assert(std::is_standard_layout_v<rgl_radar_scope_t>);
 #endif
 
+/**
+ * Radar object class used in object tracking.
+ */
+typedef enum : int32_t
+{
+	RGL_RADAR_CLASS_CAR = 0,
+	RGL_RADAR_CLASS_TRUCK,
+	RGL_RADAR_CLASS_MOTORCYCLE,
+	RGL_RADAR_CLASS_BICYCLE,
+	RGL_RADAR_CLASS_PEDESTRIAN,
+	RGL_RADAR_CLASS_ANIMAL,
+	RGL_RADAR_CLASS_HAZARD,
+	RGL_RADAR_CLASS_UNKNOWN,
+	RGL_RADAR_CLASS_COUNT
+} rgl_radar_object_class_t;
+
 /**
  * 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.
@@ -382,6 +398,16 @@ typedef enum : int32_t
 	RGL_FIELD_DYNAMIC_FORMAT = 13842,
 } rgl_field_t;
 
+/**
+ * Kinds of return type for multi-return LiDAR output.
+ */
+typedef enum : int32_t
+{
+	RGL_RETURN_TYPE_NOT_DIVERGENT = 0,
+	RGL_RETURN_TYPE_FIRST = 1,
+	RGL_RETURN_TYPE_LAST = 2,
+} rgl_return_type_t;
+
 /**
  * Helper enum for axis selection
  */
@@ -695,6 +721,26 @@ RGL_API rgl_status_t rgl_node_raytrace_configure_distortion(rgl_node_t node, boo
  */
 RGL_API rgl_status_t rgl_node_raytrace_configure_non_hits(rgl_node_t node, float nearDistance, float farDistance);
 
+/**
+ * Modifies RaytraceNode to apply mask for non-hit values.
+ * Masked rays will be non-hits and will have the default non-hit values no matter of raytracing result.
+ * @param node RaytraceNode to modify.
+ * @param rays_mask Pointer to the array of int32_t. 1 means point is hit, 0 means point is non-hit.
+ * @param rays_count Number of elements in the `points_mask` array.
+ */
+RGL_API rgl_status_t rgl_node_raytrace_configure_mask(rgl_node_t node, const int8_t* rays_mask, int32_t rays_count);
+
+/**
+ * Modifies RaytraceNode to set beam divergence.
+ * Beam divergence is used to calculate the beam width at the distance of hit point.
+ * Setting beam divergence > 0.0f is required to use query for multi-return results.
+ * Setting beam divergence == 0.0f disables multi-return.
+ * @param node RaytraceNode to modify.
+ * @param horizontal_beam_divergence Horizontal beam divergence in radians.
+ * @param vertical_beam_divergence Vertical beam divergence in radians.
+ */
+RGL_API rgl_status_t rgl_node_raytrace_configure_beam_divergence(rgl_node_t node, float horizontal_beam_divergence, float vertical_beam_divergence);
+
 /**
  * Creates or modifies FormatPointsNode.
  * The Node converts internal representation into a binary format defined by the `fields` array.
@@ -819,6 +865,41 @@ RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, const r
                                                        float cumulative_device_gain, float received_noise_mean,
                                                        float received_noise_st_dev);
 
+/**
+ * Creates or modifies RadarTrackObjectsNode.
+ * The Node takes radar detections point cloud as input (from rgl_node_points_radar_postprocess), groups detections into objects based on given thresholds (node parameters),
+ * and calculates various characteristics of these objects. The output from this Node is in the form of a point cloud, where point XYZ coordinates are tracked objects positions.
+ * Additionally, cloud points have tracked object IDs. Note that for correct calculation and publishing some of object characteristics (e.g. velocity) user has to call
+ * rgl_scene_set_time for current scene.
+ * Graph input: point cloud, containing additionally RGL_FIELD_DISTANCE_F32, RGL_FIELD_AZIMUTH_F32, RGL_FIELD_ELEVATION_F32, RGL_FIELD_RADIAL_SPEED_F32 and ENTITY_ID_I32.
+ * Graph output: point cloud, contains only XYZ_VEC3_F32 for tracked object positions and ENTITY_ID_I32 for their IDs.
+ * @param node If (*node) == nullptr, a new Node will be created. Otherwise, (*node) will be modified.
+ * @param object_distance_threshold The maximum distance between a radar detection and other closest detection classified as the same tracked object (in meters).
+ * @param object_azimuth_threshold The maximum azimuth difference between a radar detection and other closest detection classified as the same tracked object (in radians).
+ * @param object_elevation_threshold The maximum elevation difference between a radar detection and other closest detection classified as the same tracked object (in radians).
+ * @param object_radial_speed_threshold The maximum radial speed difference between a radar detection and other closest detection classified as the same tracked object (in meters per second).
+ * @param max_matching_distance The maximum distance between predicted (based on previous frame) and newly detected object position to match objects between frames (in meters).
+ * @param max_prediction_time_frame The maximum time that object state can be predicted until it will be declared lost unless measured again (in milliseconds).
+ * @param movement_sensitivity The maximum position change for an object to be qualified as stationary (in meters).
+ */
+RGL_API rgl_status_t rgl_node_points_radar_track_objects(rgl_node_t* node, float object_distance_threshold,
+                                                         float object_azimuth_threshold, float object_elevation_threshold,
+                                                         float object_radial_speed_threshold, float max_matching_distance,
+                                                         float max_prediction_time_frame, float movement_sensitivity);
+
+/**
+ * Modifies RadarTrackObjectsNode to set entity ids to radar object classes mapping.
+ * This is necessary to call for RadarTrackObjectsNode to classify tracked objects correctly. If not set, objects will be classified as RGL_RADAR_CLASS_UNKNOWN by default.
+ * Note that entity can only belong to a single class - passing entity id multiple times in entity_ids array will result in the last version to be assigned. There is no
+ * limit on how many entities can have the same class.
+ * @param node RadarTrackObjectsNode to modify.
+ * @param entity_ids Array of RGL entity ids.
+ * @param object_classes Array of radar object classes.
+ * @param count Number of elements in entity_ids and object_classes arrays.
+ */
+RGL_API rgl_status_t rgl_node_points_radar_set_classes(rgl_node_t node, const int32_t* entity_ids,
+                                                       const rgl_radar_object_class_t* object_classes, int32_t count);
+
 /**
  * Creates or modifies FilterGroundPointsNode.
  * The Node adds RGL_FIELD_IS_GROUND_I32 which indicates the point is on the ground. Points are not removed.
@@ -879,6 +960,17 @@ RGL_API rgl_status_t rgl_node_gaussian_noise_angular_hitpoint(rgl_node_t* node,
 RGL_API rgl_status_t rgl_node_gaussian_noise_distance(rgl_node_t* node, float mean, float st_dev_base,
                                                       float st_dev_rise_per_meter);
 
+/**
+ * Creates or modifies MultiReturnSwitchNode
+ * This is a special node which does not modify the data but acts as an adapter to the multi-return feature.
+ * Thanks to this node, user can attach unchanged pipelines to work with specific return type from multi-return raytracing.
+ * Graph input: point cloud (with multi-return fields)
+ * Graph output: point cloud (with a selected field from parent's multi-return point cloud)
+ * @param node If (*node) == nullptr, a new Node will be created. Otherwise, (*node) will be modified.
+ * @param return_type Return type to select from multi-return point cloud.
+ */
+RGL_API rgl_status_t rgl_node_multi_return_switch(rgl_node_t* node, rgl_return_type_t);
+
 /**
  * Assigns value true to out_alive if the given node is known and has not been destroyed,
  * assigns value false otherwise.

src/GPUFieldDescBuilder.hpp

@@ -19,8 +19,6 @@
 
 #include <RGLExceptions.hpp>
 #include <gpu/GPUFieldDesc.hpp>
-#include <graph/Interfaces.hpp>
-#include <graph/NodesCore.hpp>
 #include <memory/Array.hpp>
 
 // Builder for GPUFieldDesc. Separated struct to avoid polluting gpu-visible header (gpu/GPUFieldDesc.hpp).

src/Time.hpp

@@ -31,6 +31,7 @@ struct Time
 	static Time nanoseconds(uint64_t nanoseconds) { return Time(nanoseconds); }
 
 	HostDevFn double asSeconds() const { return static_cast<double>(timeNs) * 1.0e-9; };
+	HostDevFn double asMilliseconds() const { return static_cast<double>(timeNs) * 1.0e-6; };
 	HostDevFn uint64_t asNanoseconds() const { return timeNs; }
 
 #if RGL_BUILD_ROS2_EXTENSION

src/api/apiCore.cpp

@@ -919,6 +919,51 @@ void TapeCore::tape_node_raytrace_configure_non_hits(const YAML::Node& yamlNode,
 	rgl_node_raytrace_configure_non_hits(node, yamlNode[1].as<float>(), yamlNode[2].as<float>());
 }
 
+RGL_API rgl_status_t rgl_node_raytrace_configure_mask(rgl_node_t node, const int8_t* rays_mask, int32_t rays_count)
+{
+	auto status = rglSafeCall([&]() {
+		RGL_API_LOG("rgl_node_raytrace_configure_mask(node={}, rays_mask={}, rays_count={})", repr(node),
+		            repr(rays_mask, rays_count), rays_count);
+		CHECK_ARG(node != nullptr);
+		CHECK_ARG(rays_mask != nullptr);
+		CHECK_ARG(rays_count > 0);
+		RaytraceNode::Ptr raytraceNode = Node::validatePtr<RaytraceNode>(node);
+		raytraceNode->setNonHitsMask(rays_mask, rays_count);
+	});
+	TAPE_HOOK(node, TAPE_ARRAY(rays_mask, rays_count), rays_count);
+	return status;
+}
+
+void TapeCore::tape_node_raytrace_configure_mask(const YAML::Node& yamlNode, PlaybackState& state)
+{
+	auto nodeId = yamlNode[0].as<TapeAPIObjectID>();
+	rgl_node_t node = state.nodes.at(nodeId);
+	rgl_node_raytrace_configure_mask(node, state.getPtr<const int8_t>(yamlNode[1]), yamlNode[2].as<int32_t>());
+}
+
+RGL_API rgl_status_t rgl_node_raytrace_configure_beam_divergence(rgl_node_t node, float horizontal_beam_divergence,
+                                                                 float vertical_beam_divergence)
+{
+	auto status = rglSafeCall([&]() {
+		RGL_API_LOG("rgl_node_raytrace_configure_beam_divergence(node={}, horizontal_divergence={}, vertical_divergence={})", repr(node),
+		            horizontal_beam_divergence, vertical_beam_divergence);
+		CHECK_ARG(node != nullptr);
+		CHECK_ARG((horizontal_beam_divergence > 0.0f && vertical_beam_divergence > 0.0f) ||
+		          (horizontal_beam_divergence == 0.0f && vertical_beam_divergence == 0.0f));
+		RaytraceNode::Ptr raytraceNode = Node::validatePtr<RaytraceNode>(node);
+		raytraceNode->setBeamDivergence(horizontal_beam_divergence, vertical_beam_divergence);
+	});
+	TAPE_HOOK(node, horizontal_beam_divergence, vertical_beam_divergence);
+	return status;
+}
+
+void TapeCore::tape_node_raytrace_configure_beam_divergence(const YAML::Node& yamlNode, PlaybackState& state)
+{
+	auto nodeId = yamlNode[0].as<TapeAPIObjectID>();
+	rgl_node_t node = state.nodes.at(nodeId);
+	rgl_node_raytrace_configure_beam_divergence(node, yamlNode[1].as<float>(), yamlNode[2].as<float>());
+}
+
 RGL_API rgl_status_t rgl_node_points_format(rgl_node_t* node, const rgl_field_t* fields, int32_t field_count)
 {
 	auto status = rglSafeCall([&]() {
@@ -1131,6 +1176,71 @@ void TapeCore::tape_node_points_radar_postprocess(const YAML::Node& yamlNode, Pl
 	state.nodes.insert({nodeId, node});
 }
 
+RGL_API rgl_status_t rgl_node_points_radar_track_objects(rgl_node_t* node, float object_distance_threshold,
+                                                         float object_azimuth_threshold, float object_elevation_threshold,
+                                                         float object_radial_speed_threshold, float max_matching_distance,
+                                                         float max_prediction_time_frame, float movement_sensitivity)
+{
+	auto status = rglSafeCall([&]() {
+		RGL_API_LOG("rgl_node_points_radar_track_objects(node={}, object_distance_threshold={}, object_azimuth_threshold={}, "
+		            "object_elevation_threshold={}, object_radial_speed_threshold={}, max_matching_distance={}, "
+		            "max_prediction_time_frame={}, movement_sensitivity={})",
+		            repr(node), object_distance_threshold, object_azimuth_threshold, object_elevation_threshold,
+		            object_radial_speed_threshold, max_matching_distance, max_prediction_time_frame, movement_sensitivity);
+		CHECK_ARG(node != nullptr);
+		CHECK_ARG(object_distance_threshold > 0.0f);
+		CHECK_ARG(object_azimuth_threshold > 0.0f);
+		CHECK_ARG(object_elevation_threshold > 0.0f);
+		CHECK_ARG(object_radial_speed_threshold > 0.0f);
+		CHECK_ARG(max_matching_distance > 0.0f);
+		CHECK_ARG(max_prediction_time_frame > 0.0f);
+		CHECK_ARG(movement_sensitivity >= 0.0f);
+
+		createOrUpdateNode<RadarTrackObjectsNode>(node, object_distance_threshold, object_azimuth_threshold,
+		                                          object_elevation_threshold, object_radial_speed_threshold,
+		                                          max_matching_distance, max_prediction_time_frame, movement_sensitivity);
+	});
+	TAPE_HOOK(node, object_distance_threshold, object_azimuth_threshold, object_elevation_threshold,
+	          object_radial_speed_threshold, max_matching_distance, max_prediction_time_frame, movement_sensitivity);
+	return status;
+}
+
+void TapeCore::tape_node_points_radar_track_objects(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_track_objects(&node, yamlNode[1].as<float>(), yamlNode[2].as<float>(), yamlNode[3].as<float>(),
+	                                    yamlNode[4].as<float>(), yamlNode[5].as<float>(), yamlNode[6].as<float>(),
+	                                    yamlNode[7].as<float>());
+	state.nodes.insert({nodeId, node});
+}
+
+RGL_API rgl_status_t rgl_node_points_radar_set_classes(rgl_node_t node, const int32_t* entity_ids,
+                                                       const rgl_radar_object_class_t* object_classes, int32_t count)
+{
+	auto status = rglSafeCall([&]() {
+		RGL_API_LOG("rgl_node_points_radar_set_classes(node={}, entity_ids={}, classes={}, count={})", repr(node),
+		            repr(entity_ids, count), repr(object_classes, count), count);
+		CHECK_ARG(node != nullptr);
+		CHECK_ARG(entity_ids != nullptr);
+		CHECK_ARG(object_classes != nullptr);
+		CHECK_ARG(count >= 0);
+		RadarTrackObjectsNode::Ptr trackObjectsNode = Node::validatePtr<RadarTrackObjectsNode>(node);
+		trackObjectsNode->setObjectClasses(entity_ids, object_classes, count);
+	});
+	TAPE_HOOK(node, TAPE_ARRAY(entity_ids, count), TAPE_ARRAY(object_classes, count), count);
+	return status;
+}
+
+void TapeCore::tape_node_points_radar_set_classes(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_set_classes(node, state.getPtr<const int32_t>(yamlNode[1]),
+	                                  state.getPtr<const rgl_radar_object_class_t>(yamlNode[2]), yamlNode[3].as<int32_t>());
+	state.nodes.insert({nodeId, node});
+}
+
 RGL_API rgl_status_t rgl_node_points_filter_ground(rgl_node_t* node, const rgl_vec3f* sensor_up_vector,
                                                    float ground_angle_threshold)
 {
@@ -1230,6 +1340,27 @@ void TapeCore::tape_node_gaussian_noise_distance(const YAML::Node& yamlNode, Pla
 	state.nodes.insert({nodeId, node});
 }
 
+RGL_API rgl_status_t rgl_node_multi_return_switch(rgl_node_t* node, rgl_return_type_t return_type)
+{
+	auto status = rglSafeCall([&]() {
+		RGL_API_LOG("rgl_node_multi_return_switch(node={}, return_type={})", repr(node), return_type);
+		CHECK_ARG(node != nullptr);
+
+		createOrUpdateNode<MultiReturnSwitchNode>(node, return_type);
+	});
+	TAPE_HOOK(node, return_type);
+	return status;
+}
+
+void TapeCore::tape_node_multi_return_switch(const YAML::Node& yamlNode, PlaybackState& state)
+{
+	auto nodeId = yamlNode[0].as<TapeAPIObjectID>();
+	auto return_type = static_cast<rgl_return_type_t>(yamlNode[1].as<int>());
+	rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr;
+	rgl_node_multi_return_switch(&node, return_type);
+	state.nodes.insert({nodeId, node});
+}
+
 rgl_status_t rgl_node_is_alive(rgl_node_t node, bool* out_alive)
 {
 	auto status = rglSafeCall([&]() {

src/gpu/MultiReturn.hpp

@@ -0,0 +1,28 @@
+// Copyright 2024 Robotec.AI
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <RGLFields.hpp>
+
+#define MULTI_RETURN_BEAM_VERTICES 19
+#define MULTI_RETURN_BEAM_LAYERS 3
+#define MULTI_RETURN_BEAM_SAMPLES (1 + (MULTI_RETURN_BEAM_VERTICES * MULTI_RETURN_BEAM_LAYERS))
+
+struct MultiReturnPointers
+{
+	Field<IS_HIT_I32>::type* isHit;
+	Field<XYZ_VEC3_F32>::type* xyz;
+	Field<DISTANCE_F32>::type* distance;
+};

src/gpu/RaytraceRequestContext.hpp

@@ -16,6 +16,7 @@
 
 #include <optix.h>
 #include <RGLFields.hpp>
+#include <gpu/MultiReturn.hpp>
 
 struct RaytraceRequestContext
 {
@@ -27,7 +28,7 @@ struct RaytraceRequestContext
 	float nearNonHitDistance;
 	float farNonHitDistance;
 
-	const Mat3x4f* rays;
+	const Mat3x4f* raysWorld;
 	size_t rayCount;
 
 	Mat3x4f rayOriginToWorld;
@@ -38,9 +39,11 @@ struct RaytraceRequestContext
 	const int* ringIds;
 	size_t ringIdsCount;
 
-	const float* rayTimeOffsets;
+	const float* rayTimeOffsetsMs;
 	size_t rayTimeOffsetsCount;
 
+	const int8_t* rayMask;
+
 	OptixTraversableHandle scene;
 	double sceneTime;
 	float sceneDeltaTime;
@@ -61,5 +64,10 @@ struct RaytraceRequestContext
 	Field<ELEVATION_F32>::type* elevation;
 	Field<NORMAL_VEC3_F32>::type* normal;
 	Field<INCIDENT_ANGLE_F32>::type* incidentAngle;
+
+	// Multi-Return
+	float hBeamHalfDivergenceRad;
+	float vBeamHalfDivergenceRad;
+	MultiReturnPointers mrSamples;
 };
 static_assert(std::is_trivially_copyable<RaytraceRequestContext>::value);