Update MR beam model - distinct vertical/horizontal divergence (#297)

nebraszka · msz-rai · commit d7094d13e988 · 2024-07-13T17:16:03.000+02:00
diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h
@@ -736,9 +736,10 @@ RGL_API rgl_status_t rgl_node_raytrace_configure_mask(rgl_node_t node, const int
  * Setting beam divergence > 0.0f is required to use query for multi-return results.
  * Setting beam divergence == 0.0f disables multi-return.
  * @param node RaytraceNode to modify.
- * @param beamDivergence Beam divergence in radians.
+ * @param horizontal_beam_divergence Horizontal beam divergence in radians.
+ * @param vertical_beam_divergence Vertical beam divergence in radians.
  */
-RGL_API rgl_status_t rgl_node_raytrace_configure_beam_divergence(rgl_node_t node, float beam_divergence);
+RGL_API rgl_status_t rgl_node_raytrace_configure_beam_divergence(rgl_node_t node, float horizontal_beam_divergence, float vertical_beam_divergence);
 
 /**
  * Creates or modifies FormatPointsNode.
diff --git a/src/api/apiCore.cpp b/src/api/apiCore.cpp
@@ -958,24 +958,27 @@ void TapeCore::tape_node_raytrace_configure_mask(const YAML::Node& yamlNode, Pla
 	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_raytrace_configure_beam_divergence(rgl_node_t node, float beam_divergence)
+RGL_API rgl_status_t rgl_node_raytrace_configure_beam_divergence(rgl_node_t node, float horizontal_beam_divergence,
+                                                                 float vertical_beam_divergence)
 {
 	auto status = rglSafeCall([&]() {
-		RGL_API_LOG("rgl_node_raytrace_configure_beam_divergence(node={}, divergence={})", repr(node), beam_divergence);
+		RGL_API_LOG("rgl_node_raytrace_configure_beam_divergence(node={}, horizontal_divergence={}, vertical_divergence={})", repr(node),
+		            horizontal_beam_divergence, vertical_beam_divergence);
 		CHECK_ARG(node != nullptr);
-		CHECK_ARG(beam_divergence >= 0.0f);
+		CHECK_ARG((horizontal_beam_divergence > 0.0f && vertical_beam_divergence > 0.0f) ||
+		          (horizontal_beam_divergence == 0.0f && vertical_beam_divergence == 0.0f));
 		RaytraceNode::Ptr raytraceNode = Node::validatePtr<RaytraceNode>(node);
-		raytraceNode->setBeamDivergence(beam_divergence);
+		raytraceNode->setBeamDivergence(horizontal_beam_divergence, vertical_beam_divergence);
 	});
-	TAPE_HOOK(node, beam_divergence);
+	TAPE_HOOK(node, horizontal_beam_divergence, vertical_beam_divergence);
 	return status;
 }
 
 void TapeCore::tape_node_raytrace_configure_beam_divergence(const YAML::Node& yamlNode, PlaybackState& state)
 {
 	auto nodeId = yamlNode[0].as<TapeAPIObjectID>();
 	rgl_node_t node = state.nodes.at(nodeId);
-	rgl_node_raytrace_configure_beam_divergence(node, yamlNode[1].as<float>());
+	rgl_node_raytrace_configure_beam_divergence(node, yamlNode[1].as<float>(), yamlNode[2].as<float>());
 }
 
 RGL_API rgl_status_t rgl_node_points_format(rgl_node_t* node, const rgl_field_t* fields, int32_t field_count)
diff --git a/src/gpu/MultiReturn.hpp b/src/gpu/MultiReturn.hpp
@@ -17,8 +17,8 @@
 #include <RGLFields.hpp>
 
 #define MULTI_RETURN_BEAM_VERTICES 8
-#define MULTI_RETURN_BEAM_CIRCLES 2
-#define MULTI_RETURN_BEAM_SAMPLES (1 + (MULTI_RETURN_BEAM_VERTICES * MULTI_RETURN_BEAM_CIRCLES))
+#define MULTI_RETURN_BEAM_LAYERS 2
+#define MULTI_RETURN_BEAM_SAMPLES (1 + (MULTI_RETURN_BEAM_VERTICES * MULTI_RETURN_BEAM_LAYERS))
 
 struct MultiReturnPointers
 {
diff --git a/src/gpu/RaytraceRequestContext.hpp b/src/gpu/RaytraceRequestContext.hpp
@@ -67,7 +67,8 @@ struct RaytraceRequestContext
 	Field<INCIDENT_ANGLE_F32>::type* incidentAngle;
 
 	// Multi-Return
-	float beamHalfDivergence;
+	float hBeamHalfDivergenceRad;
+	float vBeamHalfDivergenceRad;
 	MultiReturnPointers mrSamples;
 };
 static_assert(std::is_trivially_copyable<RaytraceRequestContext>::value);
diff --git a/src/gpu/optixPrograms.cu b/src/gpu/optixPrograms.cu
@@ -39,7 +39,7 @@ __device__ void saveRayResult(const Vec3f& xyz, float distance, float intensity,
                               float laserRetro);
 __device__ void saveNonHitRayResult(float nonHitDistance);
 __device__ void shootSamplingRay(const Mat3x4f& ray, float maxRange, unsigned sampleBeamIdx);
-__device__ Mat3x4f makeBeamSampleRayTransform(float halfDivergenceAngleRad, unsigned circleIdx, unsigned vertexIdx);
+__device__ Mat3x4f makeBeamSampleRayTransform(float hHalfDivergenceRad, float vHalfDivergenceRad, unsigned layerIdx, unsigned vertexIdx);
 
 extern "C" __global__ void __raygen__()
 {
@@ -86,13 +86,13 @@ extern "C" __global__ void __raygen__()
 	float maxRange = ctx.rayRangesCount == 1 ? ctx.rayRanges[0].y() : ctx.rayRanges[rayIdx].y();
 
 	shootSamplingRay(ray, maxRange, 0); // Shoot primary ray
-	if (ctx.beamHalfDivergence > 0.0f) {
+	if (ctx.hBeamHalfDivergenceRad > 0.0f && ctx.vBeamHalfDivergenceRad > 0.0f) {
 		// Shoot multi-return sampling rays
-		for (int circleIdx = 0; circleIdx < MULTI_RETURN_BEAM_CIRCLES; ++circleIdx) {
+		for (int layerIdx = 0; layerIdx < MULTI_RETURN_BEAM_LAYERS; ++layerIdx) {
 			for (int vertexIdx = 0; vertexIdx < MULTI_RETURN_BEAM_VERTICES; vertexIdx++) {
-				Mat3x4f sampleRay = ray * makeBeamSampleRayTransform(ctx.beamHalfDivergence, circleIdx, vertexIdx);
+				Mat3x4f sampleRay = ray * makeBeamSampleRayTransform(ctx.hBeamHalfDivergenceRad, ctx.vBeamHalfDivergenceRad, layerIdx, vertexIdx);
 				// Sampling rays indexes start from 1, 0 is reserved for the primary ray
-				const unsigned beamSampleRayIdx = 1 + circleIdx * MULTI_RETURN_BEAM_VERTICES + vertexIdx;
+				const unsigned beamSampleRayIdx = 1 + layerIdx * MULTI_RETURN_BEAM_VERTICES + vertexIdx;
 				shootSamplingRay(sampleRay, maxRange, beamSampleRayIdx);
 			}
 		}
@@ -147,7 +147,7 @@ extern "C" __global__ void __closesthit__()
 			return hitWorldRaytraced;
 		}
 		Mat3x4f sampleRayTf = beamSampleRayIdx == 0 ? Mat3x4f::identity() :
-		                                              makeBeamSampleRayTransform(ctx.beamHalfDivergence, circleIdx, vertexIdx);
+		                                              makeBeamSampleRayTransform(ctx.hBeamHalfDivergenceRad, ctx.vBeamHalfDivergenceRad, circleIdx, vertexIdx);
 		Mat3x4f undistortedRay = ctx.raysWorld[beamIdx] * sampleRayTf;
 		Vec3f undistortedOrigin = undistortedRay * Vec3f{0, 0, 0};
 		Vec3f undistortedDir = undistortedRay * Vec3f{0, 0, 1} - undistortedOrigin;
@@ -257,17 +257,21 @@ __device__ void shootSamplingRay(const Mat3x4f& ray, float maxRange, unsigned in
 	optixTrace(ctx.scene, origin, dir, 0.0f, maxRange, 0.0f, OptixVisibilityMask(255), flags, 0, 1, 0, beamSampleRayIdx);
 }
 
-__device__ Mat3x4f makeBeamSampleRayTransform(float halfDivergenceAngleRad, unsigned int circleIdx, unsigned int vertexIdx)
+__device__ Mat3x4f makeBeamSampleRayTransform(float hHalfDivergenceRad, float vHalfDivergenceRad, unsigned int layerIdx, unsigned int vertexIdx)
 {
-	if (ctx.beamHalfDivergence == 0.0f) {
+	if (ctx.hBeamHalfDivergenceRad == 0.0f && ctx.vBeamHalfDivergenceRad == 0.0f) {
 		return Mat3x4f::identity();
 	}
-	const auto circleDivergence = halfDivergenceAngleRad * (1.0f - static_cast<float>(circleIdx) / MULTI_RETURN_BEAM_CIRCLES);
-	auto vertexAngleStep = 2.0f * static_cast<float>(M_PI) / MULTI_RETURN_BEAM_VERTICES;
-	// First, rotate around X to move the ray vector (initially {0,0,1}) to diverge from the Z axis by circleDivergence.
-	// Then rotate around Z to move the ray vector on the circle.
-	return Mat3x4f::rotationRad(0, 0, static_cast<float>(vertexIdx) * vertexAngleStep) * // Second
-	       Mat3x4f::rotationRad(circleDivergence, 0, 0);
+
+	const float hCurrentHalfDivergenceRad = hHalfDivergenceRad * (1.0f - static_cast<float>(layerIdx) / MULTI_RETURN_BEAM_LAYERS);
+	const float vCurrentHalfDivergenceRad = vHalfDivergenceRad * (1.0f - static_cast<float>(layerIdx) / MULTI_RETURN_BEAM_LAYERS);
+
+	const float angleStep = 2.0f * static_cast<float>(M_PI) / MULTI_RETURN_BEAM_VERTICES;
+
+	const float hAngle = hCurrentHalfDivergenceRad * cos(static_cast<float>(vertexIdx) * angleStep);
+	const float vAngle = vCurrentHalfDivergenceRad * sin(static_cast<float>(vertexIdx) * angleStep);
+
+	return Mat3x4f::rotationRad(vAngle, 0.0f, 0.0f) * Mat3x4f::rotationRad(0.0f, hAngle, 0.0f);
 }
 
 __device__ void saveNonHitRayResult(float nonHitDistance)
diff --git a/src/graph/NodesCore.hpp b/src/graph/NodesCore.hpp
@@ -135,7 +135,9 @@ struct RaytraceNode : IPointsNode
 	void enableRayDistortion(bool enabled) { doApplyDistortion = enabled; }
 	void setNonHitDistanceValues(float nearDistance, float farDistance);
 	void setNonHitsMask(const int8_t* maskRaw, size_t maskPointCount);
-	void setBeamDivergence(float divergence) { beamHalfDivergence = divergence / 2.0f; }
+	void setBeamDivergence(float hDivergenceRad, float vDivergenceRad) {
+		hBeamHalfDivergenceRad = hDivergenceRad / 2.0f;
+		vBeamHalfDivergenceRad = vDivergenceRad / 2.0f;}
 
 private:
 	IRaysNode::Ptr raysNode;
@@ -147,7 +149,8 @@ struct RaytraceNode : IPointsNode
 
 	float nearNonHitDistance{std::numeric_limits<float>::infinity()};
 	float farNonHitDistance{std::numeric_limits<float>::infinity()};
-	float beamHalfDivergence = 0.0f;
+	float hBeamHalfDivergenceRad = 0.0f;
+	float vBeamHalfDivergenceRad = 0.0f;
 
 	DeviceAsyncArray<int8_t>::Ptr rayMask;
 
diff --git a/src/graph/RaytraceNode.cpp b/src/graph/RaytraceNode.cpp
@@ -124,7 +124,8 @@ void RaytraceNode::enqueueExecImpl()
 	    .elevation = getPtrTo<ELEVATION_F32>(),
 	    .normal = getPtrTo<NORMAL_VEC3_F32>(),
 	    .incidentAngle = getPtrTo<INCIDENT_ANGLE_F32>(),
-	    .beamHalfDivergence = beamHalfDivergence,
+	    .hBeamHalfDivergenceRad = hBeamHalfDivergenceRad,
+	    .vBeamHalfDivergenceRad = vBeamHalfDivergenceRad,
 	    .mrSamples = mrSamples.getPointers(),
 	};
 
@@ -134,7 +135,7 @@ void RaytraceNode::enqueueExecImpl()
 	CHECK_OPTIX(optixLaunch(Optix::getOrCreate().pipeline, getStreamHandle(), pipelineArgsPtr, pipelineArgsSize, &sceneSBT,
 	                        launchDims.x, launchDims.y, launchDims.y));
 
-	if (beamHalfDivergence > 0.0f) {
+	if (hBeamHalfDivergenceRad > 0.0f && vBeamHalfDivergenceRad > 0.0f) {
 		gpuProcessBeamSamplesFirstLast(getStreamHandle(), raysNode->getRayCount(), MULTI_RETURN_BEAM_SAMPLES,
 		                               mrSamples.getPointers(), mrFirst.getPointers(), mrLast.getPointers(), raysPtr);
 	}
diff --git a/test/src/TapeTest.cpp b/test/src/TapeTest.cpp
@@ -235,8 +235,9 @@ TEST_F(TapeTest, RecordPlayAllCalls)
 	float farNonHitDistance = 2.0f;
 	EXPECT_RGL_SUCCESS(rgl_node_raytrace_configure_non_hits(raytrace, nearNonHitDistance, farNonHitDistance));
 
-	float beamDivergence = 0.1f;
-	EXPECT_RGL_SUCCESS(rgl_node_raytrace_configure_beam_divergence(raytrace, beamDivergence));
+	float hBeamDivergence = 0.1f;
+	float vBeamDivergence = 0.1f;
+	EXPECT_RGL_SUCCESS(rgl_node_raytrace_configure_beam_divergence(raytrace, hBeamDivergence, vBeamDivergence));
 
 	rgl_node_t format = nullptr;
 	std::vector<rgl_field_t> fields = {RGL_FIELD_XYZ_VEC3_F32, RGL_FIELD_DISTANCE_F32};
diff --git a/test/src/graph/multiReturnTest.cpp b/test/src/graph/multiReturnTest.cpp
@@ -5,6 +5,9 @@
 
 #include "RGLFields.hpp"
 #include "math/Mat3x4f.hpp"
+#include "gpu/MultiReturn.hpp"
+
+#include <cmath>
 
 using namespace std::chrono_literals;
 
@@ -17,8 +20,10 @@ class GraphMultiReturn : public RGLTest
 protected:
 	// VLP16 data
 	const float vlp16LidarObjectDistance = 100.0f;
-	const float vlp16LidarHBeamDivergence = 0.003f; // Velodyne VLP16 horizontal beam divergence in rads
-	const float vlp16LidarHBeamDiameter = 0.2868f;  // Velodyne VLP16 beam horizontal diameter at 100m
+	const float vlp16LidarHBeamDivergence = 0.003f;  // Velodyne VLP16 horizontal beam divergence in rads
+	const float vlp16LidarVBeamDivergence = 0.0015f; // Velodyne VLP16 vertical beam divergence in rads
+	const float vlp16LidarHBeamDiameter = 0.2868f;   // Velodyne VLP16 beam horizontal diameter at 100m
+	const float vlp16LidarVBeamDiameter = 0.1596f;   // Velodyne VLP16 beam vertical diameter at 100m
 
 	std::vector<Mat3x4f> vlp16Channels{Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(11.2f)}, {0.0f, -15.0f, 0.0f}),
 	                                   Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(0.7f)}, {0.0f, +1.0f, 0.0f}),
@@ -45,13 +50,13 @@ class GraphMultiReturn : public RGLTest
 	           publish = nullptr, cameraPublish = nullptr, mrPublishFirst = nullptr, mrPublishLast = nullptr,
 	           compactFirst = nullptr, compactLast = nullptr;
 
-	void constructMRGraph(const std::vector<rgl_mat3x4f>& raysTf, const rgl_mat3x4f& lidarPose, const float beamDivAngle,
-	                      bool withPublish = false)
+	void constructMRGraph(const std::vector<rgl_mat3x4f>& raysTf, const rgl_mat3x4f& lidarPose, const float hBeamDivAngle,
+	                      const float vBeamDivAngle, bool withPublish = false)
 	{
 		EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&mrRays, raysTf.data(), raysTf.size()));
 		EXPECT_RGL_SUCCESS(rgl_node_rays_transform(&mrTransform, &lidarPose));
 		EXPECT_RGL_SUCCESS(rgl_node_raytrace(&mrRaytrace, nullptr));
-		EXPECT_RGL_SUCCESS(rgl_node_raytrace_configure_beam_divergence(mrRaytrace, beamDivAngle));
+		EXPECT_RGL_SUCCESS(rgl_node_raytrace_configure_beam_divergence(mrRaytrace, hBeamDivAngle, vBeamDivAngle));
 		EXPECT_RGL_SUCCESS(rgl_node_multi_return_switch(&mrFirst, RGL_RETURN_TYPE_FIRST));
 		EXPECT_RGL_SUCCESS(rgl_node_multi_return_switch(&mrLast, RGL_RETURN_TYPE_LAST));
 		EXPECT_RGL_SUCCESS(rgl_node_points_compact_by_field(&compactFirst, RGL_FIELD_IS_HIT_I32));
@@ -134,9 +139,7 @@ TEST_F(GraphMultiReturn, vlp16_data_compare)
 	// Scene
 	spawnCubeOnScene(Mat3x4f::identity());
 
-	// VLP16 horizontal beam divergence in rads
-	const float beamDivAngle = vlp16LidarHBeamDivergence;
-	constructMRGraph(raysTf, lidarPose, beamDivAngle);
+	constructMRGraph(raysTf, lidarPose, vlp16LidarHBeamDivergence, vlp16LidarVBeamDivergence);
 
 	EXPECT_RGL_SUCCESS(rgl_graph_run(mrRays));
 
@@ -159,7 +162,7 @@ TEST_F(GraphMultiReturn, vlp16_data_compare)
 	const auto mrLastIsHits = mrLastOutPointcloud.getFieldValues<IS_HIT_I32>();
 	const auto mrLastPoints = mrLastOutPointcloud.getFieldValues<XYZ_VEC3_F32>();
 	const auto mrLastDistances = mrLastOutPointcloud.getFieldValues<DISTANCE_F32>();
-	const float expectedDiameter = vlp16LidarHBeamDiameter;
+	const float expectedDiameter = std::max(vlp16LidarHBeamDiameter, vlp16LidarVBeamDiameter);
 	const auto expectedLastDistance = static_cast<float>(sqrt(pow(lidarCubeFaceDist, 2) + pow(expectedDiameter / 2, 2)));
 	// Substract because the ray is pointing as is the negative X axis
 	const auto expectedLastPoint = lidarTransl - Vec3f{expectedLastDistance, 0.0f, 0.0f};
@@ -177,7 +180,7 @@ TEST_F(GraphMultiReturn, vlp16_data_compare)
  * with two cubes placed one behind the other, one cube cyclically moving sideways.
  * LiDAR fires the beam in such a way that in some frames the beam partially overlaps the edge of the moving cube.
  */
-TEST_F(GraphMultiReturn, pairs_of_cubes_in_motion)
+TEST_F(GraphMultiReturn, cube_in_motion)
 {
 	/*
 	 *                            gap
@@ -208,8 +211,7 @@ TEST_F(GraphMultiReturn, pairs_of_cubes_in_motion)
 	                                      spawnCubeOnScene(entitiesTransforms.at(1))};
 
 	// Lidar with MR
-	const float beamDivAngle = 0.003f;
-	constructMRGraph(raysTf, lidarPose, beamDivAngle, true);
+	constructMRGraph(raysTf, lidarPose, vlp16LidarHBeamDivergence, vlp16LidarVBeamDivergence, true);
 
 	// Lidar without MR
 	EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&rays, raysTf.data(), raysTf.size()));
@@ -264,7 +266,7 @@ TEST_F(GraphMultiReturn, stairs)
 	// Scene
 	const float stairsBaseHeight = 0.0f;
 	const float stepWidth = 1.0f;
-	const float stepHeight = vlp16LidarHBeamDiameter + 0.1f;
+	const float stepHeight = vlp16LidarVBeamDiameter + 0.1f;
 	const float stepDepth = 0.8f;
 	const Vec3f stairsTranslation{2.0f, 0.0f, 0.0f};
 
@@ -279,8 +281,7 @@ TEST_F(GraphMultiReturn, stairs)
 	const rgl_mat3x4f lidarPose{(Mat3x4f::translation(lidarTransl + stairsTranslation)).toRGL()};
 
 	// Lidar with MR
-	const float beamDivAngle = vlp16LidarHBeamDivergence;
-	constructMRGraph(raysTf, lidarPose, beamDivAngle, true);
+	constructMRGraph(raysTf, lidarPose, vlp16LidarHBeamDivergence, vlp16LidarVBeamDivergence, true);
 
 	// Camera
 	rgl_mat3x4f cameraPose = Mat3x4f::translation({0.0f, -1.5f, stepHeight * 3 + 1.0f}).toRGL();
@@ -289,8 +290,9 @@ TEST_F(GraphMultiReturn, stairs)
 
 	int frameId = 0;
 	while (true) {
-		const float newBeamDivAngle = beamDivAngle + std::sin(frameId * 0.1f) * beamDivAngle;
-		ASSERT_RGL_SUCCESS(rgl_node_raytrace_configure_beam_divergence(mrRaytrace, newBeamDivAngle));
+		const float newVBeamDivAngle = vlp16LidarVBeamDivergence + std::sin(frameId * 0.1f) * vlp16LidarVBeamDivergence;
+		ASSERT_RGL_SUCCESS(
+		    rgl_node_raytrace_configure_beam_divergence(mrRaytrace, vlp16LidarHBeamDivergence, newVBeamDivAngle));
 
 		ASSERT_RGL_SUCCESS(rgl_graph_run(mrRaytrace));
 
@@ -330,4 +332,63 @@ TEST_F(GraphMultiReturn, multiple_ray_beams)
 	ASSERT_RGL_SUCCESS(rgl_graph_run(cameraRays));
 	ASSERT_RGL_SUCCESS(rgl_graph_run(mrRays));
 }
+
+/**
+ * In the MR implementation, the beam is modeled using beam samples, from which the first and last hits are calculated.
+ * This test uses the code that generates these beam samples and fires them inside the cube.
+ * It verifies the accuracy of beam sample generation with different horizontal and vertical divergence angles.
+ */
+TEST_F(GraphMultiReturn, horizontal_vertical_beam_divergence)
+{
+	GTEST_SKIP(); // Comment to run the test
+
+	const float hHalfDivergenceAngleRad = M_PI / 4;
+	const float vHalfDivergenceAngleRad = M_PI / 8;
+
+	// Lidar
+	const auto lidarTransl = Vec3f{0.0f, 0.0f, 0.0f};
+	const rgl_mat3x4f lidarPose = Mat3x4f::TRS(lidarTransl).toRGL();
+	std::vector<rgl_mat3x4f> raysTf;
+	raysTf.reserve(MULTI_RETURN_BEAM_SAMPLES);
+
+	raysTf.emplace_back(Mat3x4f::identity().toRGL());
+
+	// Code that generates beam samples in the makeBeamSampleRayTransform function (gpu/optixPrograms.cu)
+	for (int layerIdx = 0; layerIdx < MULTI_RETURN_BEAM_LAYERS; ++layerIdx) {
+		for (int vertexIdx = 0; vertexIdx < MULTI_RETURN_BEAM_VERTICES; ++vertexIdx) {
+			const float hCurrentDivergence = hHalfDivergenceAngleRad *
+			                                 (1.0f - static_cast<float>(layerIdx) / MULTI_RETURN_BEAM_LAYERS);
+			const float vCurrentDivergence = vHalfDivergenceAngleRad *
+			                                 (1.0f - static_cast<float>(layerIdx) / MULTI_RETURN_BEAM_LAYERS);
+
+			const float angleStep = 2.0f * static_cast<float>(M_PI) / MULTI_RETURN_BEAM_VERTICES;
+
+			const float hAngle = hCurrentDivergence * std::cos(static_cast<float>(vertexIdx) * angleStep);
+			const float vAngle = vCurrentDivergence * std::sin(static_cast<float>(vertexIdx) * angleStep);
+
+			const auto rotation = Mat3x4f::rotationRad(vAngle, 0.0f, 0.0f) * Mat3x4f::rotationRad(0.0f, hAngle, 0.0f);
+			raysTf.emplace_back(rotation.toRGL());
+		}
+	}
+
+	// Scene
+	spawnCubeOnScene(Mat3x4f::TRS({0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, {6.0f, 6.0f, 6.0f}));
+
+	// Camera
+	constructCameraGraph(Mat3x4f::identity().toRGL());
+
+	// Graph without MR
+	EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&rays, raysTf.data(), raysTf.size()));
+	EXPECT_RGL_SUCCESS(rgl_node_rays_transform(&transform, &lidarPose));
+	EXPECT_RGL_SUCCESS(rgl_node_raytrace(&raytrace, nullptr));
+	EXPECT_RGL_SUCCESS(rgl_node_points_format(&format, fields.data(), fields.size()));
+	EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&publish, "MRTest_Lidar_Without_MR", "world"));
+	EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(rays, transform));
+	EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(transform, raytrace));
+	EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(raytrace, format));
+	EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(format, publish));
+
+	ASSERT_RGL_SUCCESS(rgl_graph_run(cameraRays));
+	ASSERT_RGL_SUCCESS(rgl_graph_run(rays));
+}
 #endif

Original file line number	Diff line number	Diff line change
`@@ -17,8 +17,8 @@`
`17`	`17`	`#include <RGLFields.hpp>`
`18`	`18`
`19`	`19`	`#define MULTI_RETURN_BEAM_VERTICES 8`
`20`		`-#define MULTI_RETURN_BEAM_CIRCLES 2`
`21`		`-#define MULTI_RETURN_BEAM_SAMPLES (1 + (MULTI_RETURN_BEAM_VERTICES * MULTI_RETURN_BEAM_CIRCLES))`
	`20`	`+#define MULTI_RETURN_BEAM_LAYERS 2`
	`21`	`+#define MULTI_RETURN_BEAM_SAMPLES (1 + (MULTI_RETURN_BEAM_VERTICES * MULTI_RETURN_BEAM_LAYERS))`
`22`	`22`
`23`	`23`	`struct MultiReturnPointers`
`24`	`24`	`{`