From c4136ad0120ebbe4e010c3e015e99e5c9ae1a439 Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski <112629916+msz-rai@users.noreply.github.com> Date: Wed, 29 May 2024 16:51:15 +0200 Subject: [PATCH] Add support for Laser Retro (#291) * Add LaserRetro field * Add test * Formatting fix * Change order in rgl_field_t to work with current taped test file --- include/rgl/api/core.h | 16 ++++++ src/RGLFields.hpp | 8 +++ src/api/apiCore.cpp | 17 ++++++ src/gpu/RaytraceRequestContext.hpp | 1 + src/gpu/ShaderBindingTableTypes.h | 1 + src/gpu/optixPrograms.cu | 12 ++-- src/graph/RaytraceNode.cpp | 1 + src/scene/Entity.cpp | 6 ++ src/scene/Entity.hpp | 6 ++ src/scene/Scene.cpp | 1 + src/tape/TapeCore.hpp | 2 + test/CMakeLists.txt | 1 + test/include/helpers/fieldGenerators.hpp | 3 + test/include/helpers/testPointCloud.hpp | 5 +- test/src/TapeTest.cpp | 2 + test/src/scene/entityLaserRetroTest.cpp | 70 ++++++++++++++++++++++++ 16 files changed, 146 insertions(+), 6 deletions(-) create mode 100644 test/src/scene/entityLaserRetroTest.cpp diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h index 78cdd28e0..050b7e955 100644 --- a/include/rgl/api/core.h +++ b/include/rgl/api/core.h @@ -374,6 +374,13 @@ typedef enum : int32_t */ RGL_FIELD_RAY_POSE_MAT3x4_F32, + /** + * Lidar reflective value. Similar to the `RGL_FIELD_INTENSITY_F32` but set as a single value for the entire entity. + * Could be replaced with `RGL_FIELD_INTENSITY_F32` and a 1x1 texture when float-type texture will be supported. + * For non-hit points zero is assigned. + */ + RGL_FIELD_LASER_RETRO_F32, + // Dummy fields RGL_FIELD_PADDING_8 = 1024, RGL_FIELD_PADDING_16, @@ -526,6 +533,15 @@ RGL_API rgl_status_t rgl_entity_set_id(rgl_entity_t entity, int32_t id); */ RGL_API rgl_status_t rgl_entity_set_intensity_texture(rgl_entity_t entity, rgl_texture_t texture); +/** + * Set laser retro value for the given Entity. + * The value can be retrieved from `RGL_FIELD_LASER_RETRO_F32` point cloud field. + * Default retro for the Entity is zero. + * @param entity Entity to modify. + * @param retro Laser retro value to set. + */ +RGL_API rgl_status_t rgl_entity_set_laser_retro(rgl_entity_t entity, float retro); + /** * Assigns value true to out_alive if the given entity is known and has not been destroyed, * assigns value false otherwise. diff --git a/src/RGLFields.hpp b/src/RGLFields.hpp index 93f38dd42..5a119ddec 100644 --- a/src/RGLFields.hpp +++ b/src/RGLFields.hpp @@ -40,6 +40,7 @@ typedef unsigned char TextureTexelFormat; #define RAY_IDX_U32 RGL_FIELD_RAY_IDX_U32 #define ENTITY_ID_I32 RGL_FIELD_ENTITY_ID_I32 #define INTENSITY_F32 RGL_FIELD_INTENSITY_F32 +#define LASER_RETRO_F32 RGL_FIELD_LASER_RETRO_F32 #define RING_ID_U16 RGL_FIELD_RING_ID_U16 #define AZIMUTH_F32 RGL_FIELD_AZIMUTH_F32 #define ELEVATION_F32 RGL_FIELD_ELEVATION_F32 @@ -69,6 +70,7 @@ inline const std::set& getAllRealFields() RAY_IDX_U32, ENTITY_ID_I32, INTENSITY_F32, + LASER_RETRO_F32, RING_ID_U16, AZIMUTH_F32, ELEVATION_F32, @@ -115,6 +117,7 @@ FIELD(XYZ_VEC3_F32, Vec3f); FIELD(RAY_IDX_U32, uint32_t); // PCL uses uint32_t FIELD(ENTITY_ID_I32, int32_t); FIELD(INTENSITY_F32, float); +FIELD(LASER_RETRO_F32, float); FIELD(IS_HIT_I32, int32_t); // Signed may be faster FIELD(IS_GROUND_I32, int32_t); // Signed may be faster FIELD(DISTANCE_F32, float); @@ -146,6 +149,7 @@ inline std::size_t getFieldSize(rgl_field_t type) case IS_HIT_I32: return Field::size; case IS_GROUND_I32: return Field::size; case INTENSITY_F32: return Field::size; + case LASER_RETRO_F32: return Field::size; case RING_ID_U16: return Field::size; case AZIMUTH_F32: return Field::size; case ELEVATION_F32: return Field::size; @@ -202,6 +206,7 @@ inline std::shared_ptr createArray(rgl_field_t type, Args&&... args) case RAY_IDX_U32: return Subclass::type>::create(std::forward(args)...); case ENTITY_ID_I32: return Subclass::type>::create(std::forward(args)...); case INTENSITY_F32: return Subclass::type>::create(std::forward(args)...); + case LASER_RETRO_F32: return Subclass::type>::create(std::forward(args)...); case RING_ID_U16: return Subclass::type>::create(std::forward(args)...); case AZIMUTH_F32: return Subclass::type>::create(std::forward(args)...); case ELEVATION_F32: return Subclass::type>::create(std::forward(args)...); @@ -235,6 +240,7 @@ inline std::string toString(rgl_field_t type) case RAY_IDX_U32: return "RAY_IDX_U32"; case ENTITY_ID_I32: return "ENTITY_ID_I32"; case INTENSITY_F32: return "INTENSITY_F32"; + case LASER_RETRO_F32: return "LASER_RETRO_F32"; case RING_ID_U16: return "RING_ID_U16"; case AZIMUTH_F32: return "AZIMUTH_F32"; case ELEVATION_F32: return "ELEVATION_F32"; @@ -272,6 +278,7 @@ inline std::vector toRos2Fields(rgl_field_t type) case RAY_IDX_U32: return {sensor_msgs::msg::PointField::UINT32}; case ENTITY_ID_I32: return {sensor_msgs::msg::PointField::INT32}; case INTENSITY_F32: return {sensor_msgs::msg::PointField::FLOAT32}; + case LASER_RETRO_F32: return {sensor_msgs::msg::PointField::FLOAT32}; case RING_ID_U16: return {sensor_msgs::msg::PointField::UINT16}; case AZIMUTH_F32: return {sensor_msgs::msg::PointField::FLOAT32}; case ELEVATION_F32: return {sensor_msgs::msg::PointField::FLOAT32}; @@ -318,6 +325,7 @@ inline std::vector toRos2Names(rgl_field_t type) case ENTITY_ID_I32: return {"entity_id"}; case RAY_IDX_U32: return {"ray_idx"}; case INTENSITY_F32: return {"intensity"}; + case LASER_RETRO_F32: return {"laser_retro"}; case RING_ID_U16: return {"ring"}; case AZIMUTH_F32: return {"azimuth"}; case ELEVATION_F32: return {"elevation"}; diff --git a/src/api/apiCore.cpp b/src/api/apiCore.cpp index 76030574f..0069140a4 100644 --- a/src/api/apiCore.cpp +++ b/src/api/apiCore.cpp @@ -365,6 +365,23 @@ void TapeCore::tape_entity_set_intensity_texture(const YAML::Node& yamlNode, Pla state.textures.at(yamlNode[1].as())); } +RGL_API rgl_status_t rgl_entity_set_laser_retro(rgl_entity_t entity, float retro) +{ + auto status = rglSafeCall([&]() { + RGL_API_LOG("rgl_entity_set_laser_retro(entity={}, retro={})", (void*) entity, retro); + CHECK_ARG(entity != nullptr); + Entity::validatePtr(entity)->setLaserRetro(retro); + }); + TAPE_HOOK(entity, retro); + return status; +} + +void TapeCore::tape_entity_set_laser_retro(const YAML::Node& yamlNode, PlaybackState& state) +{ + rgl_entity_set_laser_retro(state.entities.at(yamlNode[0].as()), + yamlNode[1].as::type>()); +} + rgl_status_t rgl_entity_is_alive(rgl_entity_t entity, bool* out_alive) { auto status = rglSafeCall([&]() { diff --git a/src/gpu/RaytraceRequestContext.hpp b/src/gpu/RaytraceRequestContext.hpp index b35554392..7c95a0711 100644 --- a/src/gpu/RaytraceRequestContext.hpp +++ b/src/gpu/RaytraceRequestContext.hpp @@ -52,6 +52,7 @@ struct RaytraceRequestContext Field::type* ringIdx; Field::type* distance; Field::type* intensity; + Field::type* laserRetro; Field::type* timestamp; Field::type* entityId; Field::type* pointAbsVelocity; diff --git a/src/gpu/ShaderBindingTableTypes.h b/src/gpu/ShaderBindingTableTypes.h index addb17449..49f09b012 100644 --- a/src/gpu/ShaderBindingTableTypes.h +++ b/src/gpu/ShaderBindingTableTypes.h @@ -18,6 +18,7 @@ struct EntitySBTData const Vec2f* textureCoords; size_t textureCoordsCount; cudaTextureObject_t texture; + float laserRetro; // Info about the previous frame: Mat3x4f prevFrameLocalToWorld; // Must not be used if !hasPrevFrameLocalToWorld diff --git a/src/gpu/optixPrograms.cu b/src/gpu/optixPrograms.cu index 0a1418eca..30020692f 100644 --- a/src/gpu/optixPrograms.cu +++ b/src/gpu/optixPrograms.cu @@ -58,7 +58,7 @@ __forceinline__ __device__ Vec3f decodePayloadVec3f(const Vec3fPayload& src) template __forceinline__ __device__ void saveRayResult(const Vec3f& xyz, float distance, float intensity, const int objectID, const Vec3f& absVelocity, const Vec3f& relVelocity, float radialSpeed, - const Vec3f& normal, float incidentAngle) + const Vec3f& normal, float incidentAngle, float laserRetro) { const int rayIdx = optixGetLaunchIndex().x; if (ctx.xyz != nullptr) { @@ -101,6 +101,9 @@ __forceinline__ __device__ void saveRayResult(const Vec3f& xyz, float distance, if (ctx.incidentAngle != nullptr) { ctx.incidentAngle[rayIdx] = incidentAngle; } + if (ctx.laserRetro != nullptr) { + ctx.laserRetro[rayIdx] = laserRetro; + } } __forceinline__ __device__ void saveNonHitRayResult(float nonHitDistance) @@ -112,7 +115,7 @@ __forceinline__ __device__ void saveNonHitRayResult(float nonHitDistance) displacement = {isnan(displacement.x()) ? 0 : displacement.x(), isnan(displacement.y()) ? 0 : displacement.y(), isnan(displacement.z()) ? 0 : displacement.z()}; Vec3f xyz = origin + displacement; - saveRayResult(xyz, nonHitDistance, 0, RGL_ENTITY_INVALID_ID, Vec3f{NAN}, Vec3f{NAN}, 0.001f, Vec3f{NAN}, NAN); + saveRayResult(xyz, nonHitDistance, 0, RGL_ENTITY_INVALID_ID, Vec3f{NAN}, Vec3f{NAN}, 0.0f, Vec3f{NAN}, NAN, 0.0f); } extern "C" __global__ void __raygen__() @@ -179,7 +182,8 @@ extern "C" __global__ void __closesthit__() Vec3f hitObject = Vec3f((1 - u - v) * A + u * B + v * C); Vec3f hitWorld = optixTransformPointFromObjectToWorldSpace(hitObject); - int objectID = optixGetInstanceId(); + const int objectID = optixGetInstanceId(); + const float laserRetro = entityData.laserRetro; Vec3f origin = decodePayloadVec3f({optixGetPayload_0(), optixGetPayload_1(), optixGetPayload_2()}); @@ -274,7 +278,7 @@ extern "C" __global__ void __closesthit__() } saveRayResult(hitWorld, distance, intensity, objectID, absPointVelocity, relPointVelocity, radialSpeed, wNormal, - incidentAngle); + incidentAngle, laserRetro); } extern "C" __global__ void __miss__() { saveNonHitRayResult(ctx.farNonHitDistance); } diff --git a/src/graph/RaytraceNode.cpp b/src/graph/RaytraceNode.cpp index e07a2e616..17ba2eba0 100644 --- a/src/graph/RaytraceNode.cpp +++ b/src/graph/RaytraceNode.cpp @@ -103,6 +103,7 @@ void RaytraceNode::enqueueExecImpl() .ringIdx = getPtrTo(), .distance = getPtrTo(), .intensity = getPtrTo(), + .laserRetro = getPtrTo(), .timestamp = getPtrTo(), .entityId = getPtrTo(), .pointAbsVelocity = getPtrTo(), diff --git a/src/scene/Entity.cpp b/src/scene/Entity.cpp index 71661d71f..10744a88a 100644 --- a/src/scene/Entity.cpp +++ b/src/scene/Entity.cpp @@ -44,6 +44,12 @@ void Entity::setId(int newId) Scene::instance().requestASRebuild(); // Update instanceId field in AS } +void Entity::setLaserRetro(float retro) +{ + laserRetro = retro; + Scene::instance().requestSBTRebuild(); +} + void Entity::setIntensityTexture(std::shared_ptr texture) { intensityTexture = texture; diff --git a/src/scene/Entity.hpp b/src/scene/Entity.hpp index fdb915c17..f610d56da 100644 --- a/src/scene/Entity.hpp +++ b/src/scene/Entity.hpp @@ -56,6 +56,11 @@ struct Entity : APIObject */ void setIntensityTexture(std::shared_ptr texture); + /** + * Sets laser retro that will be used as a point attribute LASER_RETRO_F32 when a ray hits this entity. + */ + void setLaserRetro(float retro); + /** * Returns Entity's transform such that it is possible to compute meaningful velocity between it and the current transform. * Most often it will return the previous frame (if Entity is updated on each frame). See source for details. @@ -84,6 +89,7 @@ struct Entity : APIObject TransformWithTime formerTransformInfo{Mat3x4f::identity(), std::nullopt}; Field::type id{RGL_DEFAULT_ENTITY_ID}; + float laserRetro{}; std::shared_ptr mesh{}; std::shared_ptr intensityTexture{}; diff --git a/src/scene/Scene.cpp b/src/scene/Scene.cpp index 385faa0e0..fd55e3d98 100644 --- a/src/scene/Scene.cpp +++ b/src/scene/Scene.cpp @@ -89,6 +89,7 @@ OptixShaderBindingTable Scene::buildSBT() .textureCoords = mesh->dTextureCoords.has_value() ? mesh->dTextureCoords.value()->getReadPtr() : nullptr, .textureCoordsCount = mesh->dTextureCoords.has_value() ? mesh->dTextureCoords.value()->getCount() : 0, .texture = entity->intensityTexture != nullptr ? entity->intensityTexture->getTextureObject() : 0, + .laserRetro = entity->laserRetro, .prevFrameLocalToWorld = prevFrameTransform.value_or(Mat3x4f::identity()), .hasPrevFrameLocalToWorld = prevFrameTransform.has_value(), .vertexDisplacementSincePrevFrame = mesh->getSkinningDisplacementSinceLastFrame(), diff --git a/src/tape/TapeCore.hpp b/src/tape/TapeCore.hpp index c8b816bca..5a21ddc7e 100644 --- a/src/tape/TapeCore.hpp +++ b/src/tape/TapeCore.hpp @@ -34,6 +34,7 @@ class TapeCore static void tape_entity_set_pose(const YAML::Node& yamlNode, PlaybackState& state); static void tape_entity_set_id(const YAML::Node& yamlNode, PlaybackState& state); static void tape_entity_set_intensity_texture(const YAML::Node& yamlNode, PlaybackState& state); + static void tape_entity_set_laser_retro(const YAML::Node& yamlNode, PlaybackState& state); static void tape_scene_set_time(const YAML::Node& yamlNode, PlaybackState& state); static void tape_graph_run(const YAML::Node& yamlNode, PlaybackState& state); static void tape_graph_destroy(const YAML::Node& yamlNode, PlaybackState& state); @@ -85,6 +86,7 @@ class TapeCore TAPE_CALL_MAPPING("rgl_entity_set_pose", TapeCore::tape_entity_set_pose), TAPE_CALL_MAPPING("rgl_entity_set_id", TapeCore::tape_entity_set_id), TAPE_CALL_MAPPING("rgl_entity_set_intensity_texture", TapeCore::tape_entity_set_intensity_texture), + TAPE_CALL_MAPPING("rgl_entity_set_laser_retro", TapeCore::tape_entity_set_laser_retro), TAPE_CALL_MAPPING("rgl_scene_set_time", TapeCore::tape_scene_set_time), TAPE_CALL_MAPPING("rgl_graph_run", TapeCore::tape_graph_run), TAPE_CALL_MAPPING("rgl_graph_destroy", TapeCore::tape_graph_destroy), diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 7131fcee5..6dbc7b331 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -41,6 +41,7 @@ set(RGL_TEST_FILES src/memory/arrayTypingTest.cpp src/scene/entityAPITest.cpp src/scene/entityIdTest.cpp + src/scene/entityLaserRetroTest.cpp src/scene/entityVelocityTest.cpp src/scene/meshAPITest.cpp src/scene/textureTest.cpp diff --git a/test/include/helpers/fieldGenerators.hpp b/test/include/helpers/fieldGenerators.hpp index 8f0040dd0..02530eabf 100644 --- a/test/include/helpers/fieldGenerators.hpp +++ b/test/include/helpers/fieldGenerators.hpp @@ -33,6 +33,9 @@ static std::function::type(int)> genCoord = [](int i) { static std::function::type(int)> genIntensity = [](int i) { return static_cast(i) / (static_cast(i + 1)); }; +static std::function::type(int)> genLaserRetro = [](int i) { + return static_cast(i) / (static_cast(i + 1)); +}; static std::function::type(int)> genAzimuth = [](int i) { return static_cast(i) / (static_cast(i + 1)); }; diff --git a/test/include/helpers/testPointCloud.hpp b/test/include/helpers/testPointCloud.hpp index e2e3e5b97..f897dce34 100644 --- a/test/include/helpers/testPointCloud.hpp +++ b/test/include/helpers/testPointCloud.hpp @@ -204,8 +204,8 @@ class TestPointCloud int fieldOffset = offsets.at(fieldIndex); bool allZeros = true; for (int i = 0; i < getPointCount(); ++i) { - if (std::memcmp(data.data() + i * getPointByteSize() + fieldOffset, std::vector(getFieldSize(field), 0).data(), - getFieldSize(field)) != 0) { + if (std::memcmp(data.data() + i * getPointByteSize() + fieldOffset, + std::vector(getFieldSize(field), 0).data(), getFieldSize(field)) != 0) { allZeros = false; break; } @@ -288,6 +288,7 @@ class TestPointCloud {RAY_IDX_U32, [&](std::size_t count) {setFieldValues(generateFieldValues(count, genRayIdx));}}, {ENTITY_ID_I32, [&](std::size_t count) {setFieldValues(generateFieldValues(count, genEntityId));}}, {INTENSITY_F32, [&](std::size_t count) {setFieldValues(generateFieldValues(count, genIntensity));}}, + {LASER_RETRO_F32, [&](std::size_t count) {setFieldValues(generateFieldValues(count, genLaserRetro));}}, {RING_ID_U16, [&](std::size_t count) {setFieldValues(generateFieldValues(count, genRingId));}}, {AZIMUTH_F32, [&](std::size_t count) {setFieldValues(generateFieldValues(count, genAzimuth));}}, {ELEVATION_F32, [&](std::size_t count) {setFieldValues(generateFieldValues(count, genElevation));}}, diff --git a/test/src/TapeTest.cpp b/test/src/TapeTest.cpp index 81b0abc35..ef960b833 100644 --- a/test/src/TapeTest.cpp +++ b/test/src/TapeTest.cpp @@ -196,6 +196,8 @@ TEST_F(TapeTest, RecordPlayAllCalls) EXPECT_RGL_SUCCESS(rgl_mesh_set_texture_coords(mesh, cubeUVs, 8)); EXPECT_RGL_SUCCESS(rgl_entity_set_intensity_texture(entity, texture)); + EXPECT_RGL_SUCCESS(rgl_entity_set_laser_retro(entity, 50.0f)); + EXPECT_RGL_SUCCESS(rgl_scene_set_time(nullptr, 1.5 * 1e9)); rgl_node_t useRays = nullptr; diff --git a/test/src/scene/entityLaserRetroTest.cpp b/test/src/scene/entityLaserRetroTest.cpp new file mode 100644 index 000000000..0210fcf2f --- /dev/null +++ b/test/src/scene/entityLaserRetroTest.cpp @@ -0,0 +1,70 @@ +#include +#include +#include +#include + +class EntityLaserRetroTest : public RGLTestWithParam +{}; + +INSTANTIATE_TEST_SUITE_P(EntityLaserRetroTest, EntityLaserRetroTest, testing::Range(-100.1f, 100.1f, 50.0f), + [](const auto& info) { + std::string valueStr = std::to_string(info.param); + // The test name suffix can only contain alphanumeric characters and underscores. + std::replace(valueStr.begin(), valueStr.end(), '-', 'm'); + std::replace(valueStr.begin(), valueStr.end(), '.', 'd'); + std::replace(valueStr.begin(), valueStr.end(), ',', 'd'); + return "retro_" + valueStr; + }); + +TEST_P(EntityLaserRetroTest, SmokeTest) +{ + // Create scene and set retro value + float inLaserRetro = GetParam(); + auto cube = spawnCubeOnScene(Mat3x4f::TRS({0, 0, 0})); + ASSERT_RGL_SUCCESS(rgl_entity_set_laser_retro(cube, inLaserRetro)); + + // Construct graph + std::vector rays = { + Mat3x4f::TRS({0, 0, 0}, {0, 0, 0}).toRGL(), // hit point + Mat3x4f::TRS({CUBE_HALF_EDGE * 3, 0, 0}, {0, 0, 0}).toRGL(), // non-hit point + }; + + std::vector outFields { IS_HIT_I32, LASER_RETRO_F32 }; + + rgl_node_t rayNode = nullptr, raytraceNode = nullptr, yieldNode = nullptr; + ASSERT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&rayNode, rays.data(), rays.size())); + ASSERT_RGL_SUCCESS(rgl_node_raytrace(&raytraceNode, nullptr)); + ASSERT_RGL_SUCCESS(rgl_node_points_yield(&yieldNode, outFields.data(), outFields.size())); + + ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(rayNode, raytraceNode)); + ASSERT_RGL_SUCCESS(rgl_graph_node_add_child(raytraceNode, yieldNode)); + + // First run - check if retro value has been assigned properly + { + ASSERT_RGL_SUCCESS(rgl_graph_run(raytraceNode)); + + auto outPointCloud = TestPointCloud::createFromNode(yieldNode, outFields); + + ASSERT_EQ(outPointCloud.getPointCount(), 2); + + ASSERT_EQ(outPointCloud.getFieldValue(0), 1); // hit point + ASSERT_EQ(outPointCloud.getFieldValue(0), inLaserRetro); + + ASSERT_EQ(outPointCloud.getFieldValue(1), 0); // non-hit point + ASSERT_EQ(outPointCloud.getFieldValue(1), 0.0f); // Retro should be zero + } + + // Second run - check if retro value has been updated properly + { + float newLaserRetro = inLaserRetro + 1.0f; + ASSERT_RGL_SUCCESS(rgl_entity_set_laser_retro(cube, newLaserRetro)); + ASSERT_RGL_SUCCESS(rgl_graph_run(raytraceNode)); + + auto outPointCloud = TestPointCloud::createFromNode(yieldNode, outFields); + ASSERT_EQ(outPointCloud.getFieldValue(0), 1); // hit point + ASSERT_EQ(outPointCloud.getFieldValue(0), newLaserRetro); + + ASSERT_EQ(outPointCloud.getFieldValue(1), 0); // non-hit point + ASSERT_EQ(outPointCloud.getFieldValue(1), 0.0f); // Retro should be zero + } +}