diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h index fdc62a59..b700820d 100644 --- a/include/rgl/api/core.h +++ b/include/rgl/api/core.h @@ -368,6 +368,12 @@ typedef enum : int32_t RGL_FIELD_DYNAMIC_FORMAT = 13842, } rgl_field_t; +typedef enum : int32_t +{ + RGL_RETURN_TYPE_FIRST = 0, + RGL_RETURN_TYPE_LAST = 1, +} rgl_return_type_t; + /** * Helper enum for axis selection */ diff --git a/src/gpu/MultiReturn.hpp b/src/gpu/MultiReturn.hpp new file mode 100644 index 00000000..4b765f8c --- /dev/null +++ b/src/gpu/MultiReturn.hpp @@ -0,0 +1,28 @@ +// Copyright 2024 Robotec.AI +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#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)) + +struct MultiReturnPointers +{ + Field::type* isHit; + Field::type* xyz; + Field::type* distance; +}; diff --git a/src/gpu/RaytraceRequestContext.hpp b/src/gpu/RaytraceRequestContext.hpp index b3555439..7ecf23e7 100644 --- a/src/gpu/RaytraceRequestContext.hpp +++ b/src/gpu/RaytraceRequestContext.hpp @@ -16,6 +16,7 @@ #include #include +#include struct RaytraceRequestContext { @@ -38,7 +39,7 @@ struct RaytraceRequestContext const int* ringIds; size_t ringIdsCount; - const float* rayTimeOffsets; + const float* rayTimeOffsetsMs; size_t rayTimeOffsetsCount; OptixTraversableHandle scene; @@ -61,5 +62,9 @@ struct RaytraceRequestContext Field::type* elevation; Field::type* normal; Field::type* incidentAngle; + + // Multi-Return + float beamHalfDivergence; + MultiReturnPointers mrSamples; }; static_assert(std::is_trivially_copyable::value); diff --git a/src/gpu/optixPrograms.cu b/src/gpu/optixPrograms.cu index ba248a71..02193549 100644 --- a/src/gpu/optixPrograms.cu +++ b/src/gpu/optixPrograms.cu @@ -26,108 +26,37 @@ #include +// Constants static constexpr float toDeg = (180.0f / M_PI); +// Globals extern "C" static __constant__ RaytraceRequestContext ctx; -struct Vec3fPayload -{ - unsigned p0; - unsigned p1; - unsigned p2; -}; - -__forceinline__ __device__ Vec3fPayload encodePayloadVec3f(const Vec3f& src) -{ - Vec3fPayload dst; - dst.p0 = *(reinterpret_cast(&src[0])); - dst.p1 = *(reinterpret_cast(&src[1])); - dst.p2 = *(reinterpret_cast(&src[2])); - return dst; -} - -__forceinline__ __device__ Vec3f decodePayloadVec3f(const Vec3fPayload& src) -{ - return Vec3f{ - *reinterpret_cast(&src.p0), - *reinterpret_cast(&src.p1), - *reinterpret_cast(&src.p2), - }; -} - +// Helper functions template -__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) { - // Return actual XYZ of the hit point or infinity vector. - ctx.xyz[rayIdx] = xyz; - } - if (ctx.isHit != nullptr) { - ctx.isHit[rayIdx] = isFinite; - } - if (ctx.rayIdx != nullptr) { - ctx.rayIdx[rayIdx] = rayIdx; - } - if (ctx.ringIdx != nullptr && ctx.ringIds != nullptr) { - ctx.ringIdx[rayIdx] = ctx.ringIds[rayIdx % ctx.ringIdsCount]; - } - if (ctx.distance != nullptr) { - ctx.distance[rayIdx] = distance; - } - if (ctx.intensity != nullptr) { - ctx.intensity[rayIdx] = intensity; - } - if (ctx.timestamp != nullptr) { - ctx.timestamp[rayIdx] = ctx.sceneTime; - } - if (ctx.entityId != nullptr) { - ctx.entityId[rayIdx] = isFinite ? objectID : RGL_ENTITY_INVALID_ID; - } - if (ctx.pointAbsVelocity != nullptr) { - ctx.pointAbsVelocity[rayIdx] = absVelocity; - } - if (ctx.pointRelVelocity != nullptr) { - ctx.pointRelVelocity[rayIdx] = relVelocity; - } - if (ctx.radialSpeed != nullptr) { - ctx.radialSpeed[rayIdx] = radialSpeed; - } - if (ctx.normal != nullptr) { - ctx.normal[rayIdx] = normal; - } - if (ctx.incidentAngle != nullptr) { - ctx.incidentAngle[rayIdx] = incidentAngle; - } -} - -__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(xyz, nonHitDistance, 0, RGL_ENTITY_INVALID_ID, Vec3f{NAN}, Vec3f{NAN}, 0.001f, Vec3f{NAN}, NAN); -} +__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); +__device__ void saveNonHitRayResult(float nonHitDistance); +__device__ void shootSamplingRay(const Mat3x4f& ray, float maxRange, unsigned sampleBeamIdx); +__device__ Mat3x4f makeBeamSampleRayTransform(float halfDivergenceAngleRad, unsigned circleIdx, unsigned vertexIdx); extern "C" __global__ void __raygen__() { if (ctx.scene == 0) { - saveNonHitRayResult(ctx.farNonHitDistance); + saveNonHitRayResult( + ctx.farNonHitDistance); // TODO(prybicki): Remove this, assume that host code will not pass invalid scene. return; } const int rayIdx = optixGetLaunchIndex().x; Mat3x4f ray = ctx.rays[rayIdx]; - const Mat3x4f rayLocal = ctx.rayOriginToWorld.inverse() * ray; + const Mat3x4f rayLocal = + ctx.rayOriginToWorld.inverse() * + ray; // TODO(prybicki): instead of computing inverse, we should pass rays in local CF and then transform them to world CF. // Assuming up vector is Y, forward vector is Z (true for Unity). // TODO(msz-rai): allow to define up and forward vectors in RGL + // TODO(prybicki): this kind of computations should be done somewhere else, it's not relevant to raytracing. if (ctx.azimuth != nullptr) { ctx.azimuth[rayIdx] = rayLocal.toRotationYRad(); } @@ -139,69 +68,91 @@ extern "C" __global__ void __raygen__() // Velocities are in the local frame. Need to operate on rays in local frame. // Ray time offsets are in milliseconds, velocities are in unit per seconds. // In order to not lose numerical precision, first multiply values and then convert to proper unit. - ray = Mat3x4f::TRS((ctx.rayTimeOffsets[rayIdx] * ctx.sensorLinearVelocityXYZ) * 0.001f, - (ctx.rayTimeOffsets[rayIdx] * (ctx.sensorAngularVelocityRPY * toDeg)) * 0.001f) * + ray = Mat3x4f::TRS((ctx.rayTimeOffsetsMs[rayIdx] * ctx.sensorLinearVelocityXYZ) * 0.001f, + (ctx.rayTimeOffsetsMs[rayIdx] * (ctx.sensorAngularVelocityRPY * toDeg)) * 0.001f) * rayLocal; // Back to the global frame. ray = ctx.rayOriginToWorld * ray; } - Vec3f origin = ray * Vec3f{0, 0, 0}; - Vec3f dir = ray * Vec3f{0, 0, 1} - origin; float maxRange = ctx.rayRangesCount == 1 ? ctx.rayRanges[0].y() : ctx.rayRanges[rayIdx].y(); - unsigned int flags = OPTIX_RAY_FLAG_DISABLE_ANYHIT; - Vec3fPayload originPayload = encodePayloadVec3f(origin); - optixTrace(ctx.scene, origin, dir, 0.0f, maxRange, 0.0f, OptixVisibilityMask(255), flags, 0, 1, 0, originPayload.p0, - originPayload.p1, originPayload.p2); + shootSamplingRay(ray, maxRange, 0); // Shoot primary ray + if (ctx.beamHalfDivergence > 0.0f) { + // Shoot multi-return sampling rays + for (int circleIdx = 0; circleIdx < MULTI_RETURN_BEAM_CIRCLES; ++circleIdx) { + for (int vertexIdx = 0; vertexIdx < MULTI_RETURN_BEAM_VERTICES; vertexIdx++) { + Mat3x4f sampleRay = ray * makeBeamSampleRayTransform(ctx.beamHalfDivergence, circleIdx, vertexIdx); + // Sampling rays indexes start from 1, 0 is reserved for the primary ray + const unsigned beamSampleRayIdx = 1 + circleIdx * MULTI_RETURN_BEAM_VERTICES + vertexIdx; + shootSamplingRay(sampleRay, maxRange, beamSampleRayIdx); + } + } + } +} + +extern "C" __global__ void __miss__() +{ + const int beamIdx = (int) optixGetLaunchIndex().x; + const int beamSampleIdx = (int) optixGetPayload_0(); + ctx.mrSamples.isHit[beamIdx * MULTI_RETURN_BEAM_SAMPLES + beamSampleIdx] = false; + if (beamSampleIdx == 0) { + saveNonHitRayResult(ctx.farNonHitDistance); + } } extern "C" __global__ void __closesthit__() { const EntitySBTData& entityData = *(const EntitySBTData*) optixGetSbtDataPointer(); + // Triangle const int primID = optixGetPrimitiveIndex(); assert(primID < entityData.indexCount); const Vec3i triangleIndices = entityData.index[primID]; const float u = optixGetTriangleBarycentrics().x; const float v = optixGetTriangleBarycentrics().y; - assert(triangleIndices.x() < entityData.vertexCount); assert(triangleIndices.y() < entityData.vertexCount); assert(triangleIndices.z() < entityData.vertexCount); - const Vec3f& A = entityData.vertex[triangleIndices.x()]; const Vec3f& B = entityData.vertex[triangleIndices.y()]; const Vec3f& C = entityData.vertex[triangleIndices.z()]; - Vec3f hitObject = Vec3f((1 - u - v) * A + u * B + v * C); - Vec3f hitWorld = optixTransformPointFromObjectToWorldSpace(hitObject); - - int objectID = optixGetInstanceId(); + // Ray + const int beamIdx = optixGetLaunchIndex().x; + const unsigned beamSampleRayIdx = optixGetPayload_0(); + const unsigned circleIdx = (beamSampleRayIdx - 1) / MULTI_RETURN_BEAM_VERTICES; + const unsigned vertexIdx = (beamSampleRayIdx - 1) % MULTI_RETURN_BEAM_VERTICES; + const unsigned mrSamplesIndex = beamIdx * MULTI_RETURN_BEAM_SAMPLES + beamSampleRayIdx; + const Vec3f beamSampleOrigin = optixGetWorldRayOrigin(); + const int entityId = (int) optixGetInstanceId(); - Vec3f origin = decodePayloadVec3f({optixGetPayload_0(), optixGetPayload_1(), optixGetPayload_2()}); - - // TODO: Optimization - we can use inversesqrt here, which is one operation cheaper then sqrt. - float distance = sqrt(pow((hitWorld)[0] - (origin)[0], 2) + pow((hitWorld)[1] - (origin)[1], 2) + - pow((hitWorld)[2] - (origin)[2], 2)); + // Hitpoint + Vec3f hitObject = Vec3f((1 - u - v) * A + u * B + v * C); + const Vec3f hitWorldRaytraced = optixTransformPointFromObjectToWorldSpace(hitObject); + const float distance = (hitWorldRaytraced - beamSampleOrigin).length(); + const Vec3f hitWorldSeenBySensor = [&]() { + if (!ctx.doApplyDistortion) { + return hitWorldRaytraced; + } + Mat3x4f undistortedRay = ctx.rays[beamIdx] * makeBeamSampleRayTransform(ctx.beamHalfDivergence, circleIdx, vertexIdx); + Vec3f undistortedOrigin = undistortedRay * Vec3f{0, 0, 0}; + Vec3f undistortedDir = undistortedRay * Vec3f{0, 0, 1} - undistortedOrigin; + return undistortedOrigin + undistortedDir * distance; + }(); + // Early out for points that are too close to the sensor float minRange = ctx.rayRangesCount == 1 ? ctx.rayRanges[0].x() : ctx.rayRanges[optixGetLaunchIndex().x].x(); if (distance < minRange) { - saveNonHitRayResult(ctx.nearNonHitDistance); + ctx.mrSamples.isHit[mrSamplesIndex] = false; + if (beamSampleRayIdx == 0) { + saveNonHitRayResult(ctx.nearNonHitDistance); + } return; } - // Fix XYZ if distortion is applied (XYZ must be calculated in sensor coordinate frame) - if (ctx.doApplyDistortion) { - const int rayIdx = optixGetLaunchIndex().x; - Mat3x4f undistortedRay = ctx.rays[rayIdx]; - Vec3f undistortedOrigin = undistortedRay * Vec3f{0, 0, 0}; - Vec3f undistortedDir = undistortedRay * Vec3f{0, 0, 1} - undistortedOrigin; - hitWorld = undistortedOrigin + undistortedDir * distance; - } - // Normal vector and incident angle - Vec3f rayDir = (hitWorld - origin).normalized(); + Vec3f rayDir = optixGetWorldRayDirection(); const Vec3f wA = optixTransformPointFromObjectToWorldSpace(A); const Vec3f wB = optixTransformPointFromObjectToWorldSpace(B); const Vec3f wC = optixTransformPointFromObjectToWorldSpace(C); @@ -226,6 +177,14 @@ extern "C" __global__ void __closesthit__() intensity = tex2D(entityData.texture, uv[0], uv[1]); } + // Save sub-sampling results + ctx.mrSamples.isHit[mrSamplesIndex] = true; + ctx.mrSamples.xyz[mrSamplesIndex] = hitWorldSeenBySensor; + ctx.mrSamples.distance[mrSamplesIndex] = distance; + if (beamSampleRayIdx != 0) { + return; + } + Vec3f absPointVelocity{NAN}; Vec3f relPointVelocity{NAN}; float radialSpeed{NAN}; @@ -241,7 +200,7 @@ extern "C" __global__ void __closesthit__() // Then, we can connect marker dot in previous raytracing frame with its current position and obtain displacementFromTransformChange vector // Dividing displacementFromTransformChange by time elapsed from the previous raytracing frame yields velocity vector. Vec3f displacementVectorOrigin = entityData.prevFrameLocalToWorld * hitObject; - displacementFromTransformChange = hitWorld - displacementVectorOrigin; + displacementFromTransformChange = hitWorldRaytraced - displacementVectorOrigin; } // Some entities may have skinned meshes - in this case entity.vertexDisplacementSincePrevFrame will be non-null @@ -264,17 +223,96 @@ extern "C" __global__ void __closesthit__() Vec3f absPointVelocityInSensorFrame = ctx.rayOriginToWorld.rotation().inverse() * absPointVelocity; Vec3f relPointVelocityBasedOnSensorLinearVelocity = absPointVelocityInSensorFrame - ctx.sensorLinearVelocityXYZ; - Vec3f hitRays = ctx.rayOriginToWorld.inverse() * hitWorld; + Vec3f hitRays = ctx.rayOriginToWorld.inverse() * hitWorldRaytraced; Vec3f relPointVelocityBasedOnSensorAngularVelocity = Vec3f(.0f) - ctx.sensorAngularVelocityRPY.cross(hitRays); relPointVelocity = relPointVelocityBasedOnSensorLinearVelocity + relPointVelocityBasedOnSensorAngularVelocity; radialSpeed = hitRays.normalized().dot(relPointVelocity); } + saveRayResult(hitWorldSeenBySensor, distance, intensity, entityId, absPointVelocity, relPointVelocity, radialSpeed, + wNormal, incidentAngle); +} + +extern "C" __global__ void __anyhit__() {} + +// Helper functions implementations - saveRayResult(hitWorld, distance, intensity, objectID, absPointVelocity, relPointVelocity, radialSpeed, wNormal, - incidentAngle); +__device__ void shootSamplingRay(const Mat3x4f& ray, float maxRange, unsigned int beamSampleRayIdx) +{ + Vec3f origin = ray * Vec3f{0, 0, 0}; + Vec3f dir = ray * Vec3f{0, 0, 1} - origin; + const unsigned int flags = OPTIX_RAY_FLAG_DISABLE_ANYHIT; // TODO: try adding OPTIX_RAY_FLAG_CULL_BACK_FACING_TRIANGLES + optixTrace(ctx.scene, origin, dir, 0.0f, maxRange, 0.0f, OptixVisibilityMask(255), flags, 0, 1, 0, beamSampleRayIdx); } -extern "C" __global__ void __miss__() { saveNonHitRayResult(ctx.farNonHitDistance); } +__device__ Mat3x4f makeBeamSampleRayTransform(float halfDivergenceAngleRad, unsigned int circleIdx, unsigned int vertexIdx) +{ + if (ctx.beamHalfDivergence == 0.0f) { + return Mat3x4f::identity(); + } + const auto circleDivergence = halfDivergenceAngleRad * (1.0f - static_cast(circleIdx) / MULTI_RETURN_BEAM_CIRCLES); + auto vertexAngleStep = 2.0f * M_PIf / 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(vertexIdx) * vertexAngleStep) * // Second + Mat3x4f::rotationRad(circleDivergence, 0, 0); +} -extern "C" __global__ void __anyhit__() {} +__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(xyz, nonHitDistance, 0, RGL_ENTITY_INVALID_ID, Vec3f{NAN}, Vec3f{NAN}, 0.001f, Vec3f{NAN}, NAN); +} + +template +__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 beamIdx = optixGetLaunchIndex().x; + if (ctx.xyz != nullptr) { + // Return actual XYZ of the hit point or infinity vector. + ctx.xyz[beamIdx] = xyz; + } + if (ctx.isHit != nullptr) { + ctx.isHit[beamIdx] = isFinite; + } + if (ctx.rayIdx != nullptr) { + ctx.rayIdx[beamIdx] = beamIdx; + } + if (ctx.ringIdx != nullptr && ctx.ringIds != nullptr) { + ctx.ringIdx[beamIdx] = ctx.ringIds[beamIdx % ctx.ringIdsCount]; + } + if (ctx.distance != nullptr) { + ctx.distance[beamIdx] = distance; + } + if (ctx.intensity != nullptr) { + ctx.intensity[beamIdx] = intensity; + } + if (ctx.timestamp != nullptr) { + ctx.timestamp[beamIdx] = ctx.sceneTime; + } + if (ctx.entityId != nullptr) { + ctx.entityId[beamIdx] = isFinite ? objectID : RGL_ENTITY_INVALID_ID; + } + if (ctx.pointAbsVelocity != nullptr) { + ctx.pointAbsVelocity[beamIdx] = absVelocity; + } + if (ctx.pointRelVelocity != nullptr) { + ctx.pointRelVelocity[beamIdx] = relVelocity; + } + if (ctx.radialSpeed != nullptr) { + ctx.radialSpeed[beamIdx] = radialSpeed; + } + if (ctx.normal != nullptr) { + ctx.normal[beamIdx] = normal; + } + if (ctx.incidentAngle != nullptr) { + ctx.incidentAngle[beamIdx] = incidentAngle; + } +} diff --git a/src/graph/NodesCore.hpp b/src/graph/NodesCore.hpp index 22e59c1f..159f2f18 100644 --- a/src/graph/NodesCore.hpp +++ b/src/graph/NodesCore.hpp @@ -30,6 +30,7 @@ #include #include #include +#include struct FormatPointsNode : IPointsNodeSingleInput @@ -143,6 +144,38 @@ struct RaytraceNode : IPointsNode std::unordered_map fieldData; // All should be DeviceAsyncArray + struct MultiReturnFields + { + MultiReturnFields(StreamBoundObjectsManager& arrayMgr) + : isHit(DeviceAsyncArray::type>::create(arrayMgr)), + xyz(DeviceAsyncArray::type>::create(arrayMgr)), + distance(DeviceAsyncArray::type>::create(arrayMgr)) + {} + void resize(size_t size) + { + isHit->resize(size, false, false); + xyz->resize(size, false, false); + distance->resize(size, false, false); + } + MultiReturnPointers getPointers() + { + return MultiReturnPointers{ + .isHit = isHit->getWritePtr(), + .xyz = xyz->getWritePtr(), + .distance = distance->getWritePtr(), + }; + } + DeviceAsyncArray::type>::Ptr isHit; + DeviceAsyncArray::type>::Ptr xyz; + DeviceAsyncArray::type>::Ptr distance; + }; + + + MultiReturnFields mrSamples = MultiReturnFields{arrayMgr}; + MultiReturnFields mrFirst = MultiReturnFields{arrayMgr}; + MultiReturnFields mrLast = MultiReturnFields{arrayMgr}; + + template auto getPtrTo(); diff --git a/src/graph/RaytraceNode.cpp b/src/graph/RaytraceNode.cpp index e07a2e61..a463813d 100644 --- a/src/graph/RaytraceNode.cpp +++ b/src/graph/RaytraceNode.cpp @@ -65,6 +65,10 @@ void RaytraceNode::enqueueExecImpl() data->resize(raysNode->getRayCount(), false, false); } + mrSamples.resize(MULTI_RETURN_BEAM_SAMPLES * raysNode->getRayCount()); + mrFirst.resize(raysNode->getRayCount()); + mrLast.resize(raysNode->getRayCount()); + // Even though we are in graph thread here, we can access Scene class (see comment there) const Mat3x4f* raysPtr = raysNode->getRays()->asSubclass()->getReadPtr(); auto sceneAS = Scene::instance().getASLocked(); @@ -92,7 +96,7 @@ void RaytraceNode::enqueueExecImpl() .rayRangesCount = rayRanges.has_value() ? (*rayRanges)->getCount() : defaultRange->getCount(), .ringIds = ringIds.has_value() ? (*ringIds)->asSubclass()->getReadPtr() : nullptr, .ringIdsCount = ringIds.has_value() ? (*ringIds)->getCount() : 0, - .rayTimeOffsets = timeOffsets.has_value() ? (*timeOffsets)->asSubclass()->getReadPtr() : nullptr, + .rayTimeOffsetsMs = timeOffsets.has_value() ? (*timeOffsets)->asSubclass()->getReadPtr() : nullptr, .rayTimeOffsetsCount = timeOffsets.has_value() ? (*timeOffsets)->getCount() : 0, .scene = sceneAS, .sceneTime = Scene::instance().getTime().value_or(Time::zero()).asSeconds(), @@ -112,6 +116,8 @@ void RaytraceNode::enqueueExecImpl() .elevation = getPtrTo(), .normal = getPtrTo(), .incidentAngle = getPtrTo(), + .beamHalfDivergence = 0.1f, // TODO: provide API to set externally + .mrSamples = mrSamples.getPointers(), }; requestCtxDev->copyFrom(requestCtxHst); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 7131fcee..7316a0df 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -48,6 +48,7 @@ set(RGL_TEST_FILES src/synchronization/graphThreadSynchronization.cpp src/synchronization/testKernel.cu src/scene/incidentAngleTest.cpp + src/graph/multiReturnTest.cpp ) if (RGL_BUILD_ROS2_EXTENSION) diff --git a/test/src/graph/multiReturnTest.cpp b/test/src/graph/multiReturnTest.cpp new file mode 100644 index 00000000..e46e8f36 --- /dev/null +++ b/test/src/graph/multiReturnTest.cpp @@ -0,0 +1,40 @@ +#include +#include "math/Mat3x4f.hpp" +#include "helpers/lidarHelpers.hpp" +#include "RGLFields.hpp" +#include "helpers/testPointCloud.hpp" +#include "helpers/sceneHelpers.hpp" + +class GraphMultiReturn : public RGLTest +{}; + +TEST_F(GraphMultiReturn, basic) +{ + rgl_node_t useRays = nullptr, raytrace = nullptr, lidarPose = nullptr, compact = nullptr, yield = nullptr; + std::vector mrFields = {IS_HIT_I32, DISTANCE_F32, XYZ_VEC3_F32, INTENSITY_F32, ENTITY_ID_I32}; + + std::vector rays = {Mat3x4f::identity().toRGL()}; + rgl_mat3x4f lidarPoseTf = Mat3x4f::TRS({1, 2, 3}, {0, 30, 0}).toRGL(); + + rgl_entity_t cube = makeEntity(); + rgl_mat3x4f cubeTf = Mat3x4f::TRS({0, 0, 0}, {0, 0, 0}, {10, 10, 10}).toRGL(); + EXPECT_RGL_SUCCESS(rgl_entity_set_pose(cube, &cubeTf)); + + EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&useRays, rays.data(), rays.size())); + EXPECT_RGL_SUCCESS(rgl_node_rays_transform(&lidarPose, &lidarPoseTf)); + EXPECT_RGL_SUCCESS(rgl_node_raytrace(&raytrace, nullptr)); + EXPECT_RGL_SUCCESS(rgl_node_points_compact_by_field(&compact, RGL_FIELD_IS_HIT_I32)); + EXPECT_RGL_SUCCESS(rgl_node_points_yield(&yield, mrFields.data(), mrFields.size())); + + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(useRays, lidarPose)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(lidarPose, raytrace)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(raytrace, compact)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(compact, yield)); + + EXPECT_RGL_SUCCESS(rgl_graph_run(useRays)); + + TestPointCloud pc = TestPointCloud::createFromNode(yield, mrFields); + EXPECT_EQ(pc.getPointCount(), 1); + + EXPECT_RGL_SUCCESS(rgl_graph_destroy(useRays)); +} \ No newline at end of file