Skip to content

Commit

Permalink
Tracking reflector with radar postprocess input test
Browse files Browse the repository at this point in the history
  • Loading branch information
@nebraszka
nebraszka committed Jun 18, 2024
1 parent bb367b1 commit 40539f7
Show file tree
Hide file tree
Showing 4 changed files with 324 additions and 32 deletions.
30 changes: 30 additions & 0 deletions test/include/helpers/lidarHelpers.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,3 +45,33 @@ static std::vector<rgl_mat3x4f> makeGridOfParallelRays(Vec2f minCoord, Vec2f max
}
return rays;
}


#if RGL_BUILD_ROS2_EXTENSION
#include <rgl/api/extensions/ros2.h>

#include <RGLFields.hpp>

/**
* @brief Auxiliary lidar graph for imaging entities on the scene
*/
static rgl_node_t constructCameraGraph(const rgl_mat3x4f& cameraPose, const char* cameraName = "camera")
{
const std::vector<rgl_field_t> fields{XYZ_VEC3_F32};
const std::vector<rgl_mat3x4f> cameraRayTf = makeLidar3dRays(360.0f, 180.0f, 0.5f, 0.5f);
rgl_node_t cameraRays = nullptr, cameraTransform = nullptr, cameraRaytrace = nullptr, cameraFormat = nullptr,
cameraPublish = nullptr;

EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&cameraRays, cameraRayTf.data(), cameraRayTf.size()));
EXPECT_RGL_SUCCESS(rgl_node_rays_transform(&cameraTransform, &cameraPose));
EXPECT_RGL_SUCCESS(rgl_node_raytrace(&cameraRaytrace, nullptr));
EXPECT_RGL_SUCCESS(rgl_node_points_format(&cameraFormat, fields.data(), fields.size()));
EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&cameraPublish, cameraName, "world"));
EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(cameraRays, cameraTransform));
EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(cameraTransform, cameraRaytrace));
EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(cameraRaytrace, cameraFormat));
EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(cameraFormat, cameraPublish));

return cameraRays;
}
#endif
99 changes: 99 additions & 0 deletions test/include/helpers/radarHelpers.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,99 @@
#pragma once

#include <vector>
#include <math/Mat3x4f.hpp>

struct PostProcessNodeParams
{
rgl_radar_scope_t scope;
int32_t radarScopesCount;
float azimuthStepRad;
float elevationStepRad;
float frequencyHz;
float powerTransmittedDdm;
float antennaGainDbi;
float noiseMean;
float noiseStdDev;
};

struct TrackObjectNodeParams
{
float distanceThreshold;
float azimuthThreshold;
float elevationThreshold;
float radialSpeedThreshold;
float maxMatchingDistance;
float maxPredictionTimeFrame;
float movementSensitivity;
};

#if RGL_BUILD_ROS2_EXTENSION
#include <rgl/api/extensions/ros2.h>

