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

@@ -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.

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<rgl_field_t>& 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<IS_HIT_I32>::size;
 		case IS_GROUND_I32: return Field<IS_GROUND_I32>::size;
 		case INTENSITY_F32: return Field<INTENSITY_F32>::size;
+		case LASER_RETRO_F32: return Field<LASER_RETRO_F32>::size;
 		case RING_ID_U16: return Field<RING_ID_U16>::size;
 		case AZIMUTH_F32: return Field<AZIMUTH_F32>::size;
 		case ELEVATION_F32: return Field<ELEVATION_F32>::size;
@@ -202,6 +206,7 @@ inline std::shared_ptr<IAnyArray> createArray(rgl_field_t type, Args&&... args)
 		case RAY_IDX_U32: return Subclass<Field<RAY_IDX_U32>::type>::create(std::forward<Args>(args)...);
 		case ENTITY_ID_I32: return Subclass<Field<ENTITY_ID_I32>::type>::create(std::forward<Args>(args)...);
 		case INTENSITY_F32: return Subclass<Field<INTENSITY_F32>::type>::create(std::forward<Args>(args)...);
+		case LASER_RETRO_F32: return Subclass<Field<LASER_RETRO_F32>::type>::create(std::forward<Args>(args)...);
 		case RING_ID_U16: return Subclass<Field<RING_ID_U16>::type>::create(std::forward<Args>(args)...);
 		case AZIMUTH_F32: return Subclass<Field<AZIMUTH_F32>::type>::create(std::forward<Args>(args)...);
 		case ELEVATION_F32: return Subclass<Field<ELEVATION_F32>::type>::create(std::forward<Args>(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<uint8_t> 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<std::string> 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"};

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<TapeAPIObjectID>()));
 }
 
