Refactor raytrace node configurations and introduce non-hit distance handling (#257)

* Add prototype of rgl_node_raytrace_configure_velocity

* Add prototype of tape_node_raytrace_configure_velocity

* Add invalid_node_object test

* Add prototype of rgl_node_raytrace_configure_distortion

* Add documentation

* Refactor usage of raytrace in motion & with distortion

* Changes in the api documentation

* Add prototype of raytrace_configure_non_hit_distance_values

* Add prototype of raytrace_configure_non_hit_distance_values

* Finish implementation and tests

* Apply suggestions from code review

Co-authored-by: Mateusz Szczygielski <[email protected]>

* Review fixes

* Re-review fixes

---------
Co-authored-by: Mateusz Szczygielski <[email protected]>

Author: nebraszka

include/rgl/api/core.h

@@ -563,53 +563,42 @@ RGL_API rgl_status_t rgl_node_points_transform(rgl_node_t* node, const rgl_mat3x
  */
 RGL_API rgl_status_t rgl_node_raytrace(rgl_node_t* node, rgl_scene_t scene);
 
-// TODO: Remove this API call on a new release. It is replaced by `rgl_node_raytrace_in_motion`.
 /**
- * Creates or modifies RaytraceNode.
- * The same as rgl_node_raytrace, but it applies velocity distortion additionally.
- * To perform raytrace with velocity distortion the time offsets must be set to the rays (using rgl_node_rays_set_time_offsets).
- * The velocities passed to that node must be in the local coordinate frame in which rays are described.
- * NOTE:
- * The distortion takes into account only sensor velocity. The velocity of the objects being scanned by the sensor is not considered.
- * Graph input: rays
- * Graph output: point cloud (sparse)
- * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified.
- * @param scene Handle to a scene to perform raytracing on. Pass null to use the default scene
- * @param linear_velocity Pointer to a single 3D vector describing the linear velocity of the sensor.
- *                        The velocity is in units per second.
- * @param angular_velocity Pointer to a single 3D vector describing the delta angular velocity of the sensor in euler angles (roll, pitch, yaw).
- *                         The velocity is in radians per second.
+ * Modifies RaytraceNode to apply sensor velocity.
+ * Necessary for velocity distortion or calculating fields: RGL_FIELD_RELATIVE_VELOCITY_VEC3_F32 and RGL_FIELD_RADIAL_SPEED_F32.
+ * Relative velocity calculation:
+ * To calculate relative velocity the pipeline must allow to compute absolute velocities. For more details refer to API calls documentation:
+ * `rgl_scene_set_time`, `rgl_entity_set_pose`, and `rgl_mesh_update_vertices`
+ * @param node RaytraceNode to modify
+ * @param linear_velocity 3D vector for linear velocity in units per second.
+ * @param angular_velocity 3D vector for angular velocity in radians per second (roll, pitch, yaw).
  */
-RGL_API rgl_status_t rgl_node_raytrace_with_distortion(rgl_node_t* node, rgl_scene_t scene, const rgl_vec3f* linear_velocity,
-                                                       const rgl_vec3f* angular_velocity);
+RGL_API rgl_status_t rgl_node_raytrace_configure_velocity(rgl_node_t node, const rgl_vec3f* linear_velocity,
+                                                          const rgl_vec3f* angular_velocity);
 
 /**
- * Creates or modifies RaytraceNode.
- * The same as rgl_node_raytrace, but it provides sensor's velocity to the pipeline.
- * It is required to perform velocity distortion or calculate fields: RGL_FIELD_RELATIVE_VELOCITY_VEC3_F32 and RGL_FIELD_RADIAL_SPEED_F32
- *
- * Velocity distortion:
- * To perform raytrace with velocity distortion the time offsets must be set to the rays (using rgl_node_rays_set_time_offsets).
- * The velocities passed to that node must be in the local coordinate frame in which rays are described.
+ * Modifies RaytraceNode to apply sensor distortion.
+ * Requires time offsets set to rays using rgl_node_rays_set_time_offsets.
  * NOTE:
  * The distortion takes into account only sensor velocity. The velocity of the objects being scanned by the sensor is not considered.
- *
- * Relative velocity calculation:
- * To calculate relative velocity the pipeline must allow to compute absolute velocities. For more details refer to API calls documentation:
- * `rgl_scene_set_time`, `rgl_entity_set_pose`, and `rgl_mesh_update_vertices`
- *
- * Graph input: rays
- * Graph output: point cloud (sparse)
- * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified.
- * @param scene Handle to a scene to perform raytracing on. Pass null to use the default scene
- * @param linear_velocity Pointer to a single 3D vector describing the linear velocity of the sensor.
- *                        The velocity is in units per second.
- * @param angular_velocity Pointer to a single 3D vector describing the delta angular velocity of the sensor in euler angles (roll, pitch, yaw).
- *                         The velocity is in radians per second.
- * @param apply_ray_distortion If true, velocity distortion feature will be enabled.
+ * Use rgl_node_raytrace_configure_velocity to set sensor velocity.
+ * @param node RaytraceNode to modify
+ * @param enable If true, velocity distortion feature will be enabled.
+ */
+RGL_API rgl_status_t rgl_node_raytrace_configure_distortion(rgl_node_t node, bool enable);
+
+/**
+ * Modifies RaytraceNode to set non-hit values for distance.
+ * Default non-hit value for the RGL_FIELD_DISTANCE_F32 field is set to infinity.
+ * This function allows to set custom values:
+ *  - for non-hits closer than a minimum range (`near`),
+ *  - for non-hits beyond a maximum range (`far`).
+ * Concurrently, it computes the RGL_FIELD_XYZ_VEC3_F32 field for these non-hit scenarios based on these distances, along with ray origin and direction.
+ * @param node RaytraceNode to modify.
+ * @param near Distance value for non-hits closer than minimum range.
+ * @param far Distance value for non-hits beyond maximum range.
  */
-RGL_API rgl_status_t rgl_node_raytrace_in_motion(rgl_node_t* node, rgl_scene_t scene, const rgl_vec3f* linear_velocity,
-                                                 const rgl_vec3f* angular_velocity, bool apply_ray_distortion);
+RGL_API rgl_status_t rgl_node_raytrace_configure_non_hits(rgl_node_t node, float near, float far);
 
 /**
  * Creates or modifies FormatPointsNode.
@@ -641,7 +630,7 @@ RGL_API rgl_status_t rgl_node_points_yield(rgl_node_t* node, const rgl_field_t*
  * Graph output: point cloud (compacted)
  * @param node If (*node) == nullptr, a new Node will be created. Otherwise, (*node) will be modified.
  */
-RGL_API [[ deprecated("Use rgl_node_points_compact_by_field(rgl_node_t* node, rgl_field_t field) instead.") ]] rgl_status_t
+RGL_API [[deprecated("Use rgl_node_points_compact_by_field(rgl_node_t* node, rgl_field_t field) instead.")]] rgl_status_t
 rgl_node_points_compact(rgl_node_t* node);
 
 /**
@@ -734,7 +723,8 @@ RGL_API rgl_status_t rgl_node_points_radar_postprocess(rgl_node_t* node, float d
  * @param sensor_up_vector Pointer to single Vec3 describing up vector of depended frame.
  * @param ground_angle_threshold The maximum angle between the sensor's ray and the normal vector of the hit point in radians.
  */
-RGL_API rgl_status_t rgl_node_points_filter_ground(rgl_node_t* node, const rgl_vec3f* sensor_up_vector, float ground_angle_threshold);
+RGL_API rgl_status_t rgl_node_points_filter_ground(rgl_node_t* node, const rgl_vec3f* sensor_up_vector,
+                                                   float ground_angle_threshold);
 
 /**
  * Creates or modifies GaussianNoiseAngularRaysNode.

src/RGLFields.hpp

@@ -31,8 +31,6 @@
  * - test/include/helpers/testPointCloud.hpp: add/delete fieldGenerator inside fieldGenerators
  */
 
-#define NON_HIT_VALUE FLT_MAX
-
 typedef unsigned char TextureTexelFormat;
 
 // Shorter versions to avoid long type names
@@ -117,7 +115,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(IS_HIT_I32, int32_t); // Signed may be faster
+FIELD(IS_HIT_I32, int32_t);    // Signed may be faster
 FIELD(IS_GROUND_I32, int32_t); // Signed may be faster
 FIELD(DISTANCE_F32, float);
 FIELD(AZIMUTH_F32, float);

src/api/apiCore.cpp

@@ -812,10 +812,6 @@ RGL_API rgl_status_t rgl_node_raytrace(rgl_node_t* node, rgl_scene_t scene)
 
 		createOrUpdateNode<RaytraceNode>(node);
 		auto raytraceNode = Node::validatePtr<RaytraceNode>(*node);
-		// Clear velocity that could be set by rgl_node_raytrace_in_motion
-		raytraceNode->setVelocity(Vec3f{0, 0, 0}, Vec3f{0, 0, 0});
-		// Disable ray distortion that could be set by rgl_node_raytrace_in_motion
-		raytraceNode->enableRayDistortion(false);
 	});
 	TAPE_HOOK(node, scene);
 	return status;
