Add object tracking to RadarTrackObjectsNode (#288)

* Add more details in documentation for object tracking node.

* Remove not necessary overload.

* Update type traits on RunningStats to be more precise.

* Add covariance calculation.  Add RunningStats specialization for Vector types.

* Change function signature in RunningStats specialization.

* Add prototype implementation of object tracking.

* Add minor comments to RunningStats.

* Move object position prediction to node. Fix predicted position calculation. Update prediction to include acceleration if available. Add getter for samples count to RunningStats.

* Update velocity calculation. Add assert on delta time and object ID.

* Fix object acceleration calculation.

* Add object orientation and orientation rate calculation.

* Make minor fixes to types in track objects node.

* Fix typos and add missing include in RunningStats header.

* Add axis-aligned bounding box implementation.

* Add aliases and staic asserts for aabb.

* Add max prediction time frame - how long object can be predicted until declared lost.

* Add option to reset aabb.

* Add keeping orientation from previous frame, when object did not move.

* Add operator overloads to Aabb. Fix field names mistake.

* Fix Aabb members naming.

* Update RadarPostprocessPointsNode to provide cluster (detection) aabbs.

* Add handling detection (and object) aabbs to RadarTrackObjectsNode.

* Rename predictionSensitivity parameter to maxMatchingDistance.

* Add remaining tracking parameters to API.

* Add kinematic object tracking test. Make object tracking tests more clear.

* Add runtime object tracking test for rviz2 preview, skipped by default.

* Add linear and angular velocity getters to IPointsNode. Add relative velocity and relative acceleration calculation to RadarTrackObjectsNode.

* Make changes according to review for #288.

---------

Signed-off-by: Paweł Liberadzki <[email protected]>

Author: PawelLiberadzki

include/rgl/api/core.h

@@ -851,19 +851,23 @@ RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, const r
  * 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. This Node is intended to be connected to object list publishing node (from rgl_node_publish_udp_objectlist).
- * The output from this Node is in the form of a point cloud, where point XYZ coordinates are tracked objects positions.
+ * 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 RGL_FIELD_DISTANCE_F32, RGL_FIELD_AZIMUTH_F32, RGL_FIELD_ELEVATION_F32 and RGL_FIELD_RADIAL_SPEED_F32
- * Graph output: point cloud
+ * 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 object_radial_speed_threshold, float max_matching_distance,
+                                                         float max_prediction_time_frame, float movement_sensitivity);
 
 /**
  * Creates or modifies FilterGroundPointsNode.

src/api/apiCore.cpp

@@ -1175,24 +1175,30 @@ void TapeCore::tape_node_points_radar_postprocess(const YAML::Node& yamlNode, Pl
 
 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 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={})",
+		            "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);
+		            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);
+		                                          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);
+	          object_radial_speed_threshold, max_matching_distance, max_prediction_time_frame, movement_sensitivity);
 	return status;
 }
 
@@ -1201,7 +1207,8 @@ void TapeCore::tape_node_points_radar_track_objects(const YAML::Node& yamlNode,
 	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[4].as<float>(), yamlNode[5].as<float>(), yamlNode[6].as<float>(),
+	                                    yamlNode[7].as<float>());
 	state.nodes.insert({nodeId, node});
 }
 

src/graph/Interfaces.hpp

@@ -87,6 +87,8 @@ struct IPointsNode : virtual Node
 	virtual std::size_t getPointCount() const { return getWidth() * getHeight(); }
 
 	virtual Mat3x4f getLookAtOriginTransform() const { return Mat3x4f::identity(); }
+	virtual Vec3f getLinearVelocity() const { return Vec3f{0.0f}; }
+	virtual Vec3f getAngularVelocity() const { return Vec3f{0.0f}; }
 
 	// Data getters
 	virtual IAnyArray::ConstPtr getFieldData(rgl_field_t field) = 0;
@@ -120,6 +122,8 @@ struct IPointsNodeSingleInput : IPointsNode
 	virtual size_t getHeight() const override { return input->getHeight(); }
 
 	virtual Mat3x4f getLookAtOriginTransform() const override { return input->getLookAtOriginTransform(); }
+	virtual Vec3f getLinearVelocity() const { return input->getLinearVelocity(); }
+	virtual Vec3f getAngularVelocity() const { return input->getAngularVelocity(); }
 
 	// Data getters
 	virtual bool hasField(rgl_field_t field) const { return input->hasField(field); }

src/graph/NodesCore.hpp

@@ -22,6 +22,7 @@
 #include <ranges>
 #include <algorithm>
 #include <random>
+#include <queue>
 #include <curand_kernel.h>
 
 #include <graph/Node.hpp>
@@ -30,6 +31,7 @@
 #include <gpu/nodeKernels.hpp>
 #include <CacheManager.hpp>
 #include <GPUFieldDescBuilder.hpp>
+#include <math/Aabb.h>
 #include <math/RunningStats.hpp>
 #include <gpu/MultiReturn.hpp>
 
@@ -114,6 +116,8 @@ struct RaytraceNode : IPointsNode
 	size_t getHeight() const override { return 1; } // TODO: implement height in use_rays
 
 	Mat3x4f getLookAtOriginTransform() const override { return raysNode->getCumulativeRayTransfrom().inverse(); }
+	Vec3f getLinearVelocity() const override { return sensorLinearVelocityXYZ; }
+	Vec3f getAngularVelocity() const override { return sensorAngularVelocityRPY; }
 
 	// Data getters
 	IAnyArray::ConstPtr getFieldData(rgl_field_t field) override
@@ -532,11 +536,14 @@ struct RadarPostprocessPointsNode : IPointsNodeSingleInput
 	// Data getters
 	IAnyArray::ConstPtr getFieldData(rgl_field_t field) override;
 
+	const std::vector<Aabb3Df>& getClusterAabbs() const { return clusterAabbs; }
+
 private:
 	// Data containers
 	std::vector<Field<RAY_IDX_U32>::type> filteredIndicesHost;
 	DeviceAsyncArray<Field<RAY_IDX_U32>::type>::Ptr filteredIndices = DeviceAsyncArray<Field<RAY_IDX_U32>::type>::create(
 	    arrayMgr);
+	HostPinnedArray<Field<XYZ_VEC3_F32>::type>::Ptr xyzInputHost = HostPinnedArray<Field<XYZ_VEC3_F32>::type>::create();
 	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 =
@@ -566,6 +573,7 @@ struct RadarPostprocessPointsNode : IPointsNodeSingleInput
 	float receivedNoiseStDevDb;
 
 	std::vector<rgl_radar_scope_t> radarScopes;
+	std::vector<Aabb3Df> clusterAabbs;
 
 	std::random_device randomDevice;
 
@@ -613,6 +621,12 @@ struct RadarTrackObjectsNode : IPointsNodeSingleInput
 		Invalid = 255
 	};
 
+	struct ObjectBounds
+	{
+		Vec3f position{0};
+		Aabb3Df aabb{};
+	};
+
 	struct ClassificationProbabilities
 	{
 		float existence{100.0f};
@@ -629,8 +643,8 @@ struct RadarTrackObjectsNode : IPointsNodeSingleInput
 	struct ObjectState
 	{
 		uint32_t id{0};
-		uint32_t framesCount{0};
 		uint32_t creationTime{0};
+		uint32_t lastMeasuredTime{0};
 		ObjectStatus objectStatus{ObjectStatus::Invalid};
 		MovementStatus movementStatus{MovementStatus::Invalid};
 		ClassificationProbabilities classificationProbabilities{};
@@ -648,15 +662,13 @@ struct RadarTrackObjectsNode : IPointsNodeSingleInput
 
 	RadarTrackObjectsNode();
 
-	void setParameters(float distanceThreshold, float azimuthThreshold, float elevationThreshold, float radialSpeedThreshold);
+	void setParameters(float distanceThreshold, float azimuthThreshold, float elevationThreshold, float radialSpeedThreshold,
+	                   float maxMatchingDistance, float maxPredictionTimeFrame, float movementSensitivity);
 
-	// Node
-	void validateImpl() override;
 	void enqueueExecImpl() override;
 
 	bool hasField(rgl_field_t field) const override { return fieldData.contains(field); }
 
-	// Node requirements
 	std::vector<rgl_field_t> getRequiredFieldList() const override;
 
 	// Data getters
@@ -667,14 +679,30 @@ struct RadarTrackObjectsNode : IPointsNodeSingleInput
 	const std::list<ObjectState>& getObjectStates() const { return objectStates; }
 
 private:
+	Vec3f predictObjectPosition(const ObjectState& objectState, double deltaTimeMs) const;
+	void createObjectState(const ObjectBounds& objectBounds, double currentTimeMs);
+	void updateObjectState(ObjectState& objectState, const Vec3f& updatedPosition, const Aabb3Df& updatedAabb,
+	                       ObjectStatus objectStatus, double currentTimeMs, double deltaTimeMs);
+	void updateOutputData();
+
 	std::list<ObjectState> objectStates;
 	std::unordered_map<rgl_field_t, IAnyArray::Ptr> fieldData;
 
+	uint32_t objectIDCounter = 0; // Not static - I assume each ObjectTrackingNode is like a separate radar.
+	std::queue<uint32_t> objectIDPoll;
+
 	float distanceThreshold;
 	float azimuthThreshold;
 	float elevationThreshold;
 	float radialSpeedThreshold;
 
+	float maxMatchingDistance =
+	    1.0f; // Max distance between predicted and newly detected position to match objects between frames.
+	float maxPredictionTimeFrame =
+	    500.0f;                        // Maximum time in milliseconds that can pass between two detections of the same object.
+	                                   // In other words, how long object state can be predicted until it will be declared lost.
+	float movementSensitivity = 0.01f; // Max position change for an object to be qualified as MovementStatus::Stationary.
+
 	HostPinnedArray<Field<XYZ_VEC3_F32>::type>::Ptr xyzHostPtr = HostPinnedArray<Field<XYZ_VEC3_F32>::type>::create();
 	HostPinnedArray<Field<DISTANCE_F32>::type>::Ptr distanceHostPtr = HostPinnedArray<Field<DISTANCE_F32>::type>::create();
 	HostPinnedArray<Field<AZIMUTH_F32>::type>::Ptr azimuthHostPtr = HostPinnedArray<Field<AZIMUTH_F32>::type>::create();

src/graph/RadarPostprocessPointsNode.cpp

@@ -78,6 +78,7 @@ void RadarPostprocessPointsNode::enqueueExecImpl()
 		return;
 	}
 
+	xyzInputHost->copyFrom(input->getFieldData(XYZ_VEC3_F32));
 	distanceInputHost->copyFrom(input->getFieldData(DISTANCE_F32));
 	azimuthInputHost->copyFrom(input->getFieldData(AZIMUTH_F32));
 	radialSpeedInputHost->copyFrom(input->getFieldData(RADIAL_SPEED_F32));
@@ -143,13 +144,19 @@ void RadarPostprocessPointsNode::enqueueExecImpl()
 	clusterPowerHost->resize(filteredIndicesHost.size(), false, false);
 	clusterNoiseHost->resize(filteredIndicesHost.size(), false, false);
 	clusterSnrHost->resize(filteredIndicesHost.size(), false, false);
+
 	std::normal_distribution<float> gaussianNoise(receivedNoiseMeanDb, receivedNoiseStDevDb);
+	clusterAabbs.resize(clusters.size());
+
 	for (int clusterIdx = 0; clusterIdx < clusters.size(); ++clusterIdx) {
 		std::complex<float> AU = 0;
 		std::complex<float> AR = 0;
-		auto&& cluster = clusters[clusterIdx];
+		auto& cluster = clusters[clusterIdx];
+		auto& clusterAabb = clusterAabbs[clusterIdx];
+		clusterAabb.reset();
 
 		for (const auto pointInCluster : cluster.indices) {
+			clusterAabb.expand(xyzInputHost->at(pointInCluster));
 			std::complex<float> BU = {outBUBRFactorHost->at(pointInCluster)[0].real(),
 			                          outBUBRFactorHost->at(pointInCluster)[0].imag()};
 			std::complex<float> BR = {outBUBRFactorHost->at(pointInCluster)[1].real(),