From 4c31872c8983a4f6517ebaf602c7f54ec5b97bf9 Mon Sep 17 00:00:00 2001 From: Mateusz Szczygielski <112629916+msz-rai@users.noreply.github.com> Date: Wed, 29 Mar 2023 08:20:55 +0100 Subject: [PATCH] Support multi-raytrace graphs (#122) * Refactor findFieldToCompute and move to RaytraceNode * Add test for spatial merge using multi-raytrace graph * Extend format test with fields update * Add comments suggested in review --- src/graph/Graph.cpp | 30 ----------- src/graph/Graph.hpp | 3 +- src/graph/NodesCore.hpp | 8 ++- src/graph/RaytraceNode.cpp | 31 +++++++++++ test/src/graphTest.cpp | 108 +++++++++++++++++++++++++++++++------ 5 files changed, 130 insertions(+), 50 deletions(-) diff --git a/src/graph/Graph.cpp b/src/graph/Graph.cpp index c50d80b3..4986e61a 100644 --- a/src/graph/Graph.cpp +++ b/src/graph/Graph.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include -#include std::list> Graph::instances; @@ -43,7 +42,6 @@ std::shared_ptr Graph::create(std::shared_ptr node) graph->nodes = Graph::findConnectedNodes(node); graph->executionOrder = Graph::findExecutionOrder(graph->nodes); - graph->fieldsToCompute = Graph::findFieldsToCompute(graph->nodes); for (auto&& currentNode : graph->nodes) { if (currentNode->hasGraph()) { @@ -64,12 +62,6 @@ void Graph::run() RGL_DEBUG("Running graph with {} nodes", nodesInExecOrder.size()); - // If Graph has RaytraceNode set fields to compute - if (!Node::filter(nodesInExecOrder).empty()) { - RaytraceNode::Ptr rt = Node::getExactlyOne(nodesInExecOrder); - rt->setFields(fieldsToCompute); - } - for (auto&& current : nodesInExecOrder) { RGL_DEBUG("Validating node: {}", *current); current->validate(); @@ -129,28 +121,6 @@ std::vector> Graph::findExecutionOrder(std::set Graph::findFieldsToCompute(std::set> nodes) -{ - std::set outFields; - for (auto&& node : nodes) { - if (auto pointNode = std::dynamic_pointer_cast(node)) { - for (auto&& field : pointNode->getRequiredFieldList()) { - if (!isDummy(field)) { - outFields.insert(field); - } - } - } - } - - outFields.insert(XYZ_F32); - - if (!Node::filter(nodes).empty()) { - outFields.insert(IS_HIT_I32); - } - - return outFields; -} - Graph::~Graph() { stream.reset(); diff --git a/src/graph/Graph.hpp b/src/graph/Graph.hpp index 019586bd..82828241 100644 --- a/src/graph/Graph.hpp +++ b/src/graph/Graph.hpp @@ -19,6 +19,7 @@ #include #include +#include #include struct Graph @@ -37,13 +38,11 @@ struct Graph Graph() : stream(std::make_shared()) {} static std::vector> findExecutionOrder(std::set> nodes); - static std::set findFieldsToCompute(std::set> nodes); private: std::shared_ptr stream; std::set> nodes; std::vector> executionOrder; - std::set fieldsToCompute; static std::list> instances; }; diff --git a/src/graph/NodesCore.hpp b/src/graph/NodesCore.hpp index e590cb81..50a6f41c 100644 --- a/src/graph/NodesCore.hpp +++ b/src/graph/NodesCore.hpp @@ -88,6 +88,9 @@ struct CompactPointsNode : Node, IPointsNodeSingleInput void validate() override; void schedule(cudaStream_t stream) override; + // Node requirements + std::vector getRequiredFieldList() const override { return {IS_HIT_I32}; } + // Point cloud description bool isDense() const override { return true; } size_t getWidth() const override; @@ -124,8 +127,6 @@ struct RaytraceNode : Node, IPointsNode VArray::ConstPtr getFieldData(rgl_field_t field, cudaStream_t stream) const override { return std::const_pointer_cast(fieldData.at(field)); } - - void setFields(const std::set& fields); private: float range; std::shared_ptr scene; @@ -135,6 +136,9 @@ struct RaytraceNode : Node, IPointsNode template auto getPtrTo(); + + std::set findFieldsToCompute(); + void setFields(const std::set& fields); }; struct TransformPointsNode : Node, IPointsNodeSingleInput diff --git a/src/graph/RaytraceNode.cpp b/src/graph/RaytraceNode.cpp index 0c1a8297..372145dd 100644 --- a/src/graph/RaytraceNode.cpp +++ b/src/graph/RaytraceNode.cpp @@ -22,6 +22,9 @@ void RaytraceNode::validate() { + // It should be viewed as a temporary solution. Will change in v14. + setFields(findFieldsToCompute()); + raysNode = getValidInput(); if (fieldData.contains(RING_ID_U16) && !raysNode->getRingIds().has_value()) { @@ -93,3 +96,31 @@ void RaytraceNode::setFields(const std::set& fields) fieldData.insert({field, VArray::create(field)}); } } + +std::set RaytraceNode::findFieldsToCompute() +{ + std::set outFields; + + // Add primary field + outFields.insert(XYZ_F32); + + // dfsInputs - if false dfs for outputs + std::function dfsRet = [&](const Node::Ptr & current, bool dfsInputs) { + auto dfsNodes = dfsInputs ? current->getInputs() : current->getOutputs(); + for (auto&& node : dfsNodes) { + if (auto pointNode = std::dynamic_pointer_cast(node)) { + for (auto&& field : pointNode->getRequiredFieldList()) { + if (!isDummy(field)) { + outFields.insert(field); + } + } + dfsRet(node, dfsInputs); + } + } + }; + + dfsRet(shared_from_this(), true); // Search in inputs. Needed for SetRingIds only. + dfsRet(shared_from_this(), false); // Search in outputs + + return outFields; +} diff --git a/test/src/graphTest.cpp b/test/src/graphTest.cpp index 75cf8a5b..14dab1c9 100644 --- a/test/src/graphTest.cpp +++ b/test/src/graphTest.cpp @@ -101,7 +101,7 @@ TEST_F(GraphCase, NodeRemoval) #endif } -TEST_F(GraphCase, SpatialMerge) +TEST_F(GraphCase, SpatialMergeFromTransforms) { auto mesh = makeCubeMesh(); @@ -144,6 +144,51 @@ TEST_F(GraphCase, SpatialMerge) #endif } +TEST_F(GraphCase, SpatialMergeFromRaytraces) +{ + // Setup cube scene + auto mesh = makeCubeMesh(); + auto entity = makeEntity(mesh); + rgl_mat3x4f entityPoseTf = Mat3x4f::identity().toRGL(); + ASSERT_RGL_SUCCESS(rgl_entity_set_pose(entity, &entityPoseTf)); + + constexpr int LIDAR_FOV_Y = 40; + constexpr int LIDAR_ROTATION_STEP = LIDAR_FOV_Y / 2; // Make laser overlaps to validate merging + + std::vector rays = makeLidar3dRays(180, LIDAR_FOV_Y, 0.18, 1); + + // Lidars will be located in the cube center with different rotations covering all the space. + std::vector lidarTfs; + for (int i = 0; i < 360 / LIDAR_ROTATION_STEP; ++i) { + lidarTfs.emplace_back(Mat3x4f::TRS({0, 0, 0}, {0, LIDAR_ROTATION_STEP * i, 0}).toRGL()); + } + + rgl_node_t spatialMerge=nullptr; + std::vector sMergeFields = { RGL_FIELD_XYZ_F32, RGL_FIELD_DISTANCE_F32 }; + EXPECT_RGL_SUCCESS(rgl_node_points_spatial_merge(&spatialMerge, sMergeFields.data(), sMergeFields.size())); + + for (auto& lidarTf : lidarTfs) { + rgl_node_t lidarRays = nullptr; + rgl_node_t lidarRaysTf = nullptr; + rgl_node_t raytrace = nullptr; + + EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&lidarRays, rays.data(), rays.size())); + EXPECT_RGL_SUCCESS(rgl_node_rays_transform(&lidarRaysTf, &lidarTf)); + EXPECT_RGL_SUCCESS(rgl_node_raytrace(&raytrace, nullptr, 1000)); + + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(lidarRays, lidarRaysTf)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(lidarRaysTf, raytrace)); + EXPECT_RGL_SUCCESS(rgl_graph_node_add_child(raytrace, spatialMerge)); + } + + EXPECT_RGL_SUCCESS(rgl_graph_run(spatialMerge)); +#ifdef RGL_BUILD_PCL_EXTENSION + EXPECT_RGL_SUCCESS(rgl_graph_write_pcd_file(spatialMerge, "cube_spatial_merge.pcd")); +#else + RGL_WARN("RGL compiled without PCL extension. Tests will not save PCD!"); +#endif +} + TEST_F(GraphCase, TemporalMerge) { auto mesh = makeCubeMesh(); @@ -197,6 +242,9 @@ TEST_F(GraphCase, FormatNodeResults) rgl_node_t useRays=nullptr, raytrace=nullptr, lidarPose=nullptr, format=nullptr; + // The cube located in 0,0,0 with width equals 1, rays shoot in perpendicular direction + constexpr float EXPECTED_HITPOINT_Z = 1.0f; + constexpr float EXPECTED_RAY_DISTANCE = 1.0f; std::vector rays = { Mat3x4f::TRS({0, 0, 0}).toRGL(), Mat3x4f::TRS({0.1, 0, 0}).toRGL(), @@ -208,11 +256,17 @@ TEST_F(GraphCase, FormatNodeResults) std::vector formatFields = { XYZ_F32, PADDING_32, - TIME_STAMP_F64 + TIME_STAMP_F64 }; + struct FormatStruct + { + Field::type xyz; + Field::type padding; + Field::type timestamp; + } formatStruct; - Time timestamp = Time::seconds(1.5); - EXPECT_RGL_SUCCESS(rgl_scene_set_time(nullptr, timestamp.asNanoseconds())); + Time timestamp = Time::seconds(1.5); + EXPECT_RGL_SUCCESS(rgl_scene_set_time(nullptr, timestamp.asNanoseconds())); EXPECT_RGL_SUCCESS(rgl_node_rays_from_mat3x4f(&useRays, rays.data(), rays.size())); EXPECT_RGL_SUCCESS(rgl_node_rays_transform(&lidarPose, &lidarPoseTf)); @@ -228,24 +282,46 @@ TEST_F(GraphCase, FormatNodeResults) int32_t outCount, outSizeOf; EXPECT_RGL_SUCCESS(rgl_graph_get_result_size(format, RGL_FIELD_DYNAMIC_FORMAT, &outCount, &outSizeOf)); - struct FormatStruct - { - Field::type xyz; - Field::type padding; - Field::type timestamp; - } formatStruct; - EXPECT_EQ(outCount, rays.size()); EXPECT_EQ(outSizeOf, sizeof(formatStruct)); - std::vector formatData{(size_t)outCount}; + std::vector formatData(outCount); EXPECT_RGL_SUCCESS(rgl_graph_get_result_data(format, RGL_FIELD_DYNAMIC_FORMAT, formatData.data())); for (int i = 0; i < formatData.size(); ++i) { - EXPECT_NEAR(formatData[i].xyz[0], rays[i].value[0][3], 1e-6); - EXPECT_NEAR(formatData[i].xyz[1], rays[i].value[1][3], 1e-6); - EXPECT_NEAR(formatData[i].xyz[2], 1, 1e-6); - EXPECT_EQ(formatData[i].timestamp, timestamp.asSeconds()); + EXPECT_NEAR(formatData[i].xyz[0], rays[i].value[0][3], EPSILON_F); + EXPECT_NEAR(formatData[i].xyz[1], rays[i].value[1][3], EPSILON_F); + EXPECT_NEAR(formatData[i].xyz[2], EXPECTED_HITPOINT_Z, EPSILON_F); + EXPECT_EQ(formatData[i].timestamp, timestamp.asSeconds()); + } + + // Test if fields update is propagated over graph properly + formatFields.push_back(DISTANCE_F32); // Add distance field + formatFields.push_back(PADDING_32); // Align to 8 bytes + struct FormatStructExtended : public FormatStruct + { + Field::type distance; + Field::type padding2; // Align to 8 bytes + } formatStructEx; + + EXPECT_RGL_SUCCESS(rgl_node_points_format(&format, formatFields.data(), formatFields.size())); + EXPECT_RGL_SUCCESS(rgl_graph_run(raytrace)); + + outCount = -1; // reset variables + outSizeOf = -1; + EXPECT_RGL_SUCCESS(rgl_graph_get_result_size(format, RGL_FIELD_DYNAMIC_FORMAT, &outCount, &outSizeOf)); + EXPECT_EQ(outCount, rays.size()); + EXPECT_EQ(outSizeOf, sizeof(formatStructEx)); + + std::vector formatDataEx{(size_t)outCount}; + EXPECT_RGL_SUCCESS(rgl_graph_get_result_data(format, RGL_FIELD_DYNAMIC_FORMAT, formatDataEx.data())); + + for (int i = 0; i < formatDataEx.size(); ++i) { + EXPECT_NEAR(formatDataEx[i].xyz[0], rays[i].value[0][3], EPSILON_F); + EXPECT_NEAR(formatDataEx[i].xyz[1], rays[i].value[1][3], EPSILON_F); + EXPECT_NEAR(formatDataEx[i].xyz[2], EXPECTED_HITPOINT_Z, EPSILON_F); + EXPECT_NEAR(formatDataEx[i].distance, EXPECTED_RAY_DISTANCE, EPSILON_F); + EXPECT_EQ(formatDataEx[i].timestamp, timestamp.asSeconds()); } }