@@ -829,46 +825,67 @@ void TapeCore::tape_node_raytrace(const YAML::Node& yamlNode, PlaybackState& sta
 	state.nodes.insert({nodeId, node});
 }
 
-RGL_API rgl_status_t rgl_node_raytrace_with_distortion(rgl_node_t* node, rgl_scene_t scene, const rgl_vec3f* linear_velocity,
-                                                       const rgl_vec3f* angular_velocity)
+RGL_API rgl_status_t rgl_node_raytrace_configure_velocity(rgl_node_t node, const rgl_vec3f* linear_velocity,
+                                                          const rgl_vec3f* angular_velocity)
 {
-	return rgl_node_raytrace_in_motion(node, scene, linear_velocity, angular_velocity, true);
+	auto status = rglSafeCall([&]() {
+		RGL_API_LOG("rgl_node_raytrace_configure_velocity(node={}, linear_velocity={}, angular_velocity={})", repr(node),
+		            repr(linear_velocity), repr(angular_velocity));
+		CHECK_ARG(node != nullptr);
+		CHECK_ARG(linear_velocity != nullptr);
+		CHECK_ARG(angular_velocity != nullptr);
+		RaytraceNode::Ptr raytraceNode = Node::validatePtr<RaytraceNode>(node);
+		raytraceNode->setVelocity(*reinterpret_cast<const Vec3f*>(linear_velocity),
+		                          *reinterpret_cast<const Vec3f*>(angular_velocity));
+	});
+	TAPE_HOOK(node, linear_velocity, angular_velocity);
+	return status;
 }
 
