Add API call to configure beam divergence (#283)

* Add API call to configure beam divergence * Review fixes
RobotecAI · Jul 15, 2024 · 04977a5 · 04977a5
1 parent 96d9df3
commit 04977a5
Show file tree

Hide file tree

Showing 6 changed files with 42 additions and 5 deletions.
diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h
@@ -729,6 +729,16 @@ RGL_API rgl_status_t rgl_node_raytrace_configure_non_hits(rgl_node_t node, float
  */
 RGL_API rgl_status_t rgl_node_raytrace_configure_mask(rgl_node_t node, const int8_t* rays_mask, int32_t rays_count);
 
+/**
+ * Modifies RaytraceNode to set beam divergence.
+ * Beam divergence is used to calculate the beam width at the distance of hit point.
+ * 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.
+ */
+RGL_API rgl_status_t rgl_node_raytrace_configure_beam_divergence(rgl_node_t node, float beam_divergence);
+
 /**
  * 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
@@ -939,8 +939,8 @@ void TapeCore::tape_node_raytrace_configure_non_hits(const YAML::Node& yamlNode,
 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);
+		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);
@@ -958,6 +958,26 @@ 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)
+{
+	auto status = rglSafeCall([&]() {
+		RGL_API_LOG("rgl_node_raytrace_configure_beam_divergence(node={}, divergence={})", repr(node), beam_divergence);
+		CHECK_ARG(node != nullptr);
+		CHECK_ARG(beam_divergence >= 0.0f);
+		RaytraceNode::Ptr raytraceNode = Node::validatePtr<RaytraceNode>(node);
+		raytraceNode->setBeamDivergence(beam_divergence);
+	});
+	TAPE_HOOK(node, 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_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/graph/NodesCore.hpp b/src/graph/NodesCore.hpp
@@ -129,6 +129,7 @@ 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; }
 
 private:
 	IRaysNode::Ptr raysNode;
@@ -140,6 +141,7 @@ struct RaytraceNode : IPointsNode
 
 	float nearNonHitDistance{std::numeric_limits<float>::infinity()};
 	float farNonHitDistance{std::numeric_limits<float>::infinity()};
+	float beamHalfDivergence = 0.0f;
 
 	DeviceAsyncArray<int8_t>::Ptr rayMask;
 
@@ -646,8 +648,7 @@ struct RadarTrackObjectsNode : IPointsNodeSingleInput
 
 	RadarTrackObjectsNode();
 
-	void setParameters(float distanceThreshold, float azimuthThreshold, float elevationThreshold,
-	                   float radialSpeedThreshold);
+	void setParameters(float distanceThreshold, float azimuthThreshold, float elevationThreshold, float radialSpeedThreshold);
 
 	// Node
 	void validateImpl() override;

diff --git a/src/graph/RaytraceNode.cpp b/src/graph/RaytraceNode.cpp
@@ -124,7 +124,7 @@ void RaytraceNode::enqueueExecImpl()
 	    .elevation = getPtrTo<ELEVATION_F32>(),
 	    .normal = getPtrTo<NORMAL_VEC3_F32>(),
 	    .incidentAngle = getPtrTo<INCIDENT_ANGLE_F32>(),
-	    .beamHalfDivergence = 0.0f, // TODO: provide API to set externally
+	    .beamHalfDivergence = beamHalfDivergence,
 	    .mrSamples = mrSamples.getPointers(),
 	};
 

diff --git a/src/tape/TapeCore.hpp b/src/tape/TapeCore.hpp
@@ -55,6 +55,7 @@ class TapeCore
 	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_raytrace_configure_beam_divergence(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);
@@ -109,6 +110,8 @@ class TapeCore
 		    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_raytrace_configure_beam_divergence",
+		                      TapeCore::tape_node_raytrace_configure_beam_divergence),
 		    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/src/TapeTest.cpp b/test/src/TapeTest.cpp
@@ -235,6 +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));
+
 	rgl_node_t format = nullptr;
 	std::vector<rgl_field_t> fields = {RGL_FIELD_XYZ_VEC3_F32, RGL_FIELD_DISTANCE_F32};
 	EXPECT_RGL_SUCCESS(rgl_node_points_format(&format, fields.data(), fields.size()));