static void constructRadarPostProcessObjectTrackingGraph(
const std::vector<rgl_mat3x4f>& radarRays, const rgl_mat3x4f& radarRayTf, rgl_node_t& postProcessNode,
const PostProcessNodeParams& postProcessNodeParams, rgl_node_t& trackObjectNode,
const TrackObjectNodeParams& trackObjectNodeParams, const std::vector<Field<ENTITY_ID_I32>::type>& entityIds,
const std::vector<rgl_radar_object_class_t>& objectClasses, bool withPublish = false)
{
rgl_node_t radarRaysNode = nullptr, radarRaysTfNode = nullptr, raytraceNode = nullptr, noiseNode = nullptr,
compactNode = nullptr, formatPostProcessNode = nullptr, formatTrackObjectNode = nullptr,
ros2PublishPostProcessNode = nullptr, ros2PublishTrackObjectNode = nullptr;

ASSERT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&radarRaysNode, radarRays.data(), radarRays.size()));
ASSERT_RGL_SUCCESS(rgl_node_rays_transform(&radarRaysTfNode, &radarRayTf));
ASSERT_RGL_SUCCESS(rgl_node_raytrace(&raytraceNode, nullptr));
ASSERT_RGL_SUCCESS(
rgl_node_points_compact_by_field(&compactNode, IS_HIT_I32)); // RadarComputeEnergyPointsNode requires dense input
ASSERT_RGL_SUCCESS(rgl_node_points_radar_postprocess(
&postProcessNode, &postProcessNodeParams.scope, postProcessNodeParams.radarScopesCount,
postProcessNodeParams.azimuthStepRad, postProcessNodeParams.elevationStepRad, postProcessNodeParams.frequencyHz,
postProcessNodeParams.powerTransmittedDdm, postProcessNodeParams.antennaGainDbi, postProcessNodeParams.noiseMean,
postProcessNodeParams.noiseStdDev));
ASSERT_RGL_SUCCESS(rgl_node_points_radar_track_objects(
&trackObjectNode, trackObjectNodeParams.distanceThreshold, trackObjectNodeParams.azimuthThreshold,
trackObjectNodeParams.elevationThreshold, trackObjectNodeParams.radialSpeedThreshold,
trackObjectNodeParams.maxMatchingDistance, trackObjectNodeParams.maxPredictionTimeFrame,
trackObjectNodeParams.movementSensitivity));
ASSERT_RGL_SUCCESS(
rgl_node_points_radar_set_classes(trackObjectNode, entityIds.data(), objectClasses.data(), entityIds.size()));

ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(radarRaysNode, radarRaysTfNode));
ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(radarRaysTfNode, raytraceNode));
ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(raytraceNode, compactNode));
ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(compactNode, postProcessNode));
ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(postProcessNode, trackObjectNode));

if (withPublish) {
const std::vector<rgl_field_t> formatFields = {XYZ_VEC3_F32, ENTITY_ID_I32};
ASSERT_RGL_SUCCESS(rgl_node_points_format(&formatPostProcessNode, formatFields.data(), formatFields.size()));
ASSERT_RGL_SUCCESS(rgl_node_points_format(&formatTrackObjectNode, formatFields.data(), formatFields.size()));
ASSERT_RGL_SUCCESS(rgl_node_points_ros2_publish(&ros2PublishPostProcessNode, "radar_detection", "world"));
ASSERT_RGL_SUCCESS(rgl_node_points_ros2_publish(&ros2PublishTrackObjectNode, "radar_track_object", "world"));
ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(trackObjectNode, formatTrackObjectNode));
ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(postProcessNode, formatPostProcessNode));
ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(formatPostProcessNode, ros2PublishPostProcessNode));
ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(formatTrackObjectNode, ros2PublishTrackObjectNode));
}
}
#endif

static std::vector<rgl_mat3x4f> genRadarRays(const float minAzimuth, const float maxAzimuth, const float minElevation,
const float maxElevation, const float azimuthStep, const float elevationStep)
{
std::vector<rgl_mat3x4f> rays;
for (auto a = minAzimuth; a <= maxAzimuth; a += azimuthStep) {
for (auto e = minElevation; e <= maxElevation; e += elevationStep) {
// By default, the rays are directed along the Z-axis
// So first, we rotate them around the Y-axis to point towards the X-axis (to be RVIZ2 compatible)
// Then, rotation around Z is azimuth, around Y is elevation
const auto ray = Mat3x4f::rotationDeg(0, e, a) * Mat3x4f::rotationDeg(0, 90, 0);
rays.emplace_back(ray.toRGL());

const auto rayDir = ray * Vec3f{0, 0, 1};
//printf("rayDir: %.2f %.2f %.2f\n", rayDir.x(), rayDir.y(), rayDir.z());
}
}

return rays;
}
182 changes: 182 additions & 0 deletions test/src/graph/nodes/RadarTrackObjectsNodeTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -394,4 +394,186 @@ TEST_F(RadarTrackObjectsNodeTest, tracking_objects_test)
++iterationCounter;
}
}

#include <filesystem>
#include <helpers/sceneHelpers.hpp>
#include <helpers/radarHelpers.hpp>
#include <helpers/lidarHelpers.hpp>

