Add radar object classification (#298)

* Add initial support for object classification in RadarTrackObjectsNode.

Signed-off-by: Paweł Liberadzki <pawel.liberadzki@robotec.ai>

* Add API call for setting radar object classes.

Signed-off-by: Paweł Liberadzki <pawel.liberadzki@robotec.ai>

* Update radar track objects tests to handle object ids.

Signed-off-by: Paweł Liberadzki <pawel.liberadzki@robotec.ai>

* Add safety static_cast.

Signed-off-by: Paweł Liberadzki <pawel.liberadzki@robotec.ai>

* Make fixes based on the review.

Signed-off-by: Paweł Liberadzki <pawel.liberadzki@robotec.ai>

* Prevent underflow in loop condition (#308)

* Add handling nan radial speeds in radar nodes (#309)

Add handling nan radial speeds in radar postprocess and object tracking nodes.

Signed-off-by: Paweł Liberadzki <pawel.liberadzki@robotec.ai>

* Radar object classification improvements (#310)

* Change array type in fieldData to be compatible with all RGL nodes

* Use velocities from raytrace instead of calculating it again (better accuracy)

* Do not output predicted objects

* Restore the conditions of merging detections into objects

* Fix required fields; skip test that fails

* Require detections in world frame to predict objects properly

* Fix displacementFromSkinning calculation

* Fix test

* Put all objects to the output

* Fix deltaTime calculation

* Fix test

* Fix time

* Fix passing time to new objects

* Add comment on skinning fix

* Fix coordinate system of width & length

* Fix bounding boxes

* Fix maxClassificationProbability

* Change unit of movement_sensitivity

---------

Signed-off-by: Paweł Liberadzki <pawel.liberadzki@robotec.ai>
Co-authored-by: Maja Nagarnowicz <72594569+nebraszka@users.noreply.github.com>
Co-authored-by: Mateusz Szczygielski <112629916+msz-rai@users.noreply.github.com>

Author: 3 people

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.
@@ -852,10 +868,12 @@ 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. 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
+ * 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.
+ * The node expects input in world frame to be able to perform prediction properly.
+ * Object's orientation, length, and width calculations assume the forward axis is Z and the left axis is -X.
+ * 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).
@@ -864,13 +882,26 @@ RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, const r
  * @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).
+ * @param movement_sensitivity The maximum velocity for an object to be qualified as stationary (in meters per seconds).
  */
 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.

src/api/apiCore.cpp

@@ -1215,6 +1215,32 @@ void TapeCore::tape_node_points_radar_track_objects(const YAML::Node& yamlNode,
 	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)
 {

src/gpu/optixPrograms.cu

@@ -219,9 +219,10 @@ extern "C" __global__ void __closesthit__()
 		if (wasSkinned) {
 			Mat3x4f objectToWorld;
 			optixGetObjectToWorldTransformMatrix(reinterpret_cast<float*>(objectToWorld.rc));
-			const Vec3f& vA = entityData.vertexDisplacementSincePrevFrame[triangleIndices.x()];
-			const Vec3f& vB = entityData.vertexDisplacementSincePrevFrame[triangleIndices.y()];
-			const Vec3f& vC = entityData.vertexDisplacementSincePrevFrame[triangleIndices.z()];
+			// TODO(msz-rai): To verify if rotation is needed (in some tests it produces more realistic results)
+			const Vec3f& vA = objectToWorld.rotation() * entityData.vertexDisplacementSincePrevFrame[triangleIndices.x()];
+			const Vec3f& vB = objectToWorld.rotation() * entityData.vertexDisplacementSincePrevFrame[triangleIndices.y()];
+			const Vec3f& vC = objectToWorld.rotation() * entityData.vertexDisplacementSincePrevFrame[triangleIndices.z()];
 			displacementFromSkinning = objectToWorld.scaleVec() * Vec3f((1 - u - v) * vA + u * vB + v * vC);
 		}
 

src/graph/Interfaces.hpp

@@ -87,8 +87,6 @@ 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;
@@ -122,8 +120,6 @@ 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

@@ -34,6 +34,7 @@
 #include <math/Aabb.h>
 #include <math/RunningStats.hpp>
 #include <gpu/MultiReturn.hpp>
+#include <Time.hpp>
 
 
 struct FormatPointsNode : IPointsNodeSingleInput
@@ -116,8 +117,6 @@ 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
@@ -656,8 +655,11 @@ struct RadarTrackObjectsNode : IPointsNodeSingleInput
 
 	struct ObjectBounds
 	{
+		Field<ENTITY_ID_I32>::type mostCommonEntityId = RGL_ENTITY_INVALID_ID;
 		Vec3f position{0};
 		Aabb3Df aabb{};
+		Vec3f absVelocity{};
+		Vec3f relVelocity{};
 	};
 
 	struct ClassificationProbabilities
@@ -684,20 +686,26 @@ struct RadarTrackObjectsNode : IPointsNodeSingleInput
 
 		RunningStats<Vec3f> position{};
 		RunningStats<float> orientation{};
-		RunningStats<Vec2f> absVelocity{};
-		RunningStats<Vec2f> relVelocity{};
-		RunningStats<Vec2f> absAccel{};
-		RunningStats<Vec2f> relAccel{};
+		RunningStats<Vec3f> absVelocity{};
+		RunningStats<Vec3f> relVelocity{};
+		RunningStats<Vec3f> absAccel{};
+		RunningStats<Vec3f> relAccel{};
 		RunningStats<float> orientationRate{};
 		RunningStats<float> length{};
 		RunningStats<float> width{};
+
+		// Workaround to be able to publish objects in the sensor frame via UDP
+		// Transform points node cannot transform ObjectState
+		Vec3f positionSensorFrame{};
 	};
 
 	RadarTrackObjectsNode();
 
 	void setParameters(float distanceThreshold, float azimuthThreshold, float elevationThreshold, float radialSpeedThreshold,
 	                   float maxMatchingDistance, float maxPredictionTimeFrame, float movementSensitivity);
 
+	void setObjectClasses(const int32_t* entityIds, const rgl_radar_object_class_t* objectClasses, int32_t count);
+
 	void enqueueExecImpl() override;
 
 	bool hasField(rgl_field_t field) const override { return fieldData.contains(field); }
@@ -713,13 +721,16 @@ struct RadarTrackObjectsNode : IPointsNodeSingleInput
 
 private:
 	Vec3f predictObjectPosition(const ObjectState& objectState, double deltaTimeMs) const;
+	void parseEntityIdToClassProbability(Field<ENTITY_ID_I32>::type entityId, ClassificationProbabilities& probabilities);
 	void createObjectState(const ObjectBounds& objectBounds, double currentTimeMs);
 	void updateObjectState(ObjectState& objectState, const Vec3f& updatedPosition, const Aabb3Df& updatedAabb,
-	                       ObjectStatus objectStatus, double currentTimeMs, double deltaTimeMs);
+	                       ObjectStatus objectStatus, double currentTimeMs, double deltaTimeMs, const Vec3f& absVelocity,
+	                       const Vec3f& relVelocity);
 	void updateOutputData();
 
 	std::list<ObjectState> objectStates;
-	std::unordered_map<rgl_field_t, IAnyArray::Ptr> fieldData;
+	std::unordered_map<Field<ENTITY_ID_I32>::type, rgl_radar_object_class_t> entityIdsToClasses;
+	std::unordered_map<rgl_field_t, IAnyArray::Ptr> fieldData; // All should be DeviceAsyncArray
 
 	uint32_t objectIDCounter = 0; // Not static - I assume each ObjectTrackingNode is like a separate radar.
 	std::queue<uint32_t> objectIDPoll;
@@ -734,14 +745,25 @@ struct RadarTrackObjectsNode : IPointsNodeSingleInput
 	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.
+	float movementSensitivity = 0.01f; // Max velocity for an object to be qualified as MovementStatus::Stationary.
+
+	Mat3x4f lookAtSensorFrameTransform { Mat3x4f::identity() };
+	decltype(Time::zero().asMilliseconds()) currentTime { Time::zero().asMilliseconds() };
 
 	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();
 	HostPinnedArray<Field<ELEVATION_F32>::type>::Ptr elevationHostPtr = HostPinnedArray<Field<ELEVATION_F32>::type>::create();
 	HostPinnedArray<Field<RADIAL_SPEED_F32>::type>::Ptr radialSpeedHostPtr =
 	    HostPinnedArray<Field<RADIAL_SPEED_F32>::type>::create();
+	HostPinnedArray<Field<ENTITY_ID_I32>::type>::Ptr entityIdHostPtr = HostPinnedArray<Field<ENTITY_ID_I32>::type>::create();
+	HostPinnedArray<Field<RELATIVE_VELOCITY_VEC3_F32>::type>::Ptr velocityRelHostPtr =
+	    HostPinnedArray<Field<RELATIVE_VELOCITY_VEC3_F32>::type>::create();
+	HostPinnedArray<Field<ABSOLUTE_VELOCITY_VEC3_F32>::type>::Ptr velocityAbsHostPtr =
+	    HostPinnedArray<Field<ABSOLUTE_VELOCITY_VEC3_F32>::type>::create();
+
+	HostPinnedArray<Field<XYZ_VEC3_F32>::type>::Ptr outXyzHostPtr = HostPinnedArray<Field<XYZ_VEC3_F32>::type>::create();
+	HostPinnedArray<Field<ENTITY_ID_I32>::type>::Ptr outEntityIdHostPtr = HostPinnedArray<Field<ENTITY_ID_I32>::type>::create();
 };
 
 struct FilterGroundPointsNode : IPointsNodeSingleInput

