Skip to content

Commit

Permalink
Add radar object classification (#298)
Browse files Browse the repository at this point in the history 
* Add initial support for object classification in RadarTrackObjectsNode.

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

* Add API call for setting radar object classes.

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

* Update radar track objects tests to handle object ids.

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

* Add safety static_cast.

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

* Make fixes based on the review.

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

* 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 <[email protected]>

* 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 <[email protected]>
Co-authored-by: Maja Nagarnowicz <[email protected]>
Co-authored-by: Mateusz Szczygielski <[email protected]>
  • Loading branch information
@PawelLiberadzki @nebraszka @msz-rai
3 people committed Jul 13, 2024
1 parent 05cdd3b commit 86ab331
Show file tree
Hide file tree
Showing 11 changed files with 343 additions and 81 deletions.
41 changes: 36 additions & 5 deletions include/rgl/api/core.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -151,6 +151,22 @@ static_assert(std::is_trivial<rgl_radar_scope_t>::value);
static_assert(std::is_standard_layout<rgl_radar_scope_t>::value);
#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.
Expand Down Expand Up @@ -868,10 +884,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).
Expand All @@ -880,13 +898,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.
Expand Down
26 changes: 26 additions & 0 deletions src/api/apiCore.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1232,6 +1232,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)
{
Expand Down
7 changes: 4 additions & 3 deletions src/gpu/optixPrograms.cu
View file
Original file line number Diff line number Diff line change
Expand Up @@ -221,9 +221,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);
}

Expand Down
4 changes: 0 additions & 4 deletions src/graph/Interfaces.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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); }
Expand Down
40 changes: 31 additions & 9 deletions src/graph/NodesCore.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
#include <math/Aabb.h>
#include <math/RunningStats.hpp>
#include <gpu/MultiReturn.hpp>
#include <Time.hpp>


struct FormatPointsNode : IPointsNodeSingleInput
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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); }
Expand All @@ -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;
Expand All @@ -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
Expand Down
37 changes: 31 additions & 6 deletions src/graph/RadarPostprocessPointsNode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand All @@ -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,
Expand Down Expand Up @@ -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)
Expand All @@ -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]);

Expand Down
Loading

0 comments on commit 86ab331

Please sign in to comment.