-void TapeCore::tape_node_raytrace_with_distortion(const YAML::Node& yamlNode, PlaybackState& state)
+void TapeCore::tape_node_raytrace_configure_velocity(const YAML::Node& yamlNode, PlaybackState& state)
 {
-	return tape_node_raytrace_in_motion(yamlNode, state);
+	auto nodeId = yamlNode[0].as<TapeAPIObjectID>();
+	rgl_node_t node = state.nodes.at(nodeId);
+	rgl_node_raytrace_configure_velocity(state.nodes.at(nodeId), state.getPtr<const rgl_vec3f>(yamlNode[1]),
+	                                     state.getPtr<const rgl_vec3f>(yamlNode[2]));
 }
 
-RGL_API rgl_status_t rgl_node_raytrace_in_motion(rgl_node_t* node, rgl_scene_t scene, const rgl_vec3f* linear_velocity,
-                                                 const rgl_vec3f* angular_velocity, bool apply_ray_distortion)
+RGL_API rgl_status_t rgl_node_raytrace_configure_distortion(rgl_node_t node, bool enable)
 {
 	auto status = rglSafeCall([&]() {
-		RGL_API_LOG(
-		    "rgl_node_raytrace_in_motion(node={}, scene={}, linear_velocity={}, angular_velocity={}, apply_ray_distortion={})",
-		    repr(node), (void*) scene, repr(linear_velocity), repr(angular_velocity), apply_ray_distortion);
+		RGL_API_LOG("rgl_node_raytrace_configure_distortion(node={}, enable={})", repr(node), enable);
 		CHECK_ARG(node != nullptr);
-		CHECK_ARG(linear_velocity != nullptr);
-		CHECK_ARG(angular_velocity != nullptr);
-		CHECK_ARG(scene == nullptr);
+		RaytraceNode::Ptr raytraceNode = Node::validatePtr<RaytraceNode>(node);
+		raytraceNode->enableRayDistortion(enable);
+	});
+	TAPE_HOOK(node, enable);
+	return status;
+}
 