TEST_F(RadarTrackObjectsNodeTest, tracking_object_with_radar_postprocess_input_test)
{
GTEST_SKIP_("Debug test on development stage.");

// Radar rays parameters
constexpr auto minAzimuth = -90.0f;
constexpr auto maxAzimuth = 90.0f;
constexpr auto minElevation = -90.0f;
constexpr auto maxElevation = 90.0f;
constexpr auto azimuthStep = 0.49f;
constexpr auto elevationStep = 0.49f;

constexpr PostProcessNodeParams radarPostProcessParams{
.scope = {.begin_distance = 1.0f,
.end_distance = 10.0f,
.distance_separation_threshold = 0.3f,
.radial_speed_separation_threshold = 0.3f,
.azimuth_separation_threshold = 8.0f * (std::numbers::pi_v<float> / 180.0f)},
.radarScopesCount = 1,
.azimuthStepRad = azimuthStep * (std::numbers::pi_v<float> / 180.0f),
.elevationStepRad = elevationStep * (std::numbers::pi_v<float> / 180.0f),
.frequencyHz = 79E9f,
.powerTransmittedDdm = 31.0f,
.antennaGainDbi = 27.0f,
.noiseMean = 60.0f,
.noiseStdDev = 1.0f,
};

constexpr TrackObjectNodeParams trackingParams{
.distanceThreshold = 2.0f,
.azimuthThreshold = 0.5f,
.elevationThreshold = 0.5f,
.radialSpeedThreshold = 0.5f,
.maxMatchingDistance = 1.0f,
.maxPredictionTimeFrame = 1.0f,
.movementSensitivity = 0.01,
};

// Scene
rgl_mesh_t reflector2dMesh = loadFromSTL(std::filesystem::path(RGL_TEST_DATA_DIR) / "reflector2d.stl");
const std::vector<Field<ENTITY_ID_I32>::type> entityIds{1};
rgl_entity_t entity = nullptr;
const auto entityTransl = Vec3f{5.0f, 0.0f, 0.0f};
const auto entityPose = Mat3x4f::translation(entityTransl).toRGL();

ASSERT_RGL_SUCCESS(rgl_entity_create(&entity, nullptr, reflector2dMesh));
ASSERT_RGL_SUCCESS(rgl_entity_set_id(entity, entityIds.at(0)));
ASSERT_RGL_SUCCESS(rgl_entity_set_pose(entity, &entityPose));

// Radar rays
const std::vector<rgl_mat3x4f> radarRays = genRadarRays(minAzimuth, maxAzimuth, minElevation, maxElevation, azimuthStep,
elevationStep);
const Vec3f radarRayTransl = Vec3f{0.0f};
const rgl_mat3x4f radarRayTf = Mat3x4f::translation(radarRayTransl).toRGL();

// Radar track objects graph
rgl_node_t postProcessNode = nullptr, trackObjectsNode = nullptr;
const std::vector<rgl_radar_object_class_t> objectClasses{
static_cast<rgl_radar_object_class_t>(entityIds.at(0) % RGL_RADAR_CLASS_COUNT)};
constructRadarPostProcessObjectTrackingGraph(radarRays, radarRayTf, postProcessNode, radarPostProcessParams,
trackObjectsNode, trackingParams, entityIds, objectClasses, true);

// Auxiliary lidar graph for imaging entities on the scene
const Vec3f cameraTransl = radarRayTransl + Vec3f{-2.0f, 0.0f, 0.0f};
const rgl_mat3x4f cameraPose = Mat3x4f::translation(cameraTransl).toRGL();
rgl_node_t cameraRays = constructCameraGraph(cameraPose);

constexpr uint64_t frameTime = 5 * 1e6; // ms
int iterationCounter = 0;
while (true) {
ASSERT_RGL_SUCCESS(rgl_scene_set_time(nullptr, iterationCounter * frameTime));

ASSERT_RGL_SUCCESS(rgl_graph_run(cameraRays));
ASSERT_RGL_SUCCESS(rgl_graph_run(trackObjectsNode));

// Verify objects count on the output - only one object is expected
int32_t detectedObjectsCount = 0, objectsSize = 0;
ASSERT_RGL_SUCCESS(rgl_graph_get_result_size(trackObjectsNode, XYZ_VEC3_F32, &detectedObjectsCount, &objectsSize));

int32_t expectedObjectsCount = 1;
ASSERT_EQ(detectedObjectsCount, expectedObjectsCount);

std::this_thread::sleep_for(std::chrono::milliseconds(1000));

++iterationCounter;
}
}

