diff --git a/include/rgl/api/core.h b/include/rgl/api/core.h index 733b2766..1589d004 100644 --- a/include/rgl/api/core.h +++ b/include/rgl/api/core.h @@ -387,8 +387,9 @@ typedef enum : int32_t */ typedef enum : int32_t { - RGL_RETURN_TYPE_FIRST = 0, - RGL_RETURN_TYPE_LAST = 1, + RGL_RETURN_TYPE_NOT_DIVERGENT = 0, + RGL_RETURN_TYPE_FIRST = 1, + RGL_RETURN_TYPE_LAST = 2, } rgl_return_type_t; /** diff --git a/src/gpu/RaytraceRequestContext.hpp b/src/gpu/RaytraceRequestContext.hpp index 41cd85c0..bf8dd2cb 100644 --- a/src/gpu/RaytraceRequestContext.hpp +++ b/src/gpu/RaytraceRequestContext.hpp @@ -28,7 +28,7 @@ struct RaytraceRequestContext float nearNonHitDistance; float farNonHitDistance; - const Mat3x4f* rays; + const Mat3x4f* raysWorld; size_t rayCount; Mat3x4f rayOriginToWorld; diff --git a/src/gpu/nodeKernels.cu b/src/gpu/nodeKernels.cu index b2b12ce0..ba45d596 100644 --- a/src/gpu/nodeKernels.cu +++ b/src/gpu/nodeKernels.cu @@ -224,7 +224,7 @@ __global__ void kFilterGroundPoints(size_t pointCount, const Vec3f sensor_up_vec } __global__ void kProcessBeamSamplesFirstLast(size_t beamCount, int samplesPerBeam, MultiReturnPointers beamSamples, - MultiReturnPointers first, MultiReturnPointers last) + MultiReturnPointers first, MultiReturnPointers last, const Mat3x4f* beamsWorld) { LIMIT(beamCount); @@ -244,14 +244,16 @@ __global__ void kProcessBeamSamplesFirstLast(size_t beamCount, int samplesPerBea lastIdx = sampleIdx; } } + Vec3f beamOrigin = beamsWorld[beamIdx] * Vec3f{0, 0, 0}; + Vec3f beamDir = ((beamsWorld[beamIdx] * Vec3f{0, 0, 1}) - beamOrigin).normalized(); bool isHit = firstIdx >= 0; // Note that firstHit >= 0 implies lastHit >= 0 first.isHit[beamIdx] = isHit; last.isHit[beamIdx] = isHit; if (isHit) { - first.xyz[beamIdx] = beamSamples.xyz[beamIdx * samplesPerBeam + firstIdx]; first.distance[beamIdx] = beamSamples.distance[beamIdx * samplesPerBeam + firstIdx]; - last.xyz[beamIdx] = beamSamples.xyz[beamIdx * samplesPerBeam + lastIdx]; last.distance[beamIdx] = beamSamples.distance[beamIdx * samplesPerBeam + lastIdx]; + first.xyz[beamIdx] = beamOrigin + beamDir * first.distance[beamIdx]; + last.xyz[beamIdx] = beamOrigin + beamDir * last.distance[beamIdx]; } } @@ -329,7 +331,7 @@ void gpuRadarComputeEnergy(cudaStream_t stream, size_t count, float rayAzimuthSt } void gpuProcessBeamSamplesFirstLast(cudaStream_t stream, size_t beamCount, int samplesPerBeam, MultiReturnPointers beamSamples, - MultiReturnPointers first, MultiReturnPointers last) + MultiReturnPointers first, MultiReturnPointers last, const Mat3x4f* beamsWorld) { - run(kProcessBeamSamplesFirstLast, stream, beamCount, samplesPerBeam, beamSamples, first, last); + run(kProcessBeamSamplesFirstLast, stream, beamCount, samplesPerBeam, beamSamples, first, last, beamsWorld); } diff --git a/src/gpu/nodeKernels.hpp b/src/gpu/nodeKernels.hpp index b8002a1a..0703aa9f 100644 --- a/src/gpu/nodeKernels.hpp +++ b/src/gpu/nodeKernels.hpp @@ -53,4 +53,4 @@ void gpuRadarComputeEnergy(cudaStream_t stream, size_t count, float rayAzimuthSt const Field::type* hitDist, const Field::type* hitNorm, const Field::type* hitPos, Vector<3, thrust::complex>* outBUBRFactor); void gpuProcessBeamSamplesFirstLast(cudaStream_t stream, size_t beamCount, int samplesPerBeam, MultiReturnPointers beamSamples, - MultiReturnPointers first, MultiReturnPointers last); \ No newline at end of file + MultiReturnPointers first, MultiReturnPointers last, const Mat3x4f* beamWorld); \ No newline at end of file diff --git a/src/gpu/optixPrograms.cu b/src/gpu/optixPrograms.cu index 7a1c345f..459d1400 100644 --- a/src/gpu/optixPrograms.cu +++ b/src/gpu/optixPrograms.cu @@ -55,7 +55,7 @@ extern "C" __global__ void __raygen__() return; } - Mat3x4f ray = ctx.rays[rayIdx]; + Mat3x4f ray = ctx.raysWorld[rayIdx]; 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. @@ -137,12 +137,16 @@ extern "C" __global__ void __closesthit__() // Hitpoint Vec3f hitObject = Vec3f((1 - u - v) * A + u * B + v * C); const Vec3f hitWorldRaytraced = optixTransformPointFromObjectToWorldSpace(hitObject); - const float distance = (hitWorldRaytraced - beamSampleOrigin).length(); + const Vector<3, double> hwrd = hitWorldRaytraced; + const Vector<3, double> hso = beamSampleOrigin; + const double distance = (hwrd - hso).length(); const Vec3f hitWorldSeenBySensor = [&]() { if (!ctx.doApplyDistortion) { return hitWorldRaytraced; } - Mat3x4f undistortedRay = ctx.rays[beamIdx] * makeBeamSampleRayTransform(ctx.beamHalfDivergence, circleIdx, vertexIdx); + Mat3x4f sampleRayTf = beamSampleRayIdx == 0 ? Mat3x4f::identity() : + makeBeamSampleRayTransform(ctx.beamHalfDivergence, circleIdx, vertexIdx); + Mat3x4f undistortedRay = ctx.raysWorld[beamIdx] * sampleRayTf; Vec3f undistortedOrigin = undistortedRay * Vec3f{0, 0, 0}; Vec3f undistortedDir = undistortedRay * Vec3f{0, 0, 1} - undistortedOrigin; return undistortedOrigin + undistortedDir * distance; @@ -186,7 +190,6 @@ extern "C" __global__ void __closesthit__() // Save sub-sampling results ctx.mrSamples.isHit[mrSamplesIdx] = true; - ctx.mrSamples.xyz[mrSamplesIdx] = hitWorldSeenBySensor; ctx.mrSamples.distance[mrSamplesIdx] = distance; if (beamSampleRayIdx != 0) { return; @@ -267,7 +270,7 @@ __device__ Mat3x4f makeBeamSampleRayTransform(float halfDivergenceAngleRad, unsi __device__ void saveNonHitRayResult(float nonHitDistance) { - Mat3x4f ray = ctx.rays[optixGetLaunchIndex().x]; + Mat3x4f ray = ctx.raysWorld[optixGetLaunchIndex().x]; Vec3f origin = ray * Vec3f{0, 0, 0}; Vec3f dir = ray * Vec3f{0, 0, 1} - origin; Vec3f displacement = dir.normalized() * nonHitDistance; diff --git a/src/graph/NodesCore.hpp b/src/graph/NodesCore.hpp index ca3982b7..ccc36d28 100644 --- a/src/graph/NodesCore.hpp +++ b/src/graph/NodesCore.hpp @@ -212,6 +212,9 @@ struct MultiReturnSwitchNode : IPointsNodeSingleInput // Data getters IAnyArray::ConstPtr getFieldData(rgl_field_t field) override { + if (returnType == RGL_RETURN_TYPE_NOT_DIVERGENT) { + return rtxInput->getFieldData(field); + } return rtxInput->getFieldDataMultiReturn(field, returnType); } diff --git a/src/graph/RaytraceNode.cpp b/src/graph/RaytraceNode.cpp index 5194116a..6003da09 100644 --- a/src/graph/RaytraceNode.cpp +++ b/src/graph/RaytraceNode.cpp @@ -94,7 +94,7 @@ void RaytraceNode::enqueueExecImpl() .doApplyDistortion = doApplyDistortion, .nearNonHitDistance = nearNonHitDistance, .farNonHitDistance = farNonHitDistance, - .rays = raysPtr, + .raysWorld = raysPtr, .rayCount = raysNode->getRayCount(), .rayOriginToWorld = raysNode->getCumulativeRayTransfrom(), .rayRanges = rayRanges.has_value() ? (*rayRanges)->asSubclass()->getReadPtr() : @@ -133,8 +133,10 @@ void RaytraceNode::enqueueExecImpl() CHECK_OPTIX(optixLaunch(Optix::getOrCreate().pipeline, getStreamHandle(), pipelineArgsPtr, pipelineArgsSize, &sceneSBT, launchDims.x, launchDims.y, launchDims.y)); - gpuProcessBeamSamplesFirstLast(getStreamHandle(), raysNode->getRayCount(), MULTI_RETURN_BEAM_SAMPLES, - mrSamples.getPointers(), mrFirst.getPointers(), mrLast.getPointers()); + if (beamHalfDivergence > 0.0f) { + gpuProcessBeamSamplesFirstLast(getStreamHandle(), raysNode->getRayCount(), MULTI_RETURN_BEAM_SAMPLES, + mrSamples.getPointers(), mrFirst.getPointers(), mrLast.getPointers(), raysPtr); + } } IAnyArray::ConstPtr RaytraceNode::getFieldDataMultiReturn(rgl_field_t field, rgl_return_type_t type) @@ -144,7 +146,7 @@ IAnyArray::ConstPtr RaytraceNode::getFieldDataMultiReturn(rgl_field_t field, rgl case XYZ_VEC3_F32: return mrFirst.xyz; case DISTANCE_F32: return mrFirst.distance; case IS_HIT_I32: return mrFirst.isHit; - default: throw InvalidPipeline(fmt::format("Multi-Return not supported for this field ({})", toString(field))); + default: return getFieldData(field); } } if (type == RGL_RETURN_TYPE_LAST) { @@ -152,7 +154,7 @@ IAnyArray::ConstPtr RaytraceNode::getFieldDataMultiReturn(rgl_field_t field, rgl case XYZ_VEC3_F32: return mrLast.xyz; case DISTANCE_F32: return mrLast.distance; case IS_HIT_I32: return mrLast.isHit; - default: throw InvalidPipeline(fmt::format("Multi-Return not supported for this field ({})", toString(field))); + default: return getFieldData(field); } } throw InvalidPipeline(fmt::format("Unknown multi-return type ({})", type)); diff --git a/test/include/helpers/geometryData.hpp b/test/include/helpers/geometryData.hpp index a3c0d0b6..59037617 100644 --- a/test/include/helpers/geometryData.hpp +++ b/test/include/helpers/geometryData.hpp @@ -3,6 +3,7 @@ #include static constexpr float CUBE_HALF_EDGE = 1.0; +static constexpr float CUBE_EDGE = 2.0; static rgl_vec3f cubeVertices[] = { {-1, -1, -1}, // 0 diff --git a/test/include/helpers/testPointCloud.hpp b/test/include/helpers/testPointCloud.hpp index e2e3e5b9..397541f6 100644 --- a/test/include/helpers/testPointCloud.hpp +++ b/test/include/helpers/testPointCloud.hpp @@ -241,7 +241,7 @@ class TestPointCloud } template - std::vector::type> getFieldValues() + std::vector::type> getFieldValues() const { int fieldIndex = std::find(fields.begin(), fields.end(), T) - fields.begin(); @@ -252,7 +252,7 @@ class TestPointCloud for (int i = 0; i < getPointCount(); i++) { fieldValues.emplace_back( - *reinterpret_cast::type*>(data.data() + i * getPointByteSize() + offset)); + *reinterpret_cast::type*>(data.data() + i * getPointByteSize() + offset)); } return fieldValues; diff --git a/test/src/graph/multiReturnTest.cpp b/test/src/graph/multiReturnTest.cpp index e46e8f36..16eb8ceb 100644 --- a/test/src/graph/multiReturnTest.cpp +++ b/test/src/graph/multiReturnTest.cpp @@ -1,40 +1,333 @@ #include -#include "math/Mat3x4f.hpp" #include "helpers/lidarHelpers.hpp" -#include "RGLFields.hpp" #include "helpers/testPointCloud.hpp" #include "helpers/sceneHelpers.hpp" +#include "RGLFields.hpp" +#include "math/Mat3x4f.hpp" + +using namespace std::chrono_literals; + +#if RGL_BUILD_ROS2_EXTENSION +#include +#endif + 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 + + std::vector 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}), + Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(9.7f)}, {0.0f, -13.0f, 0.0f}), + Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(-2.2f)}, {0.0f, +3.0f, 0.0f}), + Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(8.1f)}, {0.0f, -11.0f, 0.0f}), + Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(-3.7f)}, {0.0f, +5.0f, 0.0f}), + Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(6.6f)}, {0.0f, -9.0f, 0.0f}), + Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(-5.1f)}, {0.0f, +7.0f, 0.0f}), + Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(5.1f)}, {0.0f, -7.0f, 0.0f}), + Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(-6.6f)}, {0.0f, +9.0f, 0.0f}), + Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(3.7f)}, {0.0f, -5.0f, 0.0f}), + Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(-8.1f)}, {0.0f, +11.0f, 0.0f}), + Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(2.2f)}, {0.0f, -3.0f, 0.0f}), + Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(-9.7f)}, {0.0f, +13.0f, 0.0f}), + Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(0.7f)}, {0.0f, -1.0f, 0.0f}), + Mat3x4f::TRS({0.0f, 0.0f, mmToMeters(-11.2f)}, {0.0f, +15.0f, 0.0f})}; + + const std::vector fields = {XYZ_VEC3_F32, IS_HIT_I32, DISTANCE_F32 /*, INTENSITY_F32, ENTITY_ID_I32*/}; + + rgl_node_t rays = nullptr, mrRays = nullptr, cameraRays = nullptr, transform = nullptr, mrTransform = nullptr, + cameraTransform = nullptr, raytrace = nullptr, mrRaytrace = nullptr, cameraRaytrace = nullptr, mrFirst = nullptr, + mrLast = nullptr, format = nullptr, mrFormatFirst = nullptr, mrFormatLast = nullptr, cameraFormat = nullptr, + publish = nullptr, cameraPublish = nullptr, mrPublishFirst = nullptr, mrPublishLast = nullptr, + compactFirst = nullptr, compactLast = nullptr; + + void constructMRGraph(const std::vector& raysTf, const rgl_mat3x4f& lidarPose, const float beamDivAngle, + 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_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)); + EXPECT_RGL_SUCCESS(rgl_node_points_compact_by_field(&compactLast, RGL_FIELD_IS_HIT_I32)); + EXPECT_RGL_SUCCESS(rgl_node_points_format(&mrFormatFirst, fields.data(), fields.size())); + EXPECT_RGL_SUCCESS(rgl_node_points_format(&mrFormatLast, fields.data(), fields.size())); +#if RGL_BUILD_ROS2_EXTENSION + if (withPublish) { + EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&mrPublishFirst, "MRTest_First", "world")); + EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&mrPublishLast, "MRTest_Last", "world")); + } +#else + if (withPublish) { + GTEST_SKIP() << "Publishing is not supported without ROS2 extension. Skippping the test."; + } +#endif + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(mrRays, mrTransform)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(mrTransform, mrRaytrace)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(mrRaytrace, mrFirst)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(mrRaytrace, mrLast)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(mrFirst, compactFirst)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(mrLast, compactLast)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(compactFirst, mrFormatFirst)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(compactLast, mrFormatLast)); + if (withPublish) { + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(mrFormatFirst, mrPublishFirst)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(mrFormatLast, mrPublishLast)); + } + } + + void constructCameraGraph(const rgl_mat3x4f& cameraPose) + { +#if RGL_BUILD_ROS2_EXTENSION + const std::vector cameraRayTf = makeLidar3dRays(360.0f, 180.0f, 1.0f, 1.0f); + EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&cameraRays, cameraRayTf.data(), cameraRayTf.size())); + EXPECT_RGL_SUCCESS(rgl_node_rays_transform(&cameraTransform, &cameraPose)); + EXPECT_RGL_SUCCESS(rgl_node_raytrace(&cameraRaytrace, nullptr)); + EXPECT_RGL_SUCCESS(rgl_node_points_format(&cameraFormat, fields.data(), fields.size())); + EXPECT_RGL_SUCCESS(rgl_node_points_ros2_publish(&cameraPublish, "MRTest_Camera", "world")); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(cameraRays, cameraTransform)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(cameraTransform, cameraRaytrace)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(cameraRaytrace, cameraFormat)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(cameraFormat, cameraPublish)); +#else + GTEST_SKIP() << "Publishing is not supported without ROS2 extension. Skippping the test with camera."; +#endif + } + + void spawnStairsOnScene(const float stepWidth, const float stepHeight, const float stepDepth, const float stairsBaseHeight, + const Vec3f& stairsTranslation) const + { + const Vec3f cubeHalfEdgeScaled{CUBE_HALF_EDGE * stepDepth / CUBE_EDGE, CUBE_HALF_EDGE * stepWidth / CUBE_EDGE, + CUBE_HALF_EDGE * stepHeight / CUBE_EDGE}; + + const auto firstCubeTf = + Mat3x4f::translation(stairsTranslation) * + Mat3x4f::TRS({cubeHalfEdgeScaled.x(), 0.0f, -cubeHalfEdgeScaled.z() + stairsBaseHeight + stepHeight}, + {0.0f, 0.0f, 0.0f}, {stepDepth / CUBE_EDGE, stepWidth / CUBE_EDGE, stepHeight / CUBE_EDGE}); + spawnCubeOnScene(firstCubeTf); + spawnCubeOnScene(Mat3x4f::translation(stepDepth, 0.0f, stepHeight) * firstCubeTf); + spawnCubeOnScene(Mat3x4f::translation(2 * stepDepth, 0.0f, 2 * stepHeight) * firstCubeTf); + } + + constexpr float mmToMeters(float mm) const { return mm * 0.001f; } +}; + +/** + * This test verifies the accuracy of multiple return handling for the data specified for LiDAR VLP16 + * by firing a single beam into a cube and making sure the first and last hits are correctly calculated. + */ +TEST_F(GraphMultiReturn, vlp16_data_compare) +{ + // Lidar + const std::vector raysTf{Mat3x4f::TRS({0.0f, 0.0f, 0.0f}, {90.0f, 0.0f, -90.0f}).toRGL()}; + const float lidarCubeFaceDist = vlp16LidarObjectDistance; + const float lidarCubeCenterDist = lidarCubeFaceDist + CUBE_HALF_EDGE; + const auto lidarTransl = Vec3f{lidarCubeCenterDist, 0.0f, 0.0f}; + const rgl_mat3x4f lidarPose = Mat3x4f::TRS(lidarTransl).toRGL(); + + // Scene + spawnCubeOnScene(Mat3x4f::identity()); + + // VLP16 horizontal beam divergence in rads + const float beamDivAngle = vlp16LidarHBeamDivergence; + constructMRGraph(raysTf, lidarPose, beamDivAngle); + + EXPECT_RGL_SUCCESS(rgl_graph_run(mrRays)); + + // Verify the output + const float epsilon = 1e-4f; + + const auto mrFirstOutPointcloud = TestPointCloud::createFromNode(mrFormatFirst, fields); + const auto mrFirstIsHits = mrFirstOutPointcloud.getFieldValues(); + const auto mrFirstPoints = mrFirstOutPointcloud.getFieldValues(); + const auto mrFirstDistances = mrFirstOutPointcloud.getFieldValues(); + const auto expectedFirstPoint = Vec3f{CUBE_HALF_EDGE, 0.0f, 0.0f}; + EXPECT_EQ(mrFirstOutPointcloud.getPointCount(), raysTf.size()); + EXPECT_TRUE(mrFirstIsHits.at(0)); + EXPECT_NEAR(mrFirstDistances.at(0), lidarCubeFaceDist, epsilon); + EXPECT_NEAR(mrFirstPoints.at(0).x(), expectedFirstPoint.x(), epsilon); + EXPECT_NEAR(mrFirstPoints.at(0).y(), expectedFirstPoint.y(), epsilon); + EXPECT_NEAR(mrFirstPoints.at(0).z(), expectedFirstPoint.z(), epsilon); + + const auto mrLastOutPointcloud = TestPointCloud::createFromNode(mrFormatLast, fields); + const auto mrLastIsHits = mrLastOutPointcloud.getFieldValues(); + const auto mrLastPoints = mrLastOutPointcloud.getFieldValues(); + const auto mrLastDistances = mrLastOutPointcloud.getFieldValues(); + const float expectedDiameter = vlp16LidarHBeamDiameter; + const auto expectedLastDistance = static_cast(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}; + EXPECT_EQ(mrLastOutPointcloud.getPointCount(), raysTf.size()); + EXPECT_TRUE(mrLastIsHits.at(0)); + EXPECT_NEAR(mrLastDistances.at(0), expectedLastDistance, epsilon); + EXPECT_NEAR(mrLastPoints.at(0).x(), expectedLastPoint.x(), epsilon); + EXPECT_NEAR(mrLastPoints.at(0).y(), expectedLastPoint.y(), epsilon); + EXPECT_NEAR(mrLastPoints.at(0).z(), expectedLastPoint.z(), epsilon); +} -TEST_F(GraphMultiReturn, basic) +#if RGL_BUILD_ROS2_EXTENSION +/** + * This test verifies the performance of the multiple return feature in a dynamic scene + * 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) { - 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}; + /* + * gap + * ________ <----> + * | | | + * | | | + * |________| | + * | + * X + * LIDAR + */ - std::vector rays = {Mat3x4f::identity().toRGL()}; - rgl_mat3x4f lidarPoseTf = Mat3x4f::TRS({1, 2, 3}, {0, 30, 0}).toRGL(); + GTEST_SKIP(); // Comment to run the test - 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)); + // Lidar + const std::vector raysTf{Mat3x4f::TRS({0.0f, 0.0f, 0.0f}, {90.0f, 0.0f, 90.0f}).toRGL()}; + const float lidarCubeFaceDist = 100.0f; + const float lidarCubeCenterDist = lidarCubeFaceDist + CUBE_HALF_EDGE * 2; + const Vec3f lidarTransl = {-lidarCubeCenterDist, 3.0f, 3.0f}; + const rgl_mat3x4f lidarPose = Mat3x4f::translation(lidarTransl).toRGL(); - EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&useRays, rays.data(), rays.size())); - EXPECT_RGL_SUCCESS(rgl_node_rays_transform(&lidarPose, &lidarPoseTf)); + // Scene + const Vec2f gapRange = {0.001f, 0.5f}; + const std::vector entitiesTransforms = { + Mat3x4f::TRS(Vec3f{-5.0f, lidarTransl.y() + gapRange.x() + CUBE_HALF_EDGE, lidarTransl.z()}), + Mat3x4f::TRS(Vec3f{0.0f, lidarTransl.y(), lidarTransl.z()}, {0, 0, 0}, {2, 2, 2})}; + std::vector entities = {spawnCubeOnScene(entitiesTransforms.at(0)), + spawnCubeOnScene(entitiesTransforms.at(1))}; + + // Lidar with MR + const float beamDivAngle = 0.003f; + constructMRGraph(raysTf, lidarPose, beamDivAngle, true); + + // Lidar 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_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_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)); + + // Camera + const rgl_mat3x4f cameraPose = Mat3x4f::TRS(Vec3f{-8.0f, 1.0f, 5.0f}, {90.0f, 30.0f, -70.0f}).toRGL(); + constructCameraGraph(cameraPose); + + int frameId = 0; + while (true) { + + const auto newPose = (entitiesTransforms.at(0) * + Mat3x4f::translation(0.0f, std::abs(std::sin(frameId * 0.05f)) * gapRange.y(), 0.0f)) + .toRGL(); + EXPECT_RGL_SUCCESS(rgl_entity_set_pose(entities.at(0), &newPose)); + + ASSERT_RGL_SUCCESS(rgl_graph_run(cameraRays)); + ASSERT_RGL_SUCCESS(rgl_graph_run(rays)); + ASSERT_RGL_SUCCESS(rgl_graph_run(mrRays)); + + std::this_thread::sleep_for(50ms); + frameId += 1; + } +} +#endif + +#if RGL_BUILD_ROS2_EXTENSION +/** + * This test verifies how changing the beam divergence affects the results obtained with the multi return feature enabled. + * Three cubes arranged in the shape of a stairs are placed on the scene. LiDAR aims the only ray at the center of the middle “stair", + * during the test the beam divergence angle is increased/decreased periodically. + */ +TEST_F(GraphMultiReturn, stairs) +{ + /* + * ____ ^ + * ____| | Z+ + * ____| middle step | + * | ----> X+ + */ + + GTEST_SKIP(); // Comment to run the test + + // Scene + const float stairsBaseHeight = 0.0f; + const float stepWidth = 1.0f; + const float stepHeight = vlp16LidarHBeamDiameter + 0.1f; + const float stepDepth = 0.8f; + const Vec3f stairsTranslation{2.0f, 0.0f, 0.0f}; + + spawnStairsOnScene(stepWidth, stepHeight, stepDepth, stairsBaseHeight, stairsTranslation); + + // Lidar + const std::vector raysTf{Mat3x4f::TRS({0.0f, 0.0f, 0.0f}, {90.0f, 0.0f, 90.0f}).toRGL()}; + + const float lidarMiddleStepDist = 1.5f * vlp16LidarObjectDistance; + const Vec3f lidarTransl{-lidarMiddleStepDist + stepDepth, 0.0f, stepHeight * 1.5f}; + + const rgl_mat3x4f lidarPose{(Mat3x4f::translation(lidarTransl + stairsTranslation)).toRGL()}; + + // Lidar with MR + const float beamDivAngle = vlp16LidarHBeamDivergence; + constructMRGraph(raysTf, lidarPose, beamDivAngle, true); + + // Camera + rgl_mat3x4f cameraPose = Mat3x4f::translation({0.0f, -1.5f, stepHeight * 3 + 1.0f}).toRGL(); + constructCameraGraph(cameraPose); + ASSERT_RGL_SUCCESS(rgl_graph_run(cameraRays)); + + 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)); + + ASSERT_RGL_SUCCESS(rgl_graph_run(mrRaytrace)); + + std::this_thread::sleep_for(100ms); + frameId += 1; + } +} + +/** + * This test verifies the performance of the multi return feature when releasing multiple ray beams at once. + */ +TEST_F(GraphMultiReturn, multiple_ray_beams) +{ + GTEST_SKIP(); // Comment to run the test + + // Scene + spawnCubeOnScene(Mat3x4f::TRS({0.0f, 0.0f, 0.0f}, {0.0f, 0.0f, 0.0f}, {2.0f, 2.0f, 2.0f})); - 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)); + // Camera + constructCameraGraph(Mat3x4f::identity().toRGL()); - EXPECT_RGL_SUCCESS(rgl_graph_run(useRays)); + // Lidar with MR + const int horizontalSteps = 10; + const auto resolution = 360.0f / horizontalSteps; + std::vector vlp16RaysTf; + vlp16RaysTf.reserve(horizontalSteps * vlp16Channels.size()); - TestPointCloud pc = TestPointCloud::createFromNode(yield, mrFields); - EXPECT_EQ(pc.getPointCount(), 1); + for (int i = 0; i < horizontalSteps; ++i) { + for (const auto& velodyneVLP16Channel : vlp16Channels) { + vlp16RaysTf.emplace_back((Mat3x4f::rotationDeg(0.0f, 90.0f, resolution * i) * velodyneVLP16Channel).toRGL()); + } + } + const rgl_mat3x4f lidarPose = Mat3x4f::identity().toRGL(); + const float beamDivAngle = vlp16LidarHBeamDivergence; + constructMRGraph(vlp16RaysTf, lidarPose, beamDivAngle, true); - EXPECT_RGL_SUCCESS(rgl_graph_destroy(useRays)); -} \ No newline at end of file + ASSERT_RGL_SUCCESS(rgl_graph_run(cameraRays)); + ASSERT_RGL_SUCCESS(rgl_graph_run(mrRays)); +} +#endif