-		createOrUpdateNode<RaytraceNode>(node);
-		auto raytraceNode = Node::validatePtr<RaytraceNode>(*node);
-		raytraceNode->setVelocity(*reinterpret_cast<const Vec3f*>(linear_velocity),
-		                          *reinterpret_cast<const Vec3f*>(angular_velocity));
-		raytraceNode->enableRayDistortion(apply_ray_distortion);
+void TapeCore::tape_node_raytrace_configure_distortion(const YAML::Node& yamlNode, PlaybackState& state)
+{
+	auto nodeId = yamlNode[0].as<TapeAPIObjectID>();
+	rgl_node_t node = state.nodes.at(nodeId);
+	rgl_node_raytrace_configure_distortion(node, yamlNode[1].as<bool>());
+}
+
+RGL_API rgl_status_t rgl_node_raytrace_configure_non_hits(rgl_node_t node, float near, float far)
+{
+	auto status = rglSafeCall([&]() {
+		RGL_API_LOG("rgl_node_raytrace_configure_non_hits(node={}, near={}, far={})", repr(node), near, far);
+		CHECK_ARG(node != nullptr);
+		RaytraceNode::Ptr raytraceNode = Node::validatePtr<RaytraceNode>(node);
+		raytraceNode->setNonHitDistanceValues(near, far);
 	});
-	TAPE_HOOK(node, scene, linear_velocity, angular_velocity);
+	TAPE_HOOK(node, near, far);
 	return status;
 }
 
-void TapeCore::tape_node_raytrace_in_motion(const YAML::Node& yamlNode, PlaybackState& state)
+void TapeCore::tape_node_raytrace_configure_non_hits(const YAML::Node& yamlNode, PlaybackState& state)
 {
 	auto nodeId = yamlNode[0].as<TapeAPIObjectID>();
-	rgl_node_t node = state.nodes.contains(nodeId) ? state.nodes.at(nodeId) : nullptr;
-	rgl_node_raytrace_with_distortion(&node, nullptr, state.getPtr<const rgl_vec3f>(yamlNode[2]),
-	                                  state.getPtr<const rgl_vec3f>(yamlNode[3]));
-	state.nodes.insert({nodeId, node});
+	rgl_node_t node = state.nodes.at(nodeId);
+	rgl_node_raytrace_configure_non_hits(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)

src/gpu/RaytraceRequestContext.hpp

@@ -24,6 +24,9 @@ struct RaytraceRequestContext
 	Vec3f sensorAngularVelocityRPY;
 	bool doApplyDistortion;
 
+	float nearNonHitDistance;
+	float farNonHitDistance;
+
 	const Mat3x4f* rays;
 	size_t rayCount;
 

src/gpu/optixPrograms.cu

@@ -56,11 +56,9 @@ __forceinline__ __device__ Vec3f decodePayloadVec3f(const Vec3fPayload& src)
 }
 
 template<bool isFinite>
-__forceinline__ __device__ void saveRayResult(const Vec3f& xyz = Vec3f{NON_HIT_VALUE, NON_HIT_VALUE, NON_HIT_VALUE},
-                                              float distance = NON_HIT_VALUE, float intensity = 0,
-                                              const int objectID = RGL_ENTITY_INVALID_ID, const Vec3f& absVelocity = Vec3f{NAN},
-                                              const Vec3f& relVelocity = Vec3f{NAN}, float radialSpeed = NAN,
-                                              const Vec3f& normal = Vec3f{NAN}, float incidentAngle = NAN)
+__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 int rayIdx = optixGetLaunchIndex().x;
 	if (ctx.xyz != nullptr) {
@@ -105,10 +103,22 @@ __forceinline__ __device__ void saveRayResult(const Vec3f& xyz = Vec3f{NON_HIT_V
 	}
 }
 
+__forceinline__ __device__ void saveNonHitRayResult(float nonHitDistance)
+{
+	Mat3x4f ray = ctx.rays[optixGetLaunchIndex().x];
+	Vec3f origin = ray * Vec3f{0, 0, 0};
+	Vec3f dir = ray * Vec3f{0, 0, 1} - origin;
+	Vec3f displacement = dir.normalized() * 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}, NAN, Vec3f{NAN}, NAN);
+}
+
 extern "C" __global__ void __raygen__()
 {
 	if (ctx.scene == 0) {
-		saveRayResult<false>();
+		saveNonHitRayResult(ctx.farNonHitDistance);
 		return;
 	}
 
@@ -177,7 +187,7 @@ extern "C" __global__ void __closesthit__()
 
 	float minRange = ctx.rayRangesCount == 1 ? ctx.rayRanges[0].x() : ctx.rayRanges[optixGetLaunchIndex().x].x();
 	if (distance < minRange) {
-		saveRayResult<false>();
+		saveNonHitRayResult(ctx.nearNonHitDistance);
 		return;
 	}
 
@@ -266,6 +276,6 @@ extern "C" __global__ void __closesthit__()
 	                    incidentAngle);
 }
 