+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<TapeAPIObjectID>()),
+	                           yamlNode[1].as<Field<LASER_RETRO_F32>::type>());
+}
+
 rgl_status_t rgl_entity_is_alive(rgl_entity_t entity, bool* out_alive)
 {
 	auto status = rglSafeCall([&]() {

src/gpu/RaytraceRequestContext.hpp

@@ -52,6 +52,7 @@ struct RaytraceRequestContext
 	Field<RING_ID_U16>::type* ringIdx;
 	Field<DISTANCE_F32>::type* distance;
 	Field<INTENSITY_F32>::type* intensity;
+	Field<LASER_RETRO_F32>::type* laserRetro;
 	Field<TIME_STAMP_F64>::type* timestamp;
 	Field<ENTITY_ID_I32>::type* entityId;
 	Field<ABSOLUTE_VELOCITY_VEC3_F32>::type* pointAbsVelocity;

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

src/gpu/optixPrograms.cu

@@ -58,7 +58,7 @@ __forceinline__ __device__ Vec3f decodePayloadVec3f(const Vec3fPayload& src)
 template<bool isFinite>
 __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<false>(xyz, nonHitDistance, 0, RGL_ENTITY_INVALID_ID, Vec3f{NAN}, Vec3f{NAN}, 0.001f, Vec3f{NAN}, NAN);
+	saveRayResult<false>(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<true>(hitWorld, distance, intensity, objectID, absPointVelocity, relPointVelocity, radialSpeed, wNormal,
-	                    incidentAngle);
+	                    incidentAngle, laserRetro);
 }
 
 extern "C" __global__ void __miss__() { saveNonHitRayResult(ctx.farNonHitDistance); }

src/graph/RaytraceNode.cpp

@@ -103,6 +103,7 @@ void RaytraceNode::enqueueExecImpl()
 	    .ringIdx = getPtrTo<RING_ID_U16>(),
 	    .distance = getPtrTo<DISTANCE_F32>(),
 	    .intensity = getPtrTo<INTENSITY_F32>(),
+	    .laserRetro = getPtrTo<LASER_RETRO_F32>(),
 	    .timestamp = getPtrTo<TIME_STAMP_F64>(),
 	    .entityId = getPtrTo<ENTITY_ID_I32>(),
 	    .pointAbsVelocity = getPtrTo<ABSOLUTE_VELOCITY_VEC3_F32>(),

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> texture)
 {
 	intensityTexture = texture;

src/scene/Entity.hpp

@@ -56,6 +56,11 @@ struct Entity : APIObject<Entity>
 	 */
 	void setIntensityTexture(std::shared_ptr<Texture> 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<Entity>
 	TransformWithTime formerTransformInfo{Mat3x4f::identity(), std::nullopt};
 
 	Field<ENTITY_ID_I32>::type id{RGL_DEFAULT_ENTITY_ID};
+	float laserRetro{};
 
 	std::shared_ptr<Mesh> mesh{};
 	std::shared_ptr<Texture> intensityTexture{};

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(),

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),

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

test/include/helpers/fieldGenerators.hpp

@@ -33,6 +33,9 @@ static std::function<Field<XYZ_VEC3_F32>::type(int)> genCoord = [](int i) {
 static std::function<Field<INTENSITY_F32>::type(int)> genIntensity = [](int i) {
 	return static_cast<float>(i) / (static_cast<float>(i + 1));
 };
+static std::function<Field<LASER_RETRO_F32>::type(int)> genLaserRetro = [](int i) {
+	return static_cast<float>(i) / (static_cast<float>(i + 1));
+};
 static std::function<Field<AZIMUTH_F32>::type(int)> genAzimuth = [](int i) {
 	return static_cast<float>(i) / (static_cast<float>(i + 1));
 };

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<char>(getFieldSize(field), 0).data(),
-				                getFieldSize(field)) != 0) {
+				if (std::memcmp(data.data() + i * getPointByteSize() + fieldOffset,
+				                std::vector<char>(getFieldSize(field), 0).data(), getFieldSize(field)) != 0) {
 					allZeros = false;
 					break;
 				}
@@ -288,6 +288,7 @@ class TestPointCloud
 		{RAY_IDX_U32, [&](std::size_t count) {setFieldValues<RAY_IDX_U32>(generateFieldValues(count, genRayIdx));}},
 		{ENTITY_ID_I32, [&](std::size_t count) {setFieldValues<ENTITY_ID_I32>(generateFieldValues(count, genEntityId));}},
 		{INTENSITY_F32, [&](std::size_t count) {setFieldValues<INTENSITY_F32>(generateFieldValues(count, genIntensity));}},
+		{LASER_RETRO_F32, [&](std::size_t count) {setFieldValues<LASER_RETRO_F32>(generateFieldValues(count, genLaserRetro));}},
 		{RING_ID_U16, [&](std::size_t count) {setFieldValues<RING_ID_U16>(generateFieldValues(count, genRingId));}},
 		{AZIMUTH_F32, [&](std::size_t count) {setFieldValues<AZIMUTH_F32>(generateFieldValues(count, genAzimuth));}},
 		{ELEVATION_F32, [&](std::size_t count) {setFieldValues<ELEVATION_F32>(generateFieldValues(count, genElevation));}},

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;

test/src/scene/entityLaserRetroTest.cpp

@@ -0,0 +1,70 @@
+#include <helpers/lidarHelpers.hpp>
+#include <helpers/sceneHelpers.hpp>
+#include <helpers/commonHelpers.hpp>
+#include <helpers/testPointCloud.hpp>
+
+class EntityLaserRetroTest : public RGLTestWithParam<float>
+{};
+
+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<rgl_mat3x4f> 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<rgl_field_t> 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<IS_HIT_I32>(0), 1); // hit point
+		ASSERT_EQ(outPointCloud.getFieldValue<LASER_RETRO_F32>(0), inLaserRetro);
+
+		ASSERT_EQ(outPointCloud.getFieldValue<IS_HIT_I32>(1), 0); // non-hit point
+		ASSERT_EQ(outPointCloud.getFieldValue<LASER_RETRO_F32>(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<IS_HIT_I32>(0), 1); // hit point
+		ASSERT_EQ(outPointCloud.getFieldValue<LASER_RETRO_F32>(0), newLaserRetro);
+
+		ASSERT_EQ(outPointCloud.getFieldValue<IS_HIT_I32>(1), 0); // non-hit point
+		ASSERT_EQ(outPointCloud.getFieldValue<LASER_RETRO_F32>(1), 0.0f); // Retro should be zero
+	}
+}