TEST_F(RadarTrackObjectsNodeTest, tracking_kinematic_object_with_radar_postprocess_input_test)
{
GTEST_SKIP_("Debug test on development stage.");

// Radar rays parameters
constexpr auto minAzimuth = -90.0f;
constexpr auto maxAzimuth = 90.0f;
constexpr auto minElevation = -90.0f;
constexpr auto maxElevation = 90.0f;
constexpr auto azimuthStep = 0.49f;
constexpr auto elevationStep = 0.49f;

constexpr PostProcessNodeParams radarPostProcessParams{
.scope = {.begin_distance = 15.0f,
.end_distance = 30.0f,
.distance_separation_threshold = 0.3f,
.radial_speed_separation_threshold = 0.3f,
.azimuth_separation_threshold = 8.0f * (std::numbers::pi_v<float> / 180.0f)},
.radarScopesCount = 1,
.azimuthStepRad = azimuthStep * (std::numbers::pi_v<float> / 180.0f),
.elevationStepRad = elevationStep * (std::numbers::pi_v<float> / 180.0f),
.frequencyHz = 79E9f,
.powerTransmittedDdm = 31.0f,
.antennaGainDbi = 27.0f,
.noiseMean = 60.0f,
.noiseStdDev = 1.0f,
};

constexpr TrackObjectNodeParams trackingParams{
.distanceThreshold = 2.0f,
.azimuthThreshold = 0.5f,
.elevationThreshold = 0.5f,
.radialSpeedThreshold = 0.5f,
.maxMatchingDistance = 1.0f,
.maxPredictionTimeFrame = 1.0f,
.movementSensitivity = 0.01,
};

// Scene
rgl_mesh_t reflector2dMesh = loadFromSTL(std::filesystem::path(RGL_TEST_DATA_DIR) / "reflector2d.stl");
const std::vector<Field<ENTITY_ID_I32>::type> entityIds{1};
rgl_entity_t entity = nullptr;

ASSERT_RGL_SUCCESS(rgl_entity_create(&entity, nullptr, reflector2dMesh));
ASSERT_RGL_SUCCESS(rgl_entity_set_id(entity, entityIds.at(0)));

// Radar rays
const std::vector<rgl_mat3x4f> radarRays = genRadarRays(minAzimuth, maxAzimuth, minElevation, maxElevation, azimuthStep,
elevationStep);
const Vec3f radarRayTransl = Vec3f{0.0f};
const rgl_mat3x4f radarRayTf = Mat3x4f::translation(radarRayTransl).toRGL();

// Radar track objects graph
rgl_node_t postProcessNode = nullptr, trackObjectsNode = nullptr;
const std::vector<rgl_radar_object_class_t> objectClasses{
static_cast<rgl_radar_object_class_t>(entityIds.at(0) % RGL_RADAR_CLASS_COUNT)};
constructRadarPostProcessObjectTrackingGraph(radarRays, radarRayTf, postProcessNode, radarPostProcessParams,
trackObjectsNode, trackingParams, entityIds, objectClasses, true);

// Auxiliary lidar graph for imaging entities on the scene
const Vec3f cameraTransl = radarRayTransl + Vec3f{0.0f, 0.0f, 2.0f};
const rgl_mat3x4f cameraPose = Mat3x4f::translation(cameraTransl).toRGL();
rgl_node_t cameraRays = constructCameraGraph(cameraPose);

// Kinematic object parameters
constexpr float maxObjectRadarDistance = radarPostProcessParams.scope.end_distance + 2.0f;
constexpr float minObjectRadarDistance = radarPostProcessParams.scope.begin_distance - 2.0f;
constexpr float amplitude = (maxObjectRadarDistance - minObjectRadarDistance) / 2.0f;
constexpr float shift = (maxObjectRadarDistance + minObjectRadarDistance) / 2.0f;

constexpr uint64_t frameTime = 5 * 1e6; // ms
int iterationCounter = 0;
while (true) {
const auto entityTransl = radarRayTransl +
Vec3f{amplitude * std::sin(static_cast<float>(iterationCounter) * 0.1f) + shift, 0.0f, 0.0f};
const auto entityPose = Mat3x4f::translation(entityTransl).toRGL();
ASSERT_RGL_SUCCESS(rgl_entity_set_pose(entity, &entityPose));

ASSERT_RGL_SUCCESS(rgl_scene_set_time(nullptr, iterationCounter * frameTime));

ASSERT_RGL_SUCCESS(rgl_graph_run(postProcessNode));
ASSERT_RGL_SUCCESS(rgl_graph_run(cameraRays));

std::this_thread::sleep_for(std::chrono::milliseconds(1000));

++iterationCounter;
}
}
#endif
45 changes: 13 additions & 32 deletions test/src/scene/radarTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,14 +24,15 @@ struct numpunct : std::numpunct<char>
};