-extern "C" __global__ void __miss__() { saveRayResult<false>(); }
+extern "C" __global__ void __miss__() { saveNonHitRayResult(ctx.farNonHitDistance); }
 
 extern "C" __global__ void __anyhit__() {}

src/graph/NodesCore.hpp

@@ -125,6 +125,7 @@ struct RaytraceNode : IPointsNode
 	// RaytraceNode specific
 	void setVelocity(const Vec3f& linearVelocity, const Vec3f& angularVelocity);
 	void enableRayDistortion(bool enabled) { doApplyDistortion = enabled; }
+	void setNonHitDistanceValues(float near, float far);
 
 private:
 	IRaysNode::Ptr raysNode;
@@ -134,6 +135,9 @@ struct RaytraceNode : IPointsNode
 	Vec3f sensorLinearVelocityXYZ{0, 0, 0};
 	Vec3f sensorAngularVelocityRPY{0, 0, 0};
 
+	float nearNonHitDistance{std::numeric_limits<float>::infinity()};
+	float farNonHitDistance{std::numeric_limits<float>::infinity()};
+
 	HostPinnedArray<RaytraceRequestContext>::Ptr requestCtxHst = HostPinnedArray<RaytraceRequestContext>::create();
 	DeviceAsyncArray<RaytraceRequestContext>::Ptr requestCtxDev = DeviceAsyncArray<RaytraceRequestContext>::create(arrayMgr);
 
@@ -546,5 +550,6 @@ struct FilterGroundPointsNode : IPointsNodeSingleInput
 private:
 	Vec3f sensor_up_vector;
 	float ground_angle_threshold;
-	DeviceAsyncArray<Field<IS_GROUND_I32>::type>::Ptr outNonGround = DeviceAsyncArray<Field<IS_GROUND_I32>::type>::create(arrayMgr);
+	DeviceAsyncArray<Field<IS_GROUND_I32>::type>::Ptr outNonGround = DeviceAsyncArray<Field<IS_GROUND_I32>::type>::create(
+	    arrayMgr);
 };

src/graph/RaytraceNode.cpp

@@ -82,6 +82,8 @@ void RaytraceNode::enqueueExecImpl()
 	    .sensorLinearVelocityXYZ = sensorLinearVelocityXYZ,
 	    .sensorAngularVelocityRPY = sensorAngularVelocityRPY,
 	    .doApplyDistortion = doApplyDistortion,
+	    .nearNonHitDistance = nearNonHitDistance,
+	    .farNonHitDistance = farNonHitDistance,
 	    .rays = raysPtr,
 	    .rayCount = raysNode->getRayCount(),
 	    .rayOriginToWorld = raysNode->getCumulativeRayTransfrom(),
@@ -169,3 +171,9 @@ void RaytraceNode::setVelocity(const Vec3f& linearVelocity, const Vec3f& angular
 	sensorLinearVelocityXYZ = linearVelocity;
 	sensorAngularVelocityRPY = angularVelocity;
 }
+
+void RaytraceNode::setNonHitDistanceValues(float near, float far)
+{
+	nearNonHitDistance = near;
+	farNonHitDistance = far;
+}

src/tape/TapeCore.hpp

@@ -50,8 +50,9 @@ class TapeCore
 	static void tape_node_rays_transform(const YAML::Node& yamlNode, PlaybackState& state);
 	static void tape_node_points_transform(const YAML::Node& yamlNode, PlaybackState& state);
 	static void tape_node_raytrace(const YAML::Node& yamlNode, PlaybackState& state);
-	static void tape_node_raytrace_with_distortion(const YAML::Node& yamlNode, PlaybackState& state);
-	static void tape_node_raytrace_in_motion(const YAML::Node& yamlNode, PlaybackState& state);
+	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_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);
@@ -100,8 +101,9 @@ class TapeCore
 		    TAPE_CALL_MAPPING("rgl_node_rays_transform", TapeCore::tape_node_rays_transform),
 		    TAPE_CALL_MAPPING("rgl_node_points_transform", TapeCore::tape_node_points_transform),
 		    TAPE_CALL_MAPPING("rgl_node_raytrace", TapeCore::tape_node_raytrace),
-		    TAPE_CALL_MAPPING("rgl_node_raytrace_with_distortion", TapeCore::tape_node_raytrace_with_distortion),
-		    TAPE_CALL_MAPPING("rgl_node_raytrace_in_motion", TapeCore::tape_node_raytrace_in_motion),
+		    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_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),