Feature/reflectivity (#341)

* Introduce new field * First working version of reflectivity calculation with hardcoded alpha. * Introduce alpha of reflectivity. * Cleanup * Fix tape call * Introduce separate test. * PR fixes v1 * minor formating fixes * Update test/src/scene/reflectivityTest.cpp Co-authored-by: Piotr Rybicki <[email protected]> * fix naming conflict --------- Co-authored-by: Piotr Rybicki <[email protected]>
RobotecAI · Dec 16, 2024 · d9dcead · d9dcead
1 parent 756eb12
commit d9dcead
Show file tree

Hide file tree

Showing 14 changed files with 175 additions and 7 deletions.
diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h
@@ -441,6 +441,13 @@ typedef enum : int32_t
 	 */
 	RGL_FIELD_LASER_RETRO_F32,
 
+	/**
+	*  Reflectivity of the hit point.
+	 * Reflectivity is a scalar value that describes how much light is reflected by the hit point.
+	 * Reflectivity is a property of the material of the hit point.
+	 */
+	RGL_FIELD_REFLECTIVITY_F32,
+
 	// Dummy fields
 	RGL_FIELD_PADDING_8 = 1024,
 	RGL_FIELD_PADDING_16,
@@ -892,6 +899,15 @@ RGL_API rgl_status_t rgl_node_raytrace_configure_beam_divergence(rgl_node_t node
  */
 RGL_API rgl_status_t rgl_node_raytrace_configure_default_intensity(rgl_node_t node, float default_intensity);
 
+/**
+ * Modifies RaytraceNode to set reflectivity alpha.
+ * Reflectivity alpha is used to calculate reflectivity of the hit point. This value is constant for every hit point.
+ * Default reflectivity alpha is set to 0.1f.
+ * @param node RaytraceNode to modify.
+ * @param reflectivity_alpha Reflectivity alpha to set.
+ */
+RGL_API rgl_status_t rgl_node_raytrace_configure_reflectivity_alpha(rgl_node_t node, float reflectivity_alpha);
+
 /**
  * Modifies RaytraceNode to set return mode.
  * Point return types (RGL_FIELD_RETURN_TYPE_U8) will be set to corresponding rgl_return_type_t values, e.g. return mode

diff --git a/src/RGLFields.hpp b/src/RGLFields.hpp
@@ -41,6 +41,7 @@ typedef unsigned char TextureTexelFormat;
 #define ENTITY_ID_I32 RGL_FIELD_ENTITY_ID_I32
 #define INTENSITY_F32 RGL_FIELD_INTENSITY_F32
 #define INTENSITY_U8 RGL_FIELD_INTENSITY_U8
+#define REFLECTIVITY_F32 RGL_FIELD_REFLECTIVITY_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
@@ -73,6 +74,7 @@ inline const std::set<rgl_field_t>& getAllRealFields()
 	    ENTITY_ID_I32,
 	    INTENSITY_F32,
 	    INTENSITY_U8,
+	    REFLECTIVITY_F32,
 	    LASER_RETRO_F32,
 	    RING_ID_U16,
 	    AZIMUTH_F32,
@@ -122,6 +124,7 @@ FIELD(RAY_IDX_U32, uint32_t); // PCL uses uint32_t
 FIELD(ENTITY_ID_I32, int32_t);
 FIELD(INTENSITY_F32, float);
 FIELD(INTENSITY_U8, uint8_t);
+FIELD(REFLECTIVITY_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
@@ -156,6 +159,7 @@ inline std::size_t getFieldSize(rgl_field_t type)
 		case IS_GROUND_I32: return Field<IS_GROUND_I32>::size;
 		case INTENSITY_F32: return Field<INTENSITY_F32>::size;
 		case INTENSITY_U8: return Field<INTENSITY_U8>::size;
+		case REFLECTIVITY_F32: return Field<REFLECTIVITY_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;
@@ -215,6 +219,7 @@ inline std::shared_ptr<IAnyArray> createArray(rgl_field_t type, 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 INTENSITY_U8: return Subclass<Field<INTENSITY_U8>::type>::create(std::forward<Args>(args)...);
+		case REFLECTIVITY_F32: return Subclass<Field<REFLECTIVITY_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)...);
@@ -251,6 +256,7 @@ inline std::string toString(rgl_field_t type)
 		case ENTITY_ID_I32: return "ENTITY_ID_I32";
 		case INTENSITY_F32: return "INTENSITY_F32";
 		case INTENSITY_U8: return "INTENSITY_U8";
+		case REFLECTIVITY_F32: return "REFLECTIVITY_F32";
 		case LASER_RETRO_F32: return "LASER_RETRO_F32";
 		case RING_ID_U16: return "RING_ID_U16";
 		case AZIMUTH_F32: return "AZIMUTH_F32";
@@ -291,6 +297,7 @@ inline std::vector<uint8_t> toRos2Fields(rgl_field_t type)
 		case ENTITY_ID_I32: return {sensor_msgs::msg::PointField::INT32};
 		case INTENSITY_F32: return {sensor_msgs::msg::PointField::FLOAT32};
 		case INTENSITY_U8: return {sensor_msgs::msg::PointField::UINT8};
+		case REFLECTIVITY_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};
@@ -342,6 +349,7 @@ inline std::vector<std::string> toRos2Names(rgl_field_t type)
 		case INTENSITY_F32:
 		case INTENSITY_U8:
 			return {"intensity"};
+		case REFLECTIVITY_F32: return {"reflectivity"};
 		case LASER_RETRO_F32: return {"laser_retro"};
 		case RING_ID_U16: return {"channel"};
 		case AZIMUTH_F32: return {"azimuth"};

diff --git a/src/api/apiCore.cpp b/src/api/apiCore.cpp
@@ -1066,6 +1066,27 @@ void TapeCore::tape_node_raytrace_configure_default_intensity(const YAML::Node&
 	rgl_node_raytrace_configure_default_intensity(node, yamlNode[1].as<float>());
 }
 
+RGL_API rgl_status_t rgl_node_raytrace_configure_reflectivity_alpha(rgl_node_t node, float reflectivity_alpha)
+{
+	auto status = rglSafeCall([&]() {
+		RGL_API_LOG("rgl_node_raytrace_configure_reflectivity_alpha(node={}, default_intensity={})", repr(node),
+					reflectivity_alpha);
+		CHECK_ARG(node != nullptr);
+		CHECK_ARG(reflectivity_alpha >= 0.0f);
+		RaytraceNode::Ptr raytraceNode = Node::validatePtr<RaytraceNode>(node);
+		raytraceNode->setReflectivityAlpha(reflectivity_alpha);
+	});
+	TAPE_HOOK(node, reflectivity_alpha);
+	return status;
+}
+
+void TapeCore::tape_node_raytrace_configure_reflectivity_alpha(const YAML::Node& yamlNode, PlaybackState& state)
+{
+	auto nodeId = yamlNode[0].as<TapeAPIObjectID>();
+	rgl_node_t node = state.nodes.at(nodeId);
+	rgl_node_raytrace_configure_reflectivity_alpha(node, yamlNode[1].as<float>());
+}
+
 RGL_API rgl_status_t rgl_node_raytrace_configure_return_mode(rgl_node_t node, rgl_return_mode_t return_mode)
 {
 	auto status = rglSafeCall([&]() {

diff --git a/src/gpu/RaytraceRequestContext.hpp b/src/gpu/RaytraceRequestContext.hpp
@@ -29,6 +29,7 @@ struct RaytraceRequestContext
 	float farNonHitDistance;
 
 	float defaultIntensity;
+	float reflectivityAlpha;
 
 	const Mat3x4f* raysWorld;
 	size_t rayCount;
@@ -58,6 +59,7 @@ struct RaytraceRequestContext
 	Field<DISTANCE_F32>::type* distance;
 	Field<INTENSITY_F32>::type* intensityF32;
 	Field<INTENSITY_U8>::type* intensityU8;
+	Field<REFLECTIVITY_F32>::type* reflectivityF32;
 	Field<LASER_RETRO_F32>::type* laserRetro;
 	Field<TIME_STAMP_F64>::type* timestampF64;
 	Field<TIME_STAMP_U32>::type* timestampU32;

diff --git a/src/gpu/nodeKernels.cu b/src/gpu/nodeKernels.cu
@@ -127,6 +127,10 @@ __device__ void saveReturnAsHit(const RaytraceRequestContext* ctx, int beamIdx,
 		                                       static_cast<uint8_t>(std::round(ctx->mrSamples.intensity[sampleIdx])) :
 		                                       UINT8_MAX;
 	}
+	if (ctx->reflectivityF32 != nullptr) {
+		const auto distance2 = ctx->mrSamples.distance[sampleIdx] * ctx->mrSamples.distance[sampleIdx];
+		ctx->reflectivityF32[returnPointIdx] = ctx->reflectivityAlpha * ctx->mrSamples.intensity[sampleIdx] * distance2;
+	}
 	if (ctx->entityId != nullptr) {
 		ctx->entityId[returnPointIdx] = ctx->mrSamples.entityId[sampleIdx];
 	}
@@ -183,6 +187,9 @@ __device__ void saveReturnAsNonHit(const RaytraceRequestContext* ctx, int firstS
 	if (ctx->intensityU8 != nullptr) {
 		ctx->intensityU8[returnPointIdx] = 0;
 	}
+	if (ctx->reflectivityF32 != nullptr) {
+		ctx->reflectivityF32[returnPointIdx] = 0;
+	}
 	if (ctx->entityId != nullptr) {
 		ctx->entityId[returnPointIdx] = RGL_ENTITY_INVALID_ID;
 	}

diff --git a/src/graph/NodesCore.hpp b/src/graph/NodesCore.hpp
@@ -139,6 +139,7 @@ struct RaytraceNode : IPointsNode
 	void setNonHitDistanceValues(float nearDistance, float farDistance);
 	void setNonHitsMask(const int8_t* maskRaw, size_t maskPointCount);
 	void setDefaultIntensity(float intensity) { defaultIntensity = intensity; }
+	void setReflectivityAlpha(float reflectivity_alpha) { reflectivityAlpha = reflectivity_alpha; }
 	void setReturnMode(rgl_return_mode_t mode)
 	{
 		if (mode == RGL_RETURN_UNKNOWN) {
@@ -244,6 +245,7 @@ struct RaytraceNode : IPointsNode
 	float nearNonHitDistance{std::numeric_limits<float>::infinity()};
 	float farNonHitDistance{std::numeric_limits<float>::infinity()};
 	float defaultIntensity = 0.0f;
+	float reflectivityAlpha = 0.1f;
 
 	MultiReturnSamples mrSampleData = MultiReturnSamples{arrayMgr};
 	rgl_return_mode_t returnMode = RGL_RETURN_FIRST;

diff --git a/src/graph/RaytraceNode.cpp b/src/graph/RaytraceNode.cpp
@@ -98,6 +98,7 @@ void RaytraceNode::enqueueExecImpl()
 	    .nearNonHitDistance = nearNonHitDistance,
 	    .farNonHitDistance = farNonHitDistance,
 	    .defaultIntensity = defaultIntensity,
+	    .reflectivityAlpha = reflectivityAlpha,
 	    .raysWorld = raysPtr,
 	    .rayCount = raysNode->getRayCount(),
 	    .rayOriginToWorld = raysNode->getCumulativeRayTransfrom(),
@@ -119,6 +120,7 @@ void RaytraceNode::enqueueExecImpl()
 	    .distance = getPtrTo<DISTANCE_F32>(),
 	    .intensityF32 = getPtrTo<INTENSITY_F32>(),
 	    .intensityU8 = getPtrTo<INTENSITY_U8>(),
+	    .reflectivityF32 = getPtrTo<REFLECTIVITY_F32>(),
 	    .laserRetro = getPtrTo<LASER_RETRO_F32>(),
 	    .timestampF64 = getPtrTo<TIME_STAMP_F64>(),
 	    .timestampU32 = getPtrTo<TIME_STAMP_U32>(),

diff --git a/src/tape/TapeCore.hpp b/src/tape/TapeCore.hpp
@@ -60,6 +60,7 @@ class TapeCore
 	static void tape_node_raytrace_configure_mask(const YAML::Node& yamlNode, PlaybackState& state);
 	static void tape_node_raytrace_configure_beam_divergence(const YAML::Node& yamlNode, PlaybackState& state);
 	static void tape_node_raytrace_configure_default_intensity(const YAML::Node& yamlNode, PlaybackState& state);
+	static void tape_node_raytrace_configure_reflectivity_alpha(const YAML::Node& yamlNode, PlaybackState& state);
 	static void tape_node_raytrace_configure_return_mode(const YAML::Node& yamlNode, PlaybackState& state);
 	static void tape_node_points_format(const YAML::Node& yamlNode, PlaybackState& state);
 	static void tape_node_points_yield(const YAML::Node& yamlNode, PlaybackState& state);
@@ -122,6 +123,8 @@ class TapeCore
 		                      TapeCore::tape_node_raytrace_configure_beam_divergence),
 		    TAPE_CALL_MAPPING("rgl_node_raytrace_configure_default_intensity",
 		                      TapeCore::tape_node_raytrace_configure_default_intensity),
+			TAPE_CALL_MAPPING("rgl_node_raytrace_configure_reflectivity_alpha",
+				      TapeCore::tape_node_raytrace_configure_reflectivity_alpha),
 		    TAPE_CALL_MAPPING("rgl_node_raytrace_configure_return_mode",
 		                      TapeCore::tape_node_raytrace_configure_return_mode),
 		    TAPE_CALL_MAPPING("rgl_node_points_format", TapeCore::tape_node_points_format),

diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
@@ -48,6 +48,7 @@ set(RGL_TEST_FILES
     src/scene/entityVelocityTest.cpp
     src/scene/meshAPITest.cpp
     src/scene/textureTest.cpp
+    src/scene/reflectivityTest.cpp
     src/synchronization/graphAndCopyStream.cpp
     src/synchronization/graphThreadSynchronization.cpp
     src/synchronization/testKernel.cu

diff --git a/test/include/helpers/fieldGenerators.hpp b/test/include/helpers/fieldGenerators.hpp
@@ -36,6 +36,9 @@ static std::function<Field<INTENSITY_F32>::type(int)> genIntensityF32 = [](int i
 static std::function<Field<INTENSITY_U8>::type(int)> genIntensityU8 = [](int i) {
 	return i % std::numeric_limits<Field<INTENSITY_U8>::type>::max();
 };
+static std::function<Field<REFLECTIVITY_F32>::type(int)> genReflectivityF32 = [](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));
 };

diff --git a/test/include/helpers/testPointCloud.hpp b/test/include/helpers/testPointCloud.hpp
@@ -289,6 +289,7 @@ class TestPointCloud
 		{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, genIntensityF32));}},
 		{INTENSITY_U8, [&](std::size_t count) {setFieldValues<INTENSITY_U8>(generateFieldValues(count, genIntensityU8));}},
+		{REFLECTIVITY_F32, [&](std::size_t count) {setFieldValues<REFLECTIVITY_F32>(generateFieldValues(count, genReflectivityF32));}},
 		{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));}},

diff --git a/test/src/TapeTest.cpp b/test/src/TapeTest.cpp
@@ -252,6 +252,9 @@ TEST_F(TapeTest, RecordPlayAllCalls)
 	float defaultIntensity = 1.1f;
 	EXPECT_RGL_SUCCESS(rgl_node_raytrace_configure_default_intensity(raytrace, defaultIntensity));
 
+	float reflectivityAlpha = 0.1;
+	EXPECT_RGL_SUCCESS(rgl_node_raytrace_configure_reflectivity_alpha(raytrace, reflectivityAlpha));
+
 	rgl_return_mode_t returnMode = RGL_RETURN_FIRST;
 	EXPECT_RGL_SUCCESS(rgl_node_raytrace_configure_return_mode(raytrace, returnMode));
 

diff --git a/test/src/helpers/pointsTest.cpp b/test/src/helpers/pointsTest.cpp
@@ -5,18 +5,20 @@
 class PointCloudTest : public ::testing::TestWithParam<int>
 {
 protected:
-	std::vector<rgl_field_t> allNotDummyFields = {XYZ_VEC3_F32, IS_HIT_I32,  RAY_IDX_U32,  ENTITY_ID_I32,  INTENSITY_F32,
-	                                              RING_ID_U16,  AZIMUTH_F32, DISTANCE_F32, RETURN_TYPE_U8, TIME_STAMP_F64};
+	std::vector<rgl_field_t> allNotDummyFields = {XYZ_VEC3_F32,  IS_HIT_I32,       RAY_IDX_U32,   ENTITY_ID_I32,
+	                                              INTENSITY_F32, REFLECTIVITY_F32, RING_ID_U16,   AZIMUTH_F32,
+	                                              DISTANCE_F32,  RETURN_TYPE_U8,   TIME_STAMP_F64};
 
-	std::vector<rgl_field_t> fieldsWithPaddings = {PADDING_32,   XYZ_VEC3_F32,   PADDING_16,     IS_HIT_I32,  PADDING_8,
-	                                               RAY_IDX_U32,  ENTITY_ID_I32,  INTENSITY_F32,  RING_ID_U16, AZIMUTH_F32,
-	                                               DISTANCE_F32, RETURN_TYPE_U8, TIME_STAMP_F64, PADDING_16,  PADDING_32};
+	std::vector<rgl_field_t> fieldsWithPaddings = {
+	    PADDING_32,       XYZ_VEC3_F32, PADDING_16,  IS_HIT_I32,   PADDING_8,      RAY_IDX_U32,    ENTITY_ID_I32, INTENSITY_F32,
+	    REFLECTIVITY_F32, RING_ID_U16,  AZIMUTH_F32, DISTANCE_F32, RETURN_TYPE_U8, TIME_STAMP_F64, PADDING_16,    PADDING_32};
 
 	std::vector<Field<XYZ_VEC3_F32>::type> pointCoord;
 	std::vector<Field<IS_HIT_I32>::type> isHit;
 	std::vector<Field<RAY_IDX_U32>::type> rayIdx;
 	std::vector<Field<ENTITY_ID_I32>::type> entityId;
 	std::vector<Field<INTENSITY_F32>::type> intensity;
+	std::vector<Field<REFLECTIVITY_F32>::type> reflectivity;
 	std::vector<Field<RING_ID_U16>::type> ringId;
 	std::vector<Field<AZIMUTH_F32>::type> azimuth;
 	std::vector<Field<DISTANCE_F32>::type> distance;
@@ -31,6 +33,7 @@ class PointCloudTest : public ::testing::TestWithParam<int>
 		Field<RAY_IDX_U32>::type rayIdx;
 		Field<ENTITY_ID_I32>::type entityId;
 		Field<INTENSITY_F32>::type intensity;
+		Field<REFLECTIVITY_F32>::type reflectivity;
 		Field<RING_ID_U16>::type ringId;
 		Field<AZIMUTH_F32>::type azimuth;
 		Field<DISTANCE_F32>::type distance;
@@ -45,8 +48,8 @@ class PointCloudTest : public ::testing::TestWithParam<int>
 		points.reserve(pointsCount);
 		for (int i = 0; i < pointsCount; i++) {
 			points.emplace_back(TestPointStruct{genCoord(i), genHalfHit(i), genRayIdx(i), genEntityId(i), genIntensityF32(i),
-			                                    genRingId(i), genAzimuth(i), genDistance(i), genReturnType(i),
-			                                    genTimeStampF64(i)});
+			                                    genReflectivityF32(i), genRingId(i), genAzimuth(i), genDistance(i),
+			                                    genReturnType(i), genTimeStampF64(i)});
 		}
 		return points;
 	}
@@ -58,6 +61,7 @@ class PointCloudTest : public ::testing::TestWithParam<int>
 		rayIdx = generateFieldValues(pointsCount, genRayIdx);
 		entityId = generateFieldValues(pointsCount, genEntityId);
 		intensity = generateFieldValues(pointsCount, genIntensityF32);
+		reflectivity = generateFieldValues(pointsCount, genReflectivityF32);
 		ringId = generateFieldValues(pointsCount, genRingId);
 		azimuth = generateFieldValues(pointsCount, genAzimuth);
 		distance = generateFieldValues(pointsCount, genDistance);
@@ -72,6 +76,7 @@ class PointCloudTest : public ::testing::TestWithParam<int>
 		pointCloud.setFieldValues<RAY_IDX_U32>(rayIdx);
 		pointCloud.setFieldValues<ENTITY_ID_I32>(entityId);
 		pointCloud.setFieldValues<INTENSITY_F32>(intensity);
+		pointCloud.setFieldValues<REFLECTIVITY_F32>(reflectivity);
 		pointCloud.setFieldValues<RING_ID_U16>(ringId);
 		pointCloud.setFieldValues<AZIMUTH_F32>(azimuth);
 		pointCloud.setFieldValues<DISTANCE_F32>(distance);
@@ -92,6 +97,7 @@ class PointCloudTest : public ::testing::TestWithParam<int>
 		EXPECT_EQ(pointCloud.getFieldValues<RAY_IDX_U32>(), rayIdx);
 		EXPECT_EQ(pointCloud.getFieldValues<ENTITY_ID_I32>(), entityId);
 		EXPECT_EQ(pointCloud.getFieldValues<INTENSITY_F32>(), intensity);
+		EXPECT_EQ(pointCloud.getFieldValues<REFLECTIVITY_F32>(), reflectivity);
 		EXPECT_EQ(pointCloud.getFieldValues<RING_ID_U16>(), ringId);
 		EXPECT_EQ(pointCloud.getFieldValues<AZIMUTH_F32>(), azimuth);
 		EXPECT_EQ(pointCloud.getFieldValues<DISTANCE_F32>(), distance);