drop getPosition and eigen dep

include/spark_dsg/dynamic_scene_graph.h

@@ -33,12 +33,10 @@
  * purposes notwithstanding any copyright notation herein.
  * -------------------------------------------------------------------------- */
 #pragma once
-#include <map>
-#include <memory>
+#include <Eigen/Core>
 #include <type_traits>
 
 #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> mesh() const;
 
   //! current static layer ids in the graph
   const LayerIds layer_ids;

include/spark_dsg/dynamic_scene_graph_layer.h

@@ -33,8 +33,6 @@
  * purposes notwithstanding any copyright notation herein.
  * -------------------------------------------------------------------------- */
 #pragma once
-#include <Eigen/Core>
-
 #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<NodeId>* new_nodes = nullptr);
@@ -115,6 +105,10 @@ class DynamicSceneGraphLayer : public BaseLayer {
   void getRemovedEdges(std::vector<EdgeKey>& removed_edges,
                        bool clear_removed) override;
 
+  const LayerId id;
+
+  const LayerPrefix prefix;
+
  protected:
   bool emplaceNode(std::chrono::nanoseconds timestamp,
                    std::unique_ptr<NodeAttributes>&& attrs,

include/spark_dsg/scene_graph_layer.h

@@ -33,7 +33,7 @@
  * purposes notwithstanding any copyright notation herein.
  * -------------------------------------------------------------------------- */
 #pragma once
-#include <Eigen/Core>
+#include <functional>
 #include <map>
 #include <unordered_set>
 
@@ -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
    */

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) {

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<NodeId>& new_nodes,
                                          bool clear_new) {
   auto iter = node_status_.begin();

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<NodeId>& new_nodes, bool clear_new) {
   auto iter = nodes_status_.begin();
   while (iter != nodes_status_.end()) {

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<NodeAttributes>(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<double>(i), result(0));
     EXPECT_EQ(0.0, result(1));
     EXPECT_EQ(0.0, result(2));

tests/utest_scene_graph_layer.cpp

@@ -363,35 +363,14 @@ 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<double>(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);
     EXPECT_EQ(NodeStatus::NEW, layer_1.checkNode(i));
   }
 }
 
-TEST(SceneGraphLayerTests, getPositionCorrect) {
-  Eigen::Vector3d expected;
-  expected << 1.0, 2.0, 3.0;
-  auto attrs = std::make_unique<NodeAttributes>(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);