From df3c09f0eff246bd2ad9d1830a8460cb28a46492 Mon Sep 17 00:00:00 2001 From: Piotr Rybicki Date: Tue, 28 May 2024 13:10:42 +0200 Subject: [PATCH 01/10] Add RGL_RETURN_TYPE_NOT_DIVERGENT --- include/rgl/api/core.h | 5 +++-- src/graph/NodesCore.hpp | 3 +++ 2 files changed, 6 insertions(+), 2 deletions(-) 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/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); } From e9e5a83fee3fd8c8b5d2e0f4c2e5fb16114b50ff Mon Sep 17 00:00:00 2001 From: Piotr Rybicki Date: Wed, 29 May 2024 13:56:49 +0200 Subject: [PATCH 02/10] Compute XYZ based on raydir and distance --- src/gpu/RaytraceRequestContext.hpp | 2 +- src/gpu/nodeKernels.cu | 13 ++++++++----- src/gpu/nodeKernels.hpp | 2 +- src/gpu/optixPrograms.cu | 8 ++++---- src/graph/RaytraceNode.cpp | 8 ++++---- 5 files changed, 18 insertions(+), 15 deletions(-) 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..6fccf530 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,13 +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}).normalized(); + printf("%f\n", beamDir.length()); 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.xyz[beamIdx] = beamOrigin + beamDir * first.distance[beamIdx]; first.distance[beamIdx] = beamSamples.distance[beamIdx * samplesPerBeam + firstIdx]; - last.xyz[beamIdx] = beamSamples.xyz[beamIdx * samplesPerBeam + lastIdx]; + last.xyz[beamIdx] = beamOrigin + beamDir * last.distance[beamIdx]; last.distance[beamIdx] = beamSamples.distance[beamIdx * samplesPerBeam + lastIdx]; } } @@ -329,7 +332,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..7a50215a 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. @@ -142,7 +142,8 @@ extern "C" __global__ void __closesthit__() if (!ctx.doApplyDistortion) { return hitWorldRaytraced; } - Mat3x4f undistortedRay = ctx.rays[beamIdx] * makeBeamSampleRayTransform(ctx.beamHalfDivergence, circleIdx, vertexIdx); + Mat3x4f undistortedRay = ctx.raysWorld[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; @@ -186,7 +187,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 +267,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/RaytraceNode.cpp b/src/graph/RaytraceNode.cpp index 5194116a..552a8803 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() : @@ -134,7 +134,7 @@ void RaytraceNode::enqueueExecImpl() launchDims.x, launchDims.y, launchDims.y)); gpuProcessBeamSamplesFirstLast(getStreamHandle(), raysNode->getRayCount(), MULTI_RETURN_BEAM_SAMPLES, - mrSamples.getPointers(), mrFirst.getPointers(), mrLast.getPointers()); + mrSamples.getPointers(), mrFirst.getPointers(), mrLast.getPointers(), raysPtr); } IAnyArray::ConstPtr RaytraceNode::getFieldDataMultiReturn(rgl_field_t field, rgl_return_type_t type) @@ -144,7 +144,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 +152,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)); From 340de40358ae9682a0a332517141050e8f6b0a96 Mon Sep 17 00:00:00 2001 From: Piotr Rybicki Date: Wed, 29 May 2024 17:00:58 +0200 Subject: [PATCH 03/10] Fix XYZ computation, run beam sample processing conditionally --- src/gpu/nodeKernels.cu | 7 +++---- src/gpu/optixPrograms.cu | 5 +++-- src/graph/RaytraceNode.cpp | 6 ++++-- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/gpu/nodeKernels.cu b/src/gpu/nodeKernels.cu index 6fccf530..ba45d596 100644 --- a/src/gpu/nodeKernels.cu +++ b/src/gpu/nodeKernels.cu @@ -245,16 +245,15 @@ __global__ void kProcessBeamSamplesFirstLast(size_t beamCount, int samplesPerBea } } Vec3f beamOrigin = beamsWorld[beamIdx] * Vec3f{0, 0, 0}; - Vec3f beamDir = (beamsWorld[beamIdx] * Vec3f{0, 0, 1}).normalized(); - printf("%f\n", beamDir.length()); + 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] = beamOrigin + beamDir * first.distance[beamIdx]; first.distance[beamIdx] = beamSamples.distance[beamIdx * samplesPerBeam + firstIdx]; - last.xyz[beamIdx] = beamOrigin + beamDir * last.distance[beamIdx]; last.distance[beamIdx] = beamSamples.distance[beamIdx * samplesPerBeam + lastIdx]; + first.xyz[beamIdx] = beamOrigin + beamDir * first.distance[beamIdx]; + last.xyz[beamIdx] = beamOrigin + beamDir * last.distance[beamIdx]; } } diff --git a/src/gpu/optixPrograms.cu b/src/gpu/optixPrograms.cu index 7a50215a..7073842f 100644 --- a/src/gpu/optixPrograms.cu +++ b/src/gpu/optixPrograms.cu @@ -142,8 +142,9 @@ extern "C" __global__ void __closesthit__() if (!ctx.doApplyDistortion) { return hitWorldRaytraced; } - Mat3x4f undistortedRay = ctx.raysWorld[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; diff --git a/src/graph/RaytraceNode.cpp b/src/graph/RaytraceNode.cpp index 552a8803..6003da09 100644 --- a/src/graph/RaytraceNode.cpp +++ b/src/graph/RaytraceNode.cpp @@ -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(), raysPtr); + 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) From 02683e6d0be1394a5024adcae5d2270ce92b59d1 Mon Sep 17 00:00:00 2001 From: nebraszka Date: Thu, 23 May 2024 14:04:57 +0200 Subject: [PATCH 04/10] Add MR test --- test/include/helpers/testPointCloud.hpp | 4 +- test/src/graph/multiReturnTest.cpp | 202 +++++++++++++++++++++--- 2 files changed, 180 insertions(+), 26 deletions(-) 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..8d949da8 100644 --- a/test/src/graph/multiReturnTest.cpp +++ b/test/src/graph/multiReturnTest.cpp @@ -1,40 +1,194 @@ #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: + 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) + { + 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)); + } + } +}; + +/** + * 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 = 100.0f; + const float lidarCubeCenterDist = lidarCubeFaceDist + CUBE_HALF_EDGE; + const rgl_mat3x4f lidarPose = Mat3x4f::TRS({lidarCubeCenterDist, 0.0f, 0.0f}).toRGL(); + + // Scene + spawnCubeOnScene(Mat3x4f::identity()); + + // VLP16 horizontal beam divergence in rads + const float beamDivAngle = 0.003f; + constructMRGraph(raysTf, lidarPose, beamDivAngle, false); + + EXPECT_RGL_SUCCESS(rgl_graph_run(mrRays)); + + // Verify the output + const float epsilon = 1e-5f; + + 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(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); + EXPECT_NEAR(mrFirstDistances.at(0), lidarCubeFaceDist, epsilon); -TEST_F(GraphMultiReturn, basic) + const float expectedDiameter = 0.2868f; // VLP16 beam horizontal diameter at 100m + + const auto mrLastOutPointcloud = TestPointCloud::createFromNode(mrFormatLast, fields); + const auto mrLastIsHits = mrLastOutPointcloud.getFieldValues(); + const auto mrLastPoints = mrLastOutPointcloud.getFieldValues(); + const auto mrLastDistances = mrLastOutPointcloud.getFieldValues(); + const auto expectedLastPoint = Vec3f{CUBE_HALF_EDGE, 0.0f, -expectedDiameter / 2}; + EXPECT_EQ(mrLastOutPointcloud.getPointCount(), raysTf.size()); + EXPECT_TRUE(mrLastIsHits.at(0)); + 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(), 0.01f); +} + +#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 + */ + + GTEST_SKIP(); // Comment to run the test - std::vector rays = {Mat3x4f::identity().toRGL()}; - rgl_mat3x4f lidarPoseTf = Mat3x4f::TRS({1, 2, 3}, {0, 30, 0}).toRGL(); + // 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(); - 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)); + // 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))}; - EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&useRays, rays.data(), rays.size())); - EXPECT_RGL_SUCCESS(rgl_node_rays_transform(&lidarPose, &lidarPoseTf)); + // Camera + const rgl_mat3x4f cameraPose = Mat3x4f::TRS(Vec3f{-8.0f, 1.0f, 5.0f}, {90.0f, 30.0f, -70.0f}).toRGL(); + const std::vector cameraRayTf = makeLidar3dRays(360.0f, 180.0f, 0.5f, 0.5f); + 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_PairsOfCubes_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)); + + // 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_PairsOfCubes", "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)); + + // Lidar with MR + const float beamDivAngle = 0.003f; + constructMRGraph(raysTf, lidarPose, beamDivAngle, true); - 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)); + int frameId = 0; + while (true) { - EXPECT_RGL_SUCCESS(rgl_graph_run(useRays)); + 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)); - TestPointCloud pc = TestPointCloud::createFromNode(yield, mrFields); - EXPECT_EQ(pc.getPointCount(), 1); + ASSERT_RGL_SUCCESS(rgl_graph_run(cameraRays)); + ASSERT_RGL_SUCCESS(rgl_graph_run(rays)); + ASSERT_RGL_SUCCESS(rgl_graph_run(mrRays)); - EXPECT_RGL_SUCCESS(rgl_graph_destroy(useRays)); -} \ No newline at end of file + std::this_thread::sleep_for(50ms); + frameId += 1; + } +} +#endif \ No newline at end of file From 9d5800521ecbfa5a043a4eb92c27a5cbf491dfe7 Mon Sep 17 00:00:00 2001 From: nebraszka Date: Tue, 28 May 2024 21:19:58 +0200 Subject: [PATCH 05/10] Add stairs test --- src/gpu/optixPrograms.cu | 4 +- test/include/helpers/geometryData.hpp | 1 + test/src/graph/multiReturnTest.cpp | 132 +++++++++++++++++++++----- 3 files changed, 113 insertions(+), 24 deletions(-) diff --git a/src/gpu/optixPrograms.cu b/src/gpu/optixPrograms.cu index 7073842f..459d1400 100644 --- a/src/gpu/optixPrograms.cu +++ b/src/gpu/optixPrograms.cu @@ -137,7 +137,9 @@ 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; 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/src/graph/multiReturnTest.cpp b/test/src/graph/multiReturnTest.cpp index 8d949da8..186e370b 100644 --- a/test/src/graph/multiReturnTest.cpp +++ b/test/src/graph/multiReturnTest.cpp @@ -15,6 +15,11 @@ using namespace std::chrono_literals; class GraphMultiReturn : public RGLTest { protected: + // VLP16 data + const float vlp16LidarObjectDistance = 100.0f; + const float vlp16LidarHBeamDivergence = 0.003f; // VLP16 horizontal beam divergence in rads + const float vlp16LidarHBeamDiameter = 0.2868f; // VLP16 beam horizontal diameter at 100m + 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, @@ -24,7 +29,7 @@ class GraphMultiReturn : public RGLTest compactFirst = nullptr, compactLast = nullptr; void constructMRGraph(const std::vector& raysTf, const rgl_mat3x4f& lidarPose, const float beamDivAngle, - bool withPublish) + 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)); @@ -59,6 +64,39 @@ class GraphMultiReturn : public RGLTest EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(mrFormatLast, mrPublishLast)); } } + + void constructCameraGraph(const rgl_mat3x4f& cameraPose, const char* topic) + { +#ifdef RGL_BUILD_ROS2_EXTENSION + const std::vector cameraRayTf = makeLidar3dRays(360.0f, 180.0f, 0.5f, 0.5f); + 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, topic, "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); + } }; /** @@ -68,8 +106,8 @@ class GraphMultiReturn : public RGLTest 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 = 100.0f; + 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 rgl_mat3x4f lidarPose = Mat3x4f::TRS({lidarCubeCenterDist, 0.0f, 0.0f}).toRGL(); @@ -77,8 +115,8 @@ TEST_F(GraphMultiReturn, VLP16_data_compare) spawnCubeOnScene(Mat3x4f::identity()); // VLP16 horizontal beam divergence in rads - const float beamDivAngle = 0.003f; - constructMRGraph(raysTf, lidarPose, beamDivAngle, false); + const float beamDivAngle = vlp16LidarHBeamDivergence; + constructMRGraph(raysTf, lidarPose, beamDivAngle); EXPECT_RGL_SUCCESS(rgl_graph_run(mrRays)); @@ -97,7 +135,7 @@ TEST_F(GraphMultiReturn, VLP16_data_compare) EXPECT_NEAR(mrFirstPoints.at(0).z(), expectedFirstPoint.z(), epsilon); EXPECT_NEAR(mrFirstDistances.at(0), lidarCubeFaceDist, epsilon); - const float expectedDiameter = 0.2868f; // VLP16 beam horizontal diameter at 100m + const float expectedDiameter = vlp16LidarHBeamDiameter; const auto mrLastOutPointcloud = TestPointCloud::createFromNode(mrFormatLast, fields); const auto mrLastIsHits = mrLastOutPointcloud.getFieldValues(); @@ -133,7 +171,7 @@ TEST_F(GraphMultiReturn, pairs_of_cubes_in_motion) GTEST_SKIP(); // Comment to run the test // Lidar - const std::vector raysTf {Mat3x4f::TRS({0.0f, 0.0f, 0.0f}, {90.0f, 0.0f, 90.0f}).toRGL()}; + 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}; @@ -147,18 +185,10 @@ TEST_F(GraphMultiReturn, pairs_of_cubes_in_motion) std::vector entities = {spawnCubeOnScene(entitiesTransforms.at(0)), spawnCubeOnScene(entitiesTransforms.at(1))}; - // Camera - const rgl_mat3x4f cameraPose = Mat3x4f::TRS(Vec3f{-8.0f, 1.0f, 5.0f}, {90.0f, 30.0f, -70.0f}).toRGL(); - const std::vector cameraRayTf = makeLidar3dRays(360.0f, 180.0f, 0.5f, 0.5f); - 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_PairsOfCubes_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)); + // Lidar with MR + const float beamDivAngle = 0.003f; + rgl_node_t mrRays = nullptr, mrRaytrace = nullptr, mrFormatFirst = nullptr, mrFormatLast = nullptr; + constructMRGraph(raysTf, lidarPose, beamDivAngle, true); // Lidar without MR EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&rays, raysTf.data(), raysTf.size())); @@ -171,9 +201,9 @@ TEST_F(GraphMultiReturn, pairs_of_cubes_in_motion) EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(raytrace, format)); EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(format, publish)); - // Lidar with MR - const float beamDivAngle = 0.003f; - constructMRGraph(raysTf, lidarPose, beamDivAngle, true); + // Camera + const rgl_mat3x4f cameraPose = Mat3x4f::TRS(Vec3f{-8.0f, 1.0f, 5.0f}, {90.0f, 30.0f, -70.0f}).toRGL(); + constructCameraGraph(cameraPose, "MRTest_Camera"); int frameId = 0; while (true) { @@ -191,4 +221,60 @@ TEST_F(GraphMultiReturn, pairs_of_cubes_in_motion) frameId += 1; } } -#endif \ No newline at end of file +#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, "MRTest_Camera"); + + 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(cameraRays)); + ASSERT_RGL_SUCCESS(rgl_graph_run(mrRaytrace)); + + std::this_thread::sleep_for(100ms); + frameId += 1; + } +} +#endif From 61d5975421e2a93adbb42fd0464ca6fdfcd6b8d4 Mon Sep 17 00:00:00 2001 From: nebraszka Date: Wed, 29 May 2024 00:45:48 +0200 Subject: [PATCH 06/10] Add multiple ray beams test --- test/src/graph/multiReturnTest.cpp | 67 ++++++++++++++++++++++++++---- 1 file changed, 59 insertions(+), 8 deletions(-) diff --git a/test/src/graph/multiReturnTest.cpp b/test/src/graph/multiReturnTest.cpp index 186e370b..a2b58567 100644 --- a/test/src/graph/multiReturnTest.cpp +++ b/test/src/graph/multiReturnTest.cpp @@ -17,8 +17,25 @@ class GraphMultiReturn : public RGLTest protected: // VLP16 data const float vlp16LidarObjectDistance = 100.0f; - const float vlp16LidarHBeamDivergence = 0.003f; // VLP16 horizontal beam divergence in rads - const float vlp16LidarHBeamDiameter = 0.2868f; // VLP16 beam horizontal diameter at 100m + 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*/}; @@ -65,15 +82,15 @@ class GraphMultiReturn : public RGLTest } } - void constructCameraGraph(const rgl_mat3x4f& cameraPose, const char* topic) + void constructCameraGraph(const rgl_mat3x4f& cameraPose) { #ifdef RGL_BUILD_ROS2_EXTENSION - const std::vector cameraRayTf = makeLidar3dRays(360.0f, 180.0f, 0.5f, 0.5f); + 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, topic, "world")); + 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)); @@ -97,6 +114,8 @@ class GraphMultiReturn : public RGLTest spawnCubeOnScene(Mat3x4f::translation(stepDepth, 0.0f, stepHeight) * firstCubeTf); spawnCubeOnScene(Mat3x4f::translation(2 * stepDepth, 0.0f, 2 * stepHeight) * firstCubeTf); } + + float mmToMeters(float mm) const { return mm * 0.001f; } }; /** @@ -195,7 +214,7 @@ TEST_F(GraphMultiReturn, pairs_of_cubes_in_motion) 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_PairsOfCubes", "world")); + 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)); @@ -203,7 +222,7 @@ TEST_F(GraphMultiReturn, pairs_of_cubes_in_motion) // Camera const rgl_mat3x4f cameraPose = Mat3x4f::TRS(Vec3f{-8.0f, 1.0f, 5.0f}, {90.0f, 30.0f, -70.0f}).toRGL(); - constructCameraGraph(cameraPose, "MRTest_Camera"); + constructCameraGraph(cameraPose); int frameId = 0; while (true) { @@ -263,7 +282,7 @@ TEST_F(GraphMultiReturn, stairs) // Camera rgl_mat3x4f cameraPose = Mat3x4f::translation({0.0f, -1.5f, stepHeight * 3 + 1.0f}).toRGL(); - constructCameraGraph(cameraPose, "MRTest_Camera"); + constructCameraGraph(cameraPose); int frameId = 0; while (true) { @@ -277,4 +296,36 @@ TEST_F(GraphMultiReturn, stairs) 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})); + + // Camera + constructCameraGraph(Mat3x4f::identity().toRGL()); + + // Lidar with MR + const int horizontalSteps = 10; + const auto resolution = 360.0f / horizontalSteps; + std::vector vlp16RaysTf; + vlp16RaysTf.reserve(horizontalSteps * vlp16Channels.size()); + + 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); + + ASSERT_RGL_SUCCESS(rgl_graph_run(cameraRays)); + ASSERT_RGL_SUCCESS(rgl_graph_run(mrRays)); +} #endif From 4499a4a3a9f395fa408bf5ca91966db73fa9bf8c Mon Sep 17 00:00:00 2001 From: nebraszka Date: Wed, 29 May 2024 00:55:20 +0200 Subject: [PATCH 07/10] Fix building without ros2 --- test/src/graph/multiReturnTest.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/src/graph/multiReturnTest.cpp b/test/src/graph/multiReturnTest.cpp index a2b58567..1cd86d9e 100644 --- a/test/src/graph/multiReturnTest.cpp +++ b/test/src/graph/multiReturnTest.cpp @@ -84,7 +84,7 @@ class GraphMultiReturn : public RGLTest void constructCameraGraph(const rgl_mat3x4f& cameraPose) { -#ifdef RGL_BUILD_ROS2_EXTENSION +#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)); From c61133445744e8631fb5edfc50a1545f5ccf610c Mon Sep 17 00:00:00 2001 From: Piotr Rybicki Date: Wed, 29 May 2024 17:43:56 +0200 Subject: [PATCH 08/10] Remove unused rgl_node_t and shadowing --- test/src/graph/multiReturnTest.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/test/src/graph/multiReturnTest.cpp b/test/src/graph/multiReturnTest.cpp index 1cd86d9e..e6be2523 100644 --- a/test/src/graph/multiReturnTest.cpp +++ b/test/src/graph/multiReturnTest.cpp @@ -206,7 +206,6 @@ TEST_F(GraphMultiReturn, pairs_of_cubes_in_motion) // Lidar with MR const float beamDivAngle = 0.003f; - rgl_node_t mrRays = nullptr, mrRaytrace = nullptr, mrFormatFirst = nullptr, mrFormatLast = nullptr; constructMRGraph(raysTf, lidarPose, beamDivAngle, true); // Lidar without MR From 5cd42aba82c59d8e39dfbc1f2b96d9dbad28e8c0 Mon Sep 17 00:00:00 2001 From: nebraszka Date: Tue, 4 Jun 2024 13:47:24 +0200 Subject: [PATCH 09/10] Part of review fixes --- test/src/graph/multiReturnTest.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/src/graph/multiReturnTest.cpp b/test/src/graph/multiReturnTest.cpp index e6be2523..85958ce8 100644 --- a/test/src/graph/multiReturnTest.cpp +++ b/test/src/graph/multiReturnTest.cpp @@ -115,14 +115,14 @@ class GraphMultiReturn : public RGLTest spawnCubeOnScene(Mat3x4f::translation(2 * stepDepth, 0.0f, 2 * stepHeight) * firstCubeTf); } - float mmToMeters(float mm) const { return mm * 0.001f; } + 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) +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()}; From 92543b4a19daa179cef28a8431c25b0104ded417 Mon Sep 17 00:00:00 2001 From: nebraszka Date: Tue, 4 Jun 2024 14:54:46 +0200 Subject: [PATCH 10/10] Finish review fixes --- test/src/graph/multiReturnTest.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/test/src/graph/multiReturnTest.cpp b/test/src/graph/multiReturnTest.cpp index 85958ce8..16eb8ceb 100644 --- a/test/src/graph/multiReturnTest.cpp +++ b/test/src/graph/multiReturnTest.cpp @@ -128,7 +128,8 @@ TEST_F(GraphMultiReturn, vlp16_data_compare) 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 rgl_mat3x4f lidarPose = Mat3x4f::TRS({lidarCubeCenterDist, 0.0f, 0.0f}).toRGL(); + const auto lidarTransl = Vec3f{lidarCubeCenterDist, 0.0f, 0.0f}; + const rgl_mat3x4f lidarPose = Mat3x4f::TRS(lidarTransl).toRGL(); // Scene spawnCubeOnScene(Mat3x4f::identity()); @@ -140,7 +141,7 @@ TEST_F(GraphMultiReturn, vlp16_data_compare) EXPECT_RGL_SUCCESS(rgl_graph_run(mrRays)); // Verify the output - const float epsilon = 1e-5f; + const float epsilon = 1e-4f; const auto mrFirstOutPointcloud = TestPointCloud::createFromNode(mrFormatFirst, fields); const auto mrFirstIsHits = mrFirstOutPointcloud.getFieldValues(); @@ -149,23 +150,25 @@ TEST_F(GraphMultiReturn, vlp16_data_compare) 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); - EXPECT_NEAR(mrFirstDistances.at(0), lidarCubeFaceDist, epsilon); - - const float expectedDiameter = vlp16LidarHBeamDiameter; const auto mrLastOutPointcloud = TestPointCloud::createFromNode(mrFormatLast, fields); const auto mrLastIsHits = mrLastOutPointcloud.getFieldValues(); const auto mrLastPoints = mrLastOutPointcloud.getFieldValues(); const auto mrLastDistances = mrLastOutPointcloud.getFieldValues(); - const auto expectedLastPoint = Vec3f{CUBE_HALF_EDGE, 0.0f, -expectedDiameter / 2}; + 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(), 0.01f); + EXPECT_NEAR(mrLastPoints.at(0).z(), expectedLastPoint.z(), epsilon); } #if RGL_BUILD_ROS2_EXTENSION @@ -282,13 +285,13 @@ TEST_F(GraphMultiReturn, stairs) // 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(cameraRays)); ASSERT_RGL_SUCCESS(rgl_graph_run(mrRaytrace)); std::this_thread::sleep_for(100ms);