src/graph/RadarPostprocessPointsNode.cpp

@@ -278,8 +278,20 @@ void RadarPostprocessPointsNode::RadarCluster::addPoint(Field<RAY_IDX_U32>::type
 	minMaxDistance[1] = std::max(minMaxDistance[1], distance);
 	minMaxAzimuth[0] = std::min(minMaxAzimuth[0], azimuth);
 	minMaxAzimuth[1] = std::max(minMaxAzimuth[1], azimuth);
-	minMaxRadialSpeed[0] = std::min(minMaxRadialSpeed[0], radialSpeed);
-	minMaxRadialSpeed[1] = std::max(minMaxRadialSpeed[1], radialSpeed);
+
+	// There are three reasonable cases that should be handled as intended:
+	// - limit and radialSpeed are nans - limit will be set to nan (from radialSpeed)
+	// - limit is nan and radialSpeed if a number - limit will be set to a number (from radialSpeed)
+	// - limit and radialSpeed are numbers - std::min will be called.
+	// There can technically be a fourth case:
+	// - limit is a number and radialSpeed is nan - this should technically be handled via order of arguments in std::min
+	//   (assumption that comparison inside will return false); what is more important, such candidate should be eliminated
+	//   with isCandidate method;
+	// The fourth case should not occur - radial speed is either nan for all entities (first raycast, with delta time = 0) or
+	// has value for all entities.
+	minMaxRadialSpeed[0] = std::isnan(minMaxRadialSpeed[0]) ? radialSpeed : std::min(minMaxRadialSpeed[0], radialSpeed);
+	minMaxRadialSpeed[1] = std::isnan(minMaxRadialSpeed[1]) ? radialSpeed : std::max(minMaxRadialSpeed[1], radialSpeed);
+
 	minMaxElevation[0] = std::min(minMaxElevation[0], elevation);
 	minMaxElevation[1] = std::max(minMaxElevation[1], elevation);
 }