struct RadarTest : RGLTest
{};

const auto minAzimuth = -65.0f;
const auto maxAzimuth = 65.0f;
const auto minElevation = -7.5f;
const auto maxElevation = 7.5f;
const auto azimuthStep = 0.49f;
const auto elevationStep = 0.49f;
{
protected:
const float minAzimuth = -65.0f;
const float maxAzimuth = 65.0f;
const float minElevation = -7.5f;
const float maxElevation = 7.5f;
const float azimuthStep = 0.49f;
const float elevationStep = 0.49f;
};

rgl_mesh_t getFlatPlate(float dim)
{
Expand Down Expand Up @@ -59,37 +60,17 @@ rgl_mesh_t getFlatPlate(float dim)
return outMesh;
}

std::vector<rgl_mat3x4f> genRadarRays()
{
std::vector<rgl_mat3x4f> rays;
for (auto a = minAzimuth; a <= maxAzimuth; a += azimuthStep) {
for (auto e = minElevation; e <= maxElevation; e += elevationStep) {
// By default, the rays are directed along the Z-axis
// So first, we rotate them around the Y-axis to point towards the X-axis (to be RVIZ2 compatible)
// Then, rotation around Z is azimuth, around Y is elevation
const auto ray = Mat3x4f::rotationDeg(a, e, 0);
rays.emplace_back(ray.toRGL());

// The above will have to be modified again - we assume that target is farther in X axis when in fact
// we use Z as RGL LiDAR front. Remember to update.

const auto rayDir = ray * Vec3f{0, 0, 1};
//printf("rayDir: %.2f %.2f %.2f\n", rayDir.x(), rayDir.y(), rayDir.z());
}
}

return rays;
}

namespace fs = std::filesystem;
using namespace std::chrono_literals;

#include <helpers/radarHelpers.hpp>

TEST_F(RadarTest, rotating_reflector_2d)
{
GTEST_SKIP();

// Setup sensor and graph
std::vector<rgl_mat3x4f> raysData = genRadarRays();
std::vector<rgl_mat3x4f> raysData = genRadarRays(minAzimuth, maxAzimuth, minElevation, maxElevation, azimuthStep, elevationStep);
// std::vector<rgl_mat3x4f> raysData = {
// Mat3x4f::rotationDeg(0, 0, 0).toRGL(),
//// Mat3x4f::rotationDeg(2.5, 0, 0).toRGL(),
Expand All @@ -111,7 +92,7 @@ TEST_F(RadarTest, rotating_reflector_2d)
lidarPublish = nullptr;

rgl_node_t raysTransform = nullptr;
auto raycasterTransform = Mat3x4f::rotationDeg(0.0f, 90.0f, 0.0f).toRGL();
auto raycasterTransform = Mat3x4f::identity().toRGL();

EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&rays, raysData.data(), raysData.size()));
EXPECT_RGL_SUCCESS(rgl_node_rays_transform(&raysTransform, &raycasterTransform));
Expand Down

0 comments on commit 40539f7

Please sign in to comment.