diff --git a/include/spark_dsg/dynamic_scene_graph.h b/include/spark_dsg/dynamic_scene_graph.h index 8d2d737..4b5bca5 100644 --- a/include/spark_dsg/dynamic_scene_graph.h +++ b/include/spark_dsg/dynamic_scene_graph.h @@ -33,12 +33,10 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include -#include +#include #include #include "spark_dsg/dynamic_scene_graph_layer.h" -#include "spark_dsg/mesh.h" #include "spark_dsg/scene_graph_layer.h" #include "spark_dsg/spark_dsg_fwd.h" @@ -526,7 +524,7 @@ class DynamicSceneGraph { bool hasMesh() const; - Mesh::Ptr mesh() const; + std::shared_ptr mesh() const; //! current static layer ids in the graph const LayerIds layer_ids; diff --git a/include/spark_dsg/dynamic_scene_graph_layer.h b/include/spark_dsg/dynamic_scene_graph_layer.h index bf7a939..e29c183 100644 --- a/include/spark_dsg/dynamic_scene_graph_layer.h +++ b/include/spark_dsg/dynamic_scene_graph_layer.h @@ -33,8 +33,6 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include - #include "spark_dsg/base_layer.h" #include "spark_dsg/edge_container.h" #include "spark_dsg/layer_prefix.h" @@ -94,14 +92,6 @@ class DynamicSceneGraphLayer : public BaseLayer { bool removeEdgeByIndex(size_t source_index, size_t target_index); - Eigen::Vector3d getPosition(NodeId node) const; - - Eigen::Vector3d getPositionByIndex(size_t node_index) const; - - const LayerId id; - - const LayerPrefix prefix; - void mergeLayer(const DynamicSceneGraphLayer& other, const GraphMergeConfig& config, std::vector* new_nodes = nullptr); @@ -115,6 +105,10 @@ class DynamicSceneGraphLayer : public BaseLayer { void getRemovedEdges(std::vector& removed_edges, bool clear_removed) override; + const LayerId id; + + const LayerPrefix prefix; + protected: bool emplaceNode(std::chrono::nanoseconds timestamp, std::unique_ptr&& attrs, diff --git a/include/spark_dsg/scene_graph_layer.h b/include/spark_dsg/scene_graph_layer.h index 3d4c26a..39d8063 100644 --- a/include/spark_dsg/scene_graph_layer.h +++ b/include/spark_dsg/scene_graph_layer.h @@ -33,7 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include +#include #include #include @@ -169,11 +169,6 @@ class SceneGraphLayer : public BaseLayer { */ inline size_t numEdges() const { return edges_.size(); } - /** - * @brief Get the position of a node in the layer with bounds checking - */ - Eigen::Vector3d getPosition(NodeId node) const; - /** * @brief Get node ids of newly inserted nodes */ diff --git a/src/dynamic_scene_graph.cpp b/src/dynamic_scene_graph.cpp index b78960f..4ff70ad 100644 --- a/src/dynamic_scene_graph.cpp +++ b/src/dynamic_scene_graph.cpp @@ -38,6 +38,7 @@ #include "spark_dsg/edge_attributes.h" #include "spark_dsg/logging.h" +#include "spark_dsg/mesh.h" #include "spark_dsg/node_attributes.h" #include "spark_dsg/node_symbol.h" #include "spark_dsg/printing.h" @@ -568,19 +569,15 @@ size_t DynamicSceneGraph::numDynamicEdges() const { bool DynamicSceneGraph::empty() const { return numNodes() == 0; } -Eigen::Vector3d DynamicSceneGraph::getPosition(NodeId node) const { - auto iter = node_lookup_.find(node); +Eigen::Vector3d DynamicSceneGraph::getPosition(NodeId node_id) const { + auto iter = node_lookup_.find(node_id); if (iter == node_lookup_.end()) { - throw std::out_of_range("node " + NodeSymbol(node).getLabel() + + throw std::out_of_range("node " + NodeSymbol(node_id).getLabel() + " is not in the graph"); } - auto info = iter->second; - if (info.dynamic) { - return dynamic_layers_.at(info.layer).at(info.prefix)->getPosition(node); - } - - return layers_.at(info.layer)->getPosition(node); + const auto node = getNodePtr(node_id, iter->second); + return node->attributes().position; } bool DynamicSceneGraph::mergeNodes(NodeId node_from, NodeId node_to) { diff --git a/src/dynamic_scene_graph_layer.cpp b/src/dynamic_scene_graph_layer.cpp index ce309bc..2dc8067 100644 --- a/src/dynamic_scene_graph_layer.cpp +++ b/src/dynamic_scene_graph_layer.cpp @@ -268,26 +268,6 @@ bool DynamicSceneGraphLayer::removeNode(NodeId node) { return true; } -Eigen::Vector3d DynamicSceneGraphLayer::getPosition(NodeId node) const { - if (!hasNode(node)) { - std::stringstream ss; - ss << "node " << NodeSymbol(node).getLabel() << " is missing"; - throw std::out_of_range(ss.str()); - } - - return getPositionByIndex(prefix.index(node)); -} - -Eigen::Vector3d DynamicSceneGraphLayer::getPositionByIndex(size_t node_index) const { - if (!hasNodeByIndex(node_index)) { - std::stringstream ss; - ss << "node index" << node_index << " >= " << nodes_.size(); - throw std::out_of_range(ss.str()); - } - - return nodes_.at(node_index)->attributes().position; -} - void DynamicSceneGraphLayer::getNewNodes(std::vector& new_nodes, bool clear_new) { auto iter = node_status_.begin(); diff --git a/src/scene_graph_layer.cpp b/src/scene_graph_layer.cpp index 87c0a69..e1da92a 100644 --- a/src/scene_graph_layer.cpp +++ b/src/scene_graph_layer.cpp @@ -268,16 +268,6 @@ void SceneGraphLayer::mergeLayer(const SceneGraphLayer& other_layer, } } -Eigen::Vector3d SceneGraphLayer::getPosition(NodeId node) const { - if (!hasNode(node)) { - std::stringstream ss; - ss << "node " << NodeSymbol(node).getLabel() << " not in layer"; - throw std::out_of_range(ss.str()); - } - - return nodes_.at(node)->attributes().position; -} - void SceneGraphLayer::getNewNodes(std::vector& new_nodes, bool clear_new) { auto iter = nodes_status_.begin(); while (iter != nodes_status_.end()) { diff --git a/tests/utest_dynamic_scene_graph_layer.cpp b/tests/utest_dynamic_scene_graph_layer.cpp index 504ccf4..bc01b96 100644 --- a/tests/utest_dynamic_scene_graph_layer.cpp +++ b/tests/utest_dynamic_scene_graph_layer.cpp @@ -229,40 +229,6 @@ TEST(DynamicSceneGraphLayerTests, BasicEdgeIterationCorrect) { EXPECT_EQ(expected_targets, actual_targets); } -TEST(DynamicSceneGraphLayerTests, getPositionCorrect) { - using namespace std::chrono_literals; - Eigen::Vector3d expected; - expected << 1.0, 2.0, 3.0; - auto attrs = std::make_unique(expected); - - TestableDynamicLayer layer(1, 0); - layer.emplaceNode(1s, std::move(attrs)); - - Eigen::Vector3d result = layer.getPosition(NodeSymbol(0, 0)); - EXPECT_EQ(expected(0), result(0)); - EXPECT_EQ(expected(1), result(1)); - EXPECT_EQ(expected(2), result(2)); - - result = layer.getPositionByIndex(0); - EXPECT_EQ(expected(0), result(0)); - EXPECT_EQ(expected(1), result(1)); - EXPECT_EQ(expected(2), result(2)); - - try { - layer.getPosition(NodeSymbol(0, 5)); - FAIL(); - } catch (const std::out_of_range&) { - SUCCEED(); - } - - try { - layer.getPositionByIndex(1); - FAIL(); - } catch (const std::out_of_range&) { - SUCCEED(); - } -} - // Test that rewiring an edge does what it should TEST(DynamicSceneGraphLayerTests, MergeLayerCorrect) { TestableDynamicLayer layer_1(1, 0); @@ -292,7 +258,7 @@ TEST(DynamicSceneGraphLayerTests, MergeLayerCorrect) { EXPECT_EQ(new_nodes, expected_new_nodes); for (size_t i = 0; i < 5; i++) { - Eigen::Vector3d result = layer_1.getPosition(i); + Eigen::Vector3d result = layer_1.getNode(i).attributes().position; EXPECT_EQ(static_cast(i), result(0)); EXPECT_EQ(0.0, result(1)); EXPECT_EQ(0.0, result(2)); diff --git a/tests/utest_scene_graph_layer.cpp b/tests/utest_scene_graph_layer.cpp index 858fcf5..3abfe0a 100644 --- a/tests/utest_scene_graph_layer.cpp +++ b/tests/utest_scene_graph_layer.cpp @@ -363,7 +363,7 @@ TEST(SceneGraphLayerTests, MergeLayerCorrect) { EXPECT_EQ(new_nodes, expected_new_nodes); for (size_t i = 0; i < 5; i++) { - Eigen::Vector3d result = layer_1.getPosition(i); + Eigen::Vector3d result = layer_1.getNode(i).attributes().position; EXPECT_NEAR(static_cast(i) + 10, result(0), 1.0e-9); EXPECT_NEAR(0.0, result(1), 1.0e-9); EXPECT_NEAR(0.0, result(2), 1.0e-9); @@ -371,27 +371,6 @@ TEST(SceneGraphLayerTests, MergeLayerCorrect) { } } -TEST(SceneGraphLayerTests, getPositionCorrect) { - Eigen::Vector3d expected; - expected << 1.0, 2.0, 3.0; - auto attrs = std::make_unique(expected); - - IsolatedSceneGraphLayer layer(1); - layer.emplaceNode(NodeSymbol('x', 0), std::move(attrs)); - - Eigen::Vector3d result = layer.getPosition(NodeSymbol('x', 0)); - EXPECT_EQ(expected(0), result(0)); - EXPECT_EQ(expected(1), result(1)); - EXPECT_EQ(expected(2), result(2)); - - try { - layer.getPosition(NodeSymbol('x', 5)); - FAIL(); - } catch (const std::out_of_range&) { - SUCCEED(); - } -} - TEST(SceneGraphLayerTests, GetNeighborhoodCorrect) { IsolatedSceneGraphLayer layer(1);