@@ -291,12 +303,22 @@ inline bool RadarPostprocessPointsNode::RadarCluster::isCandidate(float distance
 	const auto isWithinDistanceUpperBound = distance - radarScope.distance_separation_threshold <= minMaxDistance[1];
 	const auto isWithinAzimuthLowerBound = azimuth + radarScope.azimuth_separation_threshold >= minMaxAzimuth[0];
 	const auto isWithinAzimuthUpperBound = azimuth - radarScope.azimuth_separation_threshold <= minMaxAzimuth[1];
+
 	const auto isWithinRadialSpeedLowerBound = radialSpeed + radarScope.radial_speed_separation_threshold >=
 	                                           minMaxRadialSpeed[0];
 	const auto isWithinRadialSpeedUpperBound = radialSpeed - radarScope.radial_speed_separation_threshold <=
 	                                           minMaxRadialSpeed[1];
+
+	// Radial speed may be nan if this is the first raytracing call (delta time equals 0). When cluster has nan radial speed
+	// limits (technically just do not have radial speed information), the goal below is to ignore radial speed checkout. The
+	// next goal is to allow adding points to cluster, if these points have non nan radial speed - to eliminate undefined
+	// radial speed information from cluster. This should also work well, when limits are fine and candidate's radial speed
+	// is nan - then radial speed checkouts will give false and the candidate will not pass.
+	// Context for radial speed checkouts below - this can be interpreted as "true if any radial speed limit is nan or
+	// candidate's radial speed is within limits"
 	return isWithinDistanceLowerBound && isWithinDistanceUpperBound && isWithinAzimuthLowerBound && isWithinAzimuthUpperBound &&
-	       isWithinRadialSpeedLowerBound && isWithinRadialSpeedUpperBound;
+	       (std::isunordered(minMaxRadialSpeed[0], minMaxRadialSpeed[1]) ||
+	        (isWithinRadialSpeedLowerBound && isWithinRadialSpeedUpperBound));
 }
 
 inline bool RadarPostprocessPointsNode::RadarCluster::canMergeWith(const RadarCluster& other,
@@ -330,7 +352,10 @@ inline bool RadarPostprocessPointsNode::RadarCluster::canMergeWith(const RadarCl
 	                         areRangesWithinThreshold(minMaxRadialSpeed, other.minMaxRadialSpeed,
 	                                                  radarScope->radial_speed_separation_threshold);
 
-	return isDistanceGood && isAzimuthGood && isRadialSpeedGood;
+	// Radial speed check is ignored if one of limits on it, in one of clusters, is nan.
+	return isDistanceGood && isAzimuthGood &&
+	       (isRadialSpeedGood || std::isunordered(minMaxRadialSpeed[0], minMaxRadialSpeed[1]) ||
+	        std::isunordered(other.minMaxRadialSpeed[0], other.minMaxRadialSpeed[1]));
 }
 
 void RadarPostprocessPointsNode::RadarCluster::takeIndicesFrom(RadarCluster&& other)
@@ -339,8 +364,8 @@ void RadarPostprocessPointsNode::RadarCluster::takeIndicesFrom(RadarCluster&& ot
 	minMaxDistance[1] = std::max(minMaxDistance[1], other.minMaxDistance[1]);
 	minMaxAzimuth[0] = std::min(minMaxAzimuth[0], other.minMaxAzimuth[0]);
 	minMaxAzimuth[1] = std::max(minMaxAzimuth[1], other.minMaxAzimuth[1]);
-	minMaxRadialSpeed[0] = std::min(minMaxRadialSpeed[0], other.minMaxRadialSpeed[0]);
-	minMaxRadialSpeed[1] = std::max(minMaxRadialSpeed[1], other.minMaxRadialSpeed[1]);
+	minMaxRadialSpeed[0] = std::isnan(other.minMaxRadialSpeed[0]) ? minMaxRadialSpeed[0] : std::min(other.minMaxRadialSpeed[0], minMaxRadialSpeed[0]);
+	minMaxRadialSpeed[1] = std::isnan(other.minMaxRadialSpeed[1]) ? minMaxRadialSpeed[1] : std::min(other.minMaxRadialSpeed[1], minMaxRadialSpeed[1]);
 	minMaxElevation[0] = std::min(minMaxElevation[0], other.minMaxElevation[0]);
 	minMaxElevation[1] = std::max(minMaxElevation[1], other.minMaxElevation[1]);