Feature/fault injection (#281)

PiotrMrozik · web-flow · commit c8ea5a1576ec · 2024-05-06T11:26:54.000+02:00
diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h
@@ -704,6 +704,15 @@ RGL_API rgl_status_t rgl_node_raytrace_configure_distortion(rgl_node_t node, boo
  */
 RGL_API rgl_status_t rgl_node_raytrace_configure_non_hits(rgl_node_t node, float nearDistance, float farDistance);
 
+/**
+ * Modifies RaytraceNode to apply mask for non-hit values.
+ * Masked rays will be non-hits and will have the default non-hit values no matter of raytracing result.
+ * @param node RaytraceNode to modify.
+ * @param rays_mask Pointer to the array of int32_t. 1 means point is hit, 0 means point is non-hit.
+ * @param rays_count Number of elements in the `points_mask` array.
+ */
+RGL_API rgl_status_t rgl_node_raytrace_configure_mask(rgl_node_t node, const int8_t* rays_mask, int32_t rays_count);
+
 /**
  * Creates or modifies FormatPointsNode.
  * The Node converts internal representation into a binary format defined by the `fields` array.
diff --git a/src/api/apiCore.cpp b/src/api/apiCore.cpp
@@ -919,6 +919,28 @@ void TapeCore::tape_node_raytrace_configure_non_hits(const YAML::Node& yamlNode,
 	rgl_node_raytrace_configure_non_hits(node, yamlNode[1].as<float>(), yamlNode[2].as<float>());
 }
 
+RGL_API rgl_status_t rgl_node_raytrace_configure_mask(rgl_node_t node, const int8_t* rays_mask, int32_t rays_count)
+{
+	auto status = rglSafeCall([&]() {
+		RGL_API_LOG("rgl_node_raytrace_configure_mask(node={}, rays_mask={}, rays_count={})", repr(node), repr(rays_mask, rays_count),
+		            rays_count);
+		CHECK_ARG(node != nullptr);
+		CHECK_ARG(rays_mask != nullptr);
+		CHECK_ARG(rays_count > 0);
+		RaytraceNode::Ptr raytraceNode = Node::validatePtr<RaytraceNode>(node);
+		raytraceNode->setNonHitsMask(rays_mask, rays_count);
+	});
+	TAPE_HOOK(node, TAPE_ARRAY(rays_mask, rays_count), rays_count);
+	return status;
+}
+
+void TapeCore::tape_node_raytrace_configure_mask(const YAML::Node& yamlNode, PlaybackState& state)
+{
+	auto nodeId = yamlNode[0].as<TapeAPIObjectID>();
+	rgl_node_t node = state.nodes.at(nodeId);
+	rgl_node_raytrace_configure_mask(node, state.getPtr<const int8_t>(yamlNode[1]), yamlNode[2].as<int32_t>());
+}
+
 RGL_API rgl_status_t rgl_node_points_format(rgl_node_t* node, const rgl_field_t* fields, int32_t field_count)
 {
 	auto status = rglSafeCall([&]() {
diff --git a/src/gpu/RaytraceRequestContext.hpp b/src/gpu/RaytraceRequestContext.hpp
@@ -42,6 +42,8 @@ struct RaytraceRequestContext
 	const float* rayTimeOffsetsMs;
 	size_t rayTimeOffsetsCount;
 
+	const int8_t* rayMask;
+
 	OptixTraversableHandle scene;
 	double sceneTime;
 	float sceneDeltaTime;
diff --git a/src/gpu/nodeKernels.cu b/src/gpu/nodeKernels.cu
@@ -223,7 +223,6 @@ __global__ void kFilterGroundPoints(size_t pointCount, const Vec3f sensor_up_vec
 	outNonGround[tid] = normalUpAngle > ground_angle_threshold;
 }
 
-
 void gpuFindCompaction(cudaStream_t stream, size_t pointCount, const int32_t* shouldCompact,
                        CompactionIndexType* hitCountInclusive, size_t* outHitCount)
 {
diff --git a/src/gpu/optixPrograms.cu b/src/gpu/optixPrograms.cu
@@ -49,6 +49,12 @@ extern "C" __global__ void __raygen__()
 	}
 
 	const int rayIdx = static_cast<int>(optixGetLaunchIndex().x);
+
+	if (ctx.rayMask != nullptr && ctx.rayMask[rayIdx] == 0) {
+		saveNonHitRayResult(ctx.farNonHitDistance);
+		return;
+	}
+
 	Mat3x4f ray = ctx.rays[rayIdx];
 	const Mat3x4f rayLocal =
 	    ctx.rayOriginToWorld.inverse() *
diff --git a/src/graph/NodesCore.hpp b/src/graph/NodesCore.hpp
@@ -128,6 +128,7 @@ struct RaytraceNode : IPointsNode
 	void setVelocity(const Vec3f& linearVelocity, const Vec3f& angularVelocity);
 	void enableRayDistortion(bool enabled) { doApplyDistortion = enabled; }
 	void setNonHitDistanceValues(float nearDistance, float farDistance);
+	void setNonHitsMask(const int8_t* maskRaw, size_t maskPointCount);
 
 private:
 	IRaysNode::Ptr raysNode;
@@ -140,6 +141,8 @@ struct RaytraceNode : IPointsNode
 	float nearNonHitDistance{std::numeric_limits<float>::infinity()};
 	float farNonHitDistance{std::numeric_limits<float>::infinity()};
 
+	DeviceAsyncArray<int8_t>::Ptr rayMask;
+
 	HostPinnedArray<RaytraceRequestContext>::Ptr requestCtxHst = HostPinnedArray<RaytraceRequestContext>::create();
 	DeviceAsyncArray<RaytraceRequestContext>::Ptr requestCtxDev = DeviceAsyncArray<RaytraceRequestContext>::create(arrayMgr);
 
diff --git a/src/graph/RaytraceNode.cpp b/src/graph/RaytraceNode.cpp
@@ -47,6 +47,11 @@ void RaytraceNode::validateImpl()
 		auto msg = fmt::format("requested for raytrace with velocity distortion, but RaytraceNode cannot get time offsets");
 		throw InvalidPipeline(msg);
 	}
+	if (rayMask != nullptr) {
+		if (rayMask->getCount() != raysNode->getRayCount()) {
+			throw InvalidPipeline("Mask size does not match the number of rays");
+		}
+	}
 }
 
 template<rgl_field_t field>
@@ -99,6 +104,7 @@ void RaytraceNode::enqueueExecImpl()
 	    .ringIdsCount = ringIds.has_value() ? (*ringIds)->getCount() : 0,
 	    .rayTimeOffsetsMs = timeOffsets.has_value() ? (*timeOffsets)->asSubclass<DeviceAsyncArray>()->getReadPtr() : nullptr,
 	    .rayTimeOffsetsCount = timeOffsets.has_value() ? (*timeOffsets)->getCount() : 0,
+	    .rayMask = (rayMask != nullptr) ? rayMask->getReadPtr() : nullptr,
 	    .scene = sceneAS,
 	    .sceneTime = Scene::instance().getTime().value_or(Time::zero()).asSeconds(),
 	    .sceneDeltaTime = static_cast<float>(Scene::instance().getDeltaTime().value_or(Time::zero()).asSeconds()),
@@ -184,3 +190,8 @@ void RaytraceNode::setNonHitDistanceValues(float nearDistance, float farDistance
 	nearNonHitDistance = nearDistance;
 	farNonHitDistance = farDistance;
 }
+void RaytraceNode::setNonHitsMask(const int8_t* maskRaw, size_t maskPointCount)
+{
+	rayMask = DeviceAsyncArray<int8_t>::create(arrayMgr);
+	rayMask->copyFromExternal(maskRaw, maskPointCount);
+}
diff --git a/src/tape/TapeCore.hpp b/src/tape/TapeCore.hpp
@@ -53,6 +53,7 @@ class TapeCore
 	static void tape_node_raytrace_configure_velocity(const YAML::Node& yamlNode, PlaybackState& state);
 	static void tape_node_raytrace_configure_distortion(const YAML::Node& yamlNode, PlaybackState& state);
 	static void tape_node_raytrace_configure_non_hits(const YAML::Node& yamlNode, PlaybackState& state);
+	static void tape_node_raytrace_configure_mask(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);
 	static void tape_node_points_compact(const YAML::Node& yamlNode, PlaybackState& state);
@@ -105,6 +106,7 @@ class TapeCore
 		    TAPE_CALL_MAPPING("rgl_node_raytrace_configure_velocity", TapeCore::tape_node_raytrace_configure_velocity),
 		    TAPE_CALL_MAPPING("rgl_node_raytrace_configure_distortion", TapeCore::tape_node_raytrace_configure_distortion),
 		    TAPE_CALL_MAPPING("rgl_node_raytrace_configure_non_hits", TapeCore::tape_node_raytrace_configure_non_hits),
+		    TAPE_CALL_MAPPING("rgl_node_raytrace_configure_mask", TapeCore::tape_node_raytrace_configure_mask),
 		    TAPE_CALL_MAPPING("rgl_node_points_format", TapeCore::tape_node_points_format),
 		    TAPE_CALL_MAPPING("rgl_node_points_yield", TapeCore::tape_node_points_yield),
 		    TAPE_CALL_MAPPING("rgl_node_points_compact", TapeCore::tape_node_points_compact),
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
@@ -24,6 +24,7 @@ set(RGL_TEST_FILES
     src/graph/nodes/GaussianNoiseAngularHitpointNodeTest.cpp
     src/graph/nodes/GaussianNoiseAngularRayNodeTest.cpp
     src/graph/nodes/GaussianNoiseDistanceNodeTest.cpp
+    src/graph/MaskRaysTest.cpp
     src/graph/nodes/RaytraceNodeTest.cpp
     src/graph/nodes/RadarPostprocessPointsNodeTest.cpp
     src/graph/nodes/RadarTrackObjectsNodeTest.cpp
diff --git a/test/src/graph/MaskRaysTest.cpp b/test/src/graph/MaskRaysTest.cpp
@@ -0,0 +1,98 @@
+#include "helpers/testPointCloud.hpp"
+#include "helpers/commonHelpers.hpp"
+#include "RGLFields.hpp"
+#include "helpers/sceneHelpers.hpp"
+#include "helpers/lidarHelpers.hpp"
+
+class MaskRaysTest : public RGLTest
+{
+
+protected:
+	const int maskCount = 100;
+	std::vector<int8_t> points_mask;
+	std::vector<rgl_field_t> fields = {XYZ_VEC3_F32, IS_HIT_I32, INTENSITY_F32, IS_GROUND_I32};
+
+	void initializeMask(int raysCount)
+	{
+		points_mask.resize(raysCount);
+		std::fill(points_mask.begin(), points_mask.end(), 1);
+		for (int i = 0; i < maskCount; i++) {
+			points_mask[i] = 0;
+		}
+	}
+};
+
+TEST_F(MaskRaysTest, invalid_argument_node) { EXPECT_RGL_INVALID_ARGUMENT(rgl_node_raytrace_configure_mask(nullptr, nullptr, 0)); }
+
+TEST_F(MaskRaysTest, invalid_argument_mask)
+{
+	rgl_node_t raytraceNode = nullptr;
+	EXPECT_RGL_SUCCESS(rgl_node_raytrace(&raytraceNode, nullptr));
+
+	EXPECT_RGL_INVALID_ARGUMENT(rgl_node_raytrace_configure_mask(raytraceNode, nullptr, 0));
+}
+
+TEST_F(MaskRaysTest, invalid_argument_count)
+{
+	rgl_node_t raytraceNode = nullptr;
+	EXPECT_RGL_SUCCESS(rgl_node_raytrace(&raytraceNode, nullptr));
+
+	EXPECT_RGL_INVALID_ARGUMENT(rgl_node_raytrace_configure_mask(raytraceNode, points_mask.data(), 0));
+}
+
+TEST_F(MaskRaysTest, valid_arguments)
+{
+	const int raysNumber = 100;
+	rgl_node_t raytraceNode = nullptr;
+	EXPECT_RGL_SUCCESS(rgl_node_raytrace(&raytraceNode, nullptr));
+
+	// Mask
+	initializeMask(raysNumber);
+
+	EXPECT_RGL_SUCCESS(rgl_node_raytrace_configure_mask(raytraceNode, points_mask.data(), raysNumber));
+}
+
+TEST_F(MaskRaysTest, use_case)
+{
+	// Scene
+	rgl_entity_t wall = makeEntity(makeCubeMesh());
+	rgl_mat3x4f wallPose = Mat3x4f::TRS(Vec3f(0,0,0), Vec3f(0, 0, 0), Vec3f(10,10,10)).toRGL();
+	EXPECT_RGL_SUCCESS(rgl_entity_set_pose(wall, &wallPose));
+
+	// Rays
+	std::vector<rgl_mat3x4f> rays = makeLidar3dRays(360, 180, 0.36, 0.18);
+
+	const int raysNumber = rays.size();
+	// Mask
+	initializeMask(raysNumber);
+
+	// Graph
+	rgl_node_t useRaysNode = nullptr;
+	rgl_node_t raytraceNode = nullptr;
+	rgl_node_t compactNode = nullptr;
+	rgl_node_t yieldNode = nullptr;
+
+	// Prepare graph without filtering
+	EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&useRaysNode, rays.data(), rays.size()));
+	EXPECT_RGL_SUCCESS(rgl_node_raytrace(&raytraceNode, nullptr));
+	EXPECT_RGL_SUCCESS(rgl_node_points_compact_by_field(&compactNode, IS_HIT_I32));
+	EXPECT_RGL_SUCCESS(rgl_node_points_yield(&yieldNode, fields.data(), fields.size()));
+
+	EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(useRaysNode, raytraceNode));
+	EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(raytraceNode, compactNode));
+	EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(compactNode, yieldNode));
+
+	EXPECT_RGL_SUCCESS(rgl_graph_run(raytraceNode));
+
+	TestPointCloud outputPointCloud = TestPointCloud::createFromNode(yieldNode, fields);
+	auto fullCloudSize = outputPointCloud.getPointCount();
+
+	EXPECT_RGL_SUCCESS(rgl_node_raytrace_configure_mask(raytraceNode, points_mask.data(), raysNumber));
+
+	EXPECT_RGL_SUCCESS(rgl_graph_run(raytraceNode));
+	TestPointCloud outputPointCloudMasked = TestPointCloud::createFromNode(yieldNode, fields);
+	auto maskedCloudSize = outputPointCloudMasked.getPointCount();
+
+	EXPECT_EQ(fullCloudSize - maskCount, maskedCloudSize);
+
+}

Original file line number	Diff line number	Diff line change
`@@ -223,7 +223,6 @@ __global__ void kFilterGroundPoints(size_t pointCount, const Vec3f sensor_up_vec`
`223`	`223`	`outNonGround[tid] = normalUpAngle > ground_angle_threshold;`
`224`	`224`	`}`
`225`	`225`
`226`		`-`
`227`	`226`	`void gpuFindCompaction(cudaStream_t stream, size_t pointCount, const int32_t* shouldCompact,`
`228`	`227`	`CompactionIndexType* hitCountInclusive, size_t* outHitCount)`
`229`	`228`	`{`