Feature/reduce compilation

CMakeLists.txt

@@ -11,6 +11,7 @@ option(SPARK_DSG_BUILD_EXAMPLES "Build examples" ON)
 option(SPARK_DSG_BUILD_TESTS "Build tests" ON)
 option(SPARK_DSG_BUILD_PYTHON "Build python bindings" ON)
 option(SPARK_DSG_BUILD_ZMQ "Build zmq message interface" ON)
+option(SPARK_DSG_PROFILE_BUILD "Profile build time" OFF)
 option(BUILD_SHARED_LIBS "Build shared libs" ON)
 
 configure_file(cmake/spark_dsg_version.h.in include/spark_dsg_version.h)
@@ -22,20 +23,27 @@ find_package(Threads REQUIRED)
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(zmq libzmq)
 
+if(SPARK_DSG_PROFILE_BUILD)
+  add_compile_options(-ftime-trace)
+endif()
+
 add_library(
   ${PROJECT_NAME}
   src/adjacency_matrix.cpp
+  src/base_layer.cpp
   src/bounding_box_extraction.cpp
   src/bounding_box.cpp
   src/color.cpp
   src/dynamic_scene_graph_layer.cpp
   src/dynamic_scene_graph.cpp
   src/edge_attributes.cpp
   src/edge_container.cpp
+  src/layer_prefix.cpp
   src/layer_view.cpp
   src/mesh.cpp
   src/node_attributes.cpp
   src/node_symbol.cpp
+  src/printing.cpp
   src/scene_graph_layer.cpp
   src/scene_graph_logger.cpp
   src/scene_graph_node.cpp
@@ -48,8 +56,8 @@ add_library(
   src/serialization/graph_binary_serialization.cpp
   src/serialization/graph_json_serialization.cpp
   src/serialization/json_conversions.cpp
-  src/serialization/versioning.cpp
   src/serialization/mesh_serialization.cpp
+  src/serialization/versioning.cpp
 )
 
 if(NOT BUILD_SHARED_LIBS)

examples/dsg_endpoint.cpp

@@ -1,6 +1,8 @@
+#include <spark_dsg/dynamic_scene_graph.h>
 #include <spark_dsg/zmq_interface.h>
 
 #include <chrono>
+#include <iostream>
 #include <thread>
 
 auto main(int argc, char* argv[]) -> int {

examples/dsg_repeater.cpp

@@ -1,6 +1,8 @@
+#include <spark_dsg/dynamic_scene_graph.h>
 #include <spark_dsg/zmq_interface.h>
 
 #include <chrono>
+#include <iostream>
 #include <thread>
 
 auto main(int argc, char* argv[]) -> int {

include/spark_dsg/adjacency_matrix.h

@@ -36,7 +36,8 @@
 #include <Eigen/Dense>
 #include <Eigen/SparseCore>
 
-#include "spark_dsg/scene_graph_layer.h"
+#include "spark_dsg/scene_graph_types.h"
+#include "spark_dsg/spark_dsg_fwd.h"
 
 namespace spark_dsg {
 

include/spark_dsg/base_layer.h

@@ -33,9 +33,10 @@
  * purposes notwithstanding any copyright notation herein.
  * -------------------------------------------------------------------------- */
 #pragma once
-#include "spark_dsg/edge_attributes.h"
-#include "spark_dsg/edge_container.h"
+#include <map>
+
 #include "spark_dsg/scene_graph_node.h"
+#include "spark_dsg/spark_dsg_fwd.h"
 
 namespace spark_dsg {
 
@@ -47,16 +48,16 @@ struct GraphMergeConfig {
   bool clear_removed = false;
   bool enforce_parent_constraints = true;
 
-  NodeId getMergedId(NodeId original) const {
-    if (!previous_merges) {
-      return original;
-    }
-
-    auto iter = previous_merges->find(original);
-    return iter == previous_merges->end() ? original : iter->second;
-  }
+  NodeId getMergedId(NodeId original) const;
 };
 
+/**
+ * @brief Base node status.
+ *
+ * Mostly for keeping history and status of nodes in a graph
+ */
+enum class NodeStatus { NEW, VISIBLE, MERGED, DELETED, NONEXISTENT };
+
 class BaseLayer {
  public:
   friend class DynamicSceneGraph;
@@ -67,33 +68,19 @@ class BaseLayer {
 
   virtual bool removeEdge(NodeId source, NodeId target) = 0;
 
-  virtual bool insertEdge(NodeId source, NodeId target, EdgeAttributes::Ptr&& info) = 0;
+  virtual bool insertEdge(NodeId source,
+                          NodeId target,
+                          std::unique_ptr<EdgeAttributes>&& info) = 0;
 
   virtual NodeStatus checkNode(NodeId node_id) const = 0;
 
   virtual const SceneGraphNode* findNode(NodeId node) const = 0;
 
   virtual const SceneGraphEdge* findEdge(NodeId source, NodeId target) const = 0;
 
-  virtual const SceneGraphNode& getNode(NodeId node_id) const {
-    const auto node = findNode(node_id);
-    if (!node) {
-      throw std::out_of_range("missing node '" + NodeSymbol(node_id).getLabel() + "'");
-    }
-
-    return *node;
-  }
-
-  virtual const SceneGraphEdge& getEdge(NodeId source, NodeId target) const {
-    const auto edge = findEdge(source, target);
-    if (!edge) {
-      std::stringstream ss;
-      ss << "Missing edge '" << EdgeKey(source, target) << "'";
-      throw std::out_of_range(ss.str());
-    }
-
-    return *edge;
-  }
+  virtual const SceneGraphNode& getNode(NodeId node_id) const;
+
+  virtual const SceneGraphEdge& getEdge(NodeId source, NodeId target) const;
 
   /**
    * @brief Get node ids of newly inserted nodes

include/spark_dsg/bounding_box.h

@@ -35,9 +35,8 @@
 #pragma once
 
 #include <Eigen/Geometry>
-#include <iostream>
 
-#include "spark_dsg/mesh.h"
+#include "spark_dsg/spark_dsg_fwd.h"
 
 namespace spark_dsg {
 
@@ -251,16 +250,15 @@ struct BoundingBox {
  public:
   // Specialized point adaptors.
   struct MeshAdaptor : PointAdaptor {
-    MeshAdaptor(const Mesh& mesh, const std::vector<size_t>* indices = nullptr)
-        : mesh(mesh), indices(indices) {}
+    MeshAdaptor(const Mesh& mesh, const std::vector<size_t>* indices = nullptr);
     size_t size() const override;
     Eigen::Vector3f get(size_t index) const override;
     const Mesh& mesh;
     const std::vector<size_t>* indices;
   };
 
   struct PointVectorAdaptor : PointAdaptor {
-    PointVectorAdaptor(const std::vector<Eigen::Vector3f>& points) : points(points) {}
+    PointVectorAdaptor(const std::vector<Eigen::Vector3f>& points);
     size_t size() const override;
     Eigen::Vector3f get(size_t index) const override;
     const std::vector<Eigen::Vector3f>& points;

include/spark_dsg/dynamic_scene_graph.h

@@ -33,13 +33,12 @@
  * 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"
 
 namespace spark_dsg {
 
@@ -107,7 +106,9 @@ class DynamicSceneGraph {
    * @param attrs node attributes
    * @return true if the node was added successfully
    */
-  bool emplaceNode(LayerId layer_id, NodeId node_id, NodeAttributes::Ptr&& attrs);
+  bool emplaceNode(LayerId layer_id,
+                   NodeId node_id,
+                   std::unique_ptr<NodeAttributes>&& attrs);
 
   /**
    * @brief construct and add a dynamic node to the specified layer in the graph
@@ -121,7 +122,7 @@ class DynamicSceneGraph {
   bool emplaceNode(LayerId layer_id,
                    LayerPrefix prefix,
                    std::chrono::nanoseconds timestamp,
-                   NodeAttributes::Ptr&& attrs,
+                   std::unique_ptr<NodeAttributes>&& attrs,
                    bool add_edge_to_previous = true);
 
   /**
@@ -135,7 +136,7 @@ class DynamicSceneGraph {
   bool emplacePrevDynamicNode(LayerId layer_id,
                               NodeId prev_node_id,
                               std::chrono::nanoseconds timestamp,
-                              NodeAttributes::Ptr&& attrs);
+                              std::unique_ptr<NodeAttributes>&& attrs);
 
   /**
    * @brief add a node to the graph
@@ -146,7 +147,7 @@ class DynamicSceneGraph {
    * @param node to add
    * @return true if the node was added successfully
    */
-  bool insertNode(SceneGraphNode::Ptr&& node);
+  bool insertNode(std::unique_ptr<SceneGraphNode>&& node);
 
   /**
    * @brief add a node to the graph or update an existing node
@@ -159,7 +160,7 @@ class DynamicSceneGraph {
   bool addOrUpdateNode(
       LayerId layer_id,
       NodeId node_id,
-      NodeAttributes::Ptr&& attrs,
+      std::unique_ptr<NodeAttributes>&& attrs,
       std::optional<std::chrono::nanoseconds> timestamp = std::nullopt);
 
   /**
@@ -178,7 +179,7 @@ class DynamicSceneGraph {
    */
   bool insertEdge(NodeId source,
                   NodeId target,
-                  EdgeAttributes::Ptr&& edge_info = nullptr);
+                  std::unique_ptr<EdgeAttributes>&& edge_info = nullptr);
 
   /**
    * @brief Insert a parent edge between two nodes
@@ -196,7 +197,7 @@ class DynamicSceneGraph {
    */
   bool insertParentEdge(NodeId source,
                         NodeId target,
-                        EdgeAttributes::Ptr&& edge_info = nullptr);
+                        std::unique_ptr<EdgeAttributes>&& edge_info = nullptr);
 
   /**
    * @brief Add an edge to the graph or update an existing edge
@@ -205,15 +206,17 @@ class DynamicSceneGraph {
    * @param target edge target id
    * @param edge_info edge attributes
    */
-  bool addOrUpdateEdge(NodeId source, NodeId target, EdgeAttributes::Ptr&& edge_info);
+  bool addOrUpdateEdge(NodeId source,
+                       NodeId target,
+                       std::unique_ptr<EdgeAttributes>&& edge_info);
 
   /**
    * @brief Set the attributes of an existing node
    * @param node Node ID to set the attributes for
    * @param attrs New attributes for the node
    * @return Returns true if update was successful
    */
-  bool setNodeAttributes(NodeId node, NodeAttributes::Ptr&& attrs);
+  bool setNodeAttributes(NodeId node, std::unique_ptr<NodeAttributes>&& attrs);
 
   /**
    * @brief Set the attributes of an existing edge
@@ -222,7 +225,9 @@ class DynamicSceneGraph {
    * @param attrs New attributes for the edge
    * @return Returns true if update was successful
    */
-  bool setEdgeAttributes(NodeId source, NodeId target, EdgeAttributes::Ptr&& attrs);
+  bool setEdgeAttributes(NodeId source,
+                         NodeId target,
+                         std::unique_ptr<EdgeAttributes>&& attrs);
 
   /**
    * @brief Check whether the layer exists and is valid
@@ -519,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

@@ -34,7 +34,8 @@
  * -------------------------------------------------------------------------- */
 #pragma once
 #include "spark_dsg/base_layer.h"
-#include "spark_dsg/node_symbol.h"
+#include "spark_dsg/edge_container.h"
+#include "spark_dsg/layer_prefix.h"
 
 namespace spark_dsg {
 
@@ -43,7 +44,7 @@ class DynamicSceneGraphLayer : public BaseLayer {
   //! desired pointer type for the layer
   using Ptr = std::unique_ptr<DynamicSceneGraphLayer>;
   //! node container for the layer
-  using Nodes = std::vector<SceneGraphNode::Ptr>;
+  using Nodes = std::vector<std::unique_ptr<SceneGraphNode>>;
   //! edge container type for the layer
   using Edges = EdgeContainer::Edges;
 
@@ -81,27 +82,19 @@ class DynamicSceneGraphLayer : public BaseLayer {
 
   bool insertEdge(NodeId source,
                   NodeId target,
-                  EdgeAttributes::Ptr&& edge_info = nullptr) override;
+                  std::unique_ptr<EdgeAttributes>&& edge_info = nullptr) override;
 
   bool insertEdgeByIndex(size_t source_index,
                          size_t target_index,
-                         EdgeAttributes::Ptr&& edge_info = nullptr);
+                         std::unique_ptr<EdgeAttributes>&& edge_info = nullptr);
 
   bool removeEdge(NodeId source, NodeId target) override;
 
   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;
-
-  bool mergeLayer(const DynamicSceneGraphLayer& other,
+  void mergeLayer(const DynamicSceneGraphLayer& other,
                   const GraphMergeConfig& config,
-                  std::map<NodeId, LayerKey>* layer_lookup = nullptr);
+                  std::vector<NodeId>* new_nodes = nullptr);
 
   void getNewNodes(std::vector<NodeId>& new_nodes, bool clear_new) override;
 
@@ -112,14 +105,18 @@ 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,
-                   NodeAttributes::Ptr&& attrs,
+                   std::unique_ptr<NodeAttributes>&& attrs,
                    bool add_edge = true);
 
   bool emplaceNodeAtIndex(std::chrono::nanoseconds stamp,
                           size_t index,
-                          NodeAttributes::Ptr&& attrs);
+                          std::unique_ptr<NodeAttributes>&& attrs);
 
   bool removeNode(NodeId node) override;