From 62f4cfa73f5afcc24100de5f5232e0eb70d1e5b4 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 28 Jun 2024 20:42:55 +0000 Subject: [PATCH 01/18] forward declare DynamicSceneGraph where possible --- CMakeLists.txt | 5 +++++ examples/dsg_endpoint.cpp | 1 + examples/dsg_repeater.cpp | 1 + include/spark_dsg/dynamic_scene_graph_fwd.h | 7 +++++++ include/spark_dsg/scene_graph_logger.h | 8 +++++--- include/spark_dsg/scene_graph_utilities.h | 3 ++- include/spark_dsg/serialization/file_io.h | 7 ++++--- .../serialization/graph_binary_serialization.h | 6 +++--- .../spark_dsg/serialization/graph_json_serialization.h | 4 ++-- include/spark_dsg/serialization/versioning.h | 4 ++-- include/spark_dsg/zmq_interface.h | 6 +++--- python/bindings/zmq_bindings.cpp | 1 + src/scene_graph_logger.cpp | 10 ++++++---- src/scene_graph_utilities.cpp | 1 + src/serialization/file_io.cpp | 5 ++--- src/serialization/graph_binary_serialization.cpp | 1 + src/serialization/graph_json_serialization.cpp | 5 ++--- tests/utest_zmq_interface.cpp | 1 + 18 files changed, 49 insertions(+), 27 deletions(-) create mode 100644 include/spark_dsg/dynamic_scene_graph_fwd.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 1e5de81..dff4d37 100644 --- a/CMakeLists.txt +++ b/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,6 +23,10 @@ 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 diff --git a/examples/dsg_endpoint.cpp b/examples/dsg_endpoint.cpp index bb506ad..2000617 100644 --- a/examples/dsg_endpoint.cpp +++ b/examples/dsg_endpoint.cpp @@ -1,3 +1,4 @@ +#include #include #include diff --git a/examples/dsg_repeater.cpp b/examples/dsg_repeater.cpp index 272f5cd..5e0eaf6 100644 --- a/examples/dsg_repeater.cpp +++ b/examples/dsg_repeater.cpp @@ -1,3 +1,4 @@ +#include #include #include diff --git a/include/spark_dsg/dynamic_scene_graph_fwd.h b/include/spark_dsg/dynamic_scene_graph_fwd.h new file mode 100644 index 0000000..34d162a --- /dev/null +++ b/include/spark_dsg/dynamic_scene_graph_fwd.h @@ -0,0 +1,7 @@ +#pragma once +#include + +namespace spark_dsg { +class DynamicSceneGraph; +using DynamicSceneGraphPtr = std::shared_ptr; +} // namespace spark_dsg diff --git a/include/spark_dsg/scene_graph_logger.h b/include/spark_dsg/scene_graph_logger.h index 41eee7f..4c24594 100644 --- a/include/spark_dsg/scene_graph_logger.h +++ b/include/spark_dsg/scene_graph_logger.h @@ -34,9 +34,11 @@ * -------------------------------------------------------------------------- */ #pragma once +#include #include -#include "spark_dsg/dynamic_scene_graph.h" +#include "spark_dsg/dynamic_scene_graph_fwd.h" +#include "spark_dsg/scene_graph_types.h" namespace spark_dsg { @@ -50,11 +52,11 @@ class SceneGraphLogger { inline void setOutputPath(const std::string& folder) { output_dir_ = folder; } - inline void setLayerName(const LayerId& id, const std::string& name) { + inline void setLayerName(LayerId id, const std::string& name) { layer_names_.insert({id, name}); } - void logGraph(const DynamicSceneGraph::Ptr& graph); + void logGraph(const DynamicSceneGraph& graph); private: std::string output_dir_; diff --git a/include/spark_dsg/scene_graph_utilities.h b/include/spark_dsg/scene_graph_utilities.h index 24dc991..ba36e03 100644 --- a/include/spark_dsg/scene_graph_utilities.h +++ b/include/spark_dsg/scene_graph_utilities.h @@ -34,7 +34,8 @@ * -------------------------------------------------------------------------- */ #pragma once #include "spark_dsg/bounding_box.h" -#include "spark_dsg/dynamic_scene_graph.h" +#include "spark_dsg/dynamic_scene_graph_fwd.h" +#include "spark_dsg/scene_graph_types.h" namespace spark_dsg { diff --git a/include/spark_dsg/serialization/file_io.h b/include/spark_dsg/serialization/file_io.h index d43ce89..a80c9f3 100644 --- a/include/spark_dsg/serialization/file_io.h +++ b/include/spark_dsg/serialization/file_io.h @@ -34,11 +34,12 @@ * -------------------------------------------------------------------------- */ #pragma once +#include #include #include #include -#include "spark_dsg/dynamic_scene_graph.h" +#include "spark_dsg/dynamic_scene_graph_fwd.h" namespace spark_dsg::io { @@ -78,7 +79,7 @@ void saveDsgJson(const DynamicSceneGraph& graph, * @param filepath The filepath including extension to load from. * @return A pointer to the loaded graph or nullptr if loading failed. */ -DynamicSceneGraph::Ptr loadDsgJson(const std::string& filepath); +DynamicSceneGraphPtr loadDsgJson(const std::string& filepath); /** * @brief Save a DynamicSceneGraph to a file in binary serialization. @@ -95,6 +96,6 @@ void saveDsgBinary(const DynamicSceneGraph& graph, * @param filepath The filepath including extension to load from. * @return A pointer to the loaded graph or nullptr if loading failed. */ -DynamicSceneGraph::Ptr loadDsgBinary(const std::string& filepath); +DynamicSceneGraphPtr loadDsgBinary(const std::string& filepath); } // namespace spark_dsg::io diff --git a/include/spark_dsg/serialization/graph_binary_serialization.h b/include/spark_dsg/serialization/graph_binary_serialization.h index 4420911..e411b3d 100644 --- a/include/spark_dsg/serialization/graph_binary_serialization.h +++ b/include/spark_dsg/serialization/graph_binary_serialization.h @@ -34,7 +34,7 @@ * -------------------------------------------------------------------------- */ #pragma once -#include "spark_dsg/dynamic_scene_graph.h" +#include "spark_dsg/dynamic_scene_graph_fwd.h" #include "spark_dsg/serialization/versioning.h" namespace spark_dsg::io::binary { @@ -43,9 +43,9 @@ void writeGraph(const DynamicSceneGraph& graph, std::vector& buffer, bool include_mesh = false); -DynamicSceneGraph::Ptr readGraph(const uint8_t* const buffer, size_t length); +DynamicSceneGraphPtr readGraph(const uint8_t* const buffer, size_t length); -inline DynamicSceneGraph::Ptr readGraph(const std::vector& buffer) { +inline DynamicSceneGraphPtr readGraph(const std::vector& buffer) { return readGraph(buffer.data(), buffer.size()); } diff --git a/include/spark_dsg/serialization/graph_json_serialization.h b/include/spark_dsg/serialization/graph_json_serialization.h index 7838b8d..077e4e8 100644 --- a/include/spark_dsg/serialization/graph_json_serialization.h +++ b/include/spark_dsg/serialization/graph_json_serialization.h @@ -33,7 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "spark_dsg/dynamic_scene_graph.h" +#include "spark_dsg/dynamic_scene_graph_fwd.h" namespace spark_dsg::io::json { @@ -49,6 +49,6 @@ std::string writeGraph(const DynamicSceneGraph& graph, bool include_mesh = false * @param contents JSON string to parse * @returns Resulting parsed scene graph */ -DynamicSceneGraph::Ptr readGraph(const std::string& contents); +DynamicSceneGraphPtr readGraph(const std::string& contents); } // namespace spark_dsg::io::json diff --git a/include/spark_dsg/serialization/versioning.h b/include/spark_dsg/serialization/versioning.h index 6941cf8..1ac6200 100644 --- a/include/spark_dsg/serialization/versioning.h +++ b/include/spark_dsg/serialization/versioning.h @@ -37,8 +37,8 @@ #include #include #include - -#include "spark_dsg/dynamic_scene_graph.h" +#include +#include namespace spark_dsg::io { diff --git a/include/spark_dsg/zmq_interface.h b/include/spark_dsg/zmq_interface.h index 2a4c303..3b8cf7a 100644 --- a/include/spark_dsg/zmq_interface.h +++ b/include/spark_dsg/zmq_interface.h @@ -34,7 +34,7 @@ * -------------------------------------------------------------------------- */ #pragma once -#include "spark_dsg/dynamic_scene_graph.h" +#include "spark_dsg/dynamic_scene_graph_fwd.h" namespace spark_dsg { @@ -60,7 +60,7 @@ class ZmqReceiver { bool recv(size_t timeout_ms, bool recv_all = false); - DynamicSceneGraph::Ptr graph() const; + DynamicSceneGraphPtr graph() const; private: struct Detail; @@ -74,7 +74,7 @@ class ZmqGraph { ~ZmqGraph(); bool hasChange() const; - DynamicSceneGraph::Ptr graph() const; + DynamicSceneGraphPtr graph() const; private: struct Detail; diff --git a/python/bindings/zmq_bindings.cpp b/python/bindings/zmq_bindings.cpp index d69a13d..68b01b6 100644 --- a/python/bindings/zmq_bindings.cpp +++ b/python/bindings/zmq_bindings.cpp @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #include "zmq_bindings.h" +#include #include using namespace spark_dsg; diff --git a/src/scene_graph_logger.cpp b/src/scene_graph_logger.cpp index ee3bed6..61388c9 100644 --- a/src/scene_graph_logger.cpp +++ b/src/scene_graph_logger.cpp @@ -37,6 +37,8 @@ #include #include +#include "spark_dsg/dynamic_scene_graph.h" + namespace spark_dsg { void writeStatsToCsv(size_t num_active, @@ -66,10 +68,10 @@ SceneGraphLogger::SceneGraphLogger() {} SceneGraphLogger::~SceneGraphLogger() {} -void SceneGraphLogger::logGraph(const DynamicSceneGraph::Ptr& graph) { +void SceneGraphLogger::logGraph(const DynamicSceneGraph& graph) { // What I want to log: for each layer, the number of active nodes, number of // merged node, number of deleted nodes - for (const auto& id_layer : graph->layers_) { + for (const auto& id_layer : graph.layers_) { if (layer_names_.count(id_layer.first) > 0) { if (id_layer.second->numNodes() == 0 && !write_header_) { continue; @@ -84,10 +86,10 @@ void SceneGraphLogger::logGraph(const DynamicSceneGraph::Ptr& graph) { case NodeStatus::NEW: case NodeStatus::VISIBLE: num_active_nodes++; - if (graph->getNode(id_node_status.first).hasParent()) { + if (graph.getNode(id_node_status.first).hasParent()) { num_nodes_with_parents++; } - if (graph->getNode(id_node_status.first).hasChildren()) { + if (graph.getNode(id_node_status.first).hasChildren()) { num_nodes_with_children++; } break; diff --git a/src/scene_graph_utilities.cpp b/src/scene_graph_utilities.cpp index c606f74..a1f0cbe 100644 --- a/src/scene_graph_utilities.cpp +++ b/src/scene_graph_utilities.cpp @@ -35,6 +35,7 @@ #include "spark_dsg/scene_graph_utilities.h" #include "spark_dsg/bounding_box_extraction.h" +#include "spark_dsg/dynamic_scene_graph.h" namespace spark_dsg { diff --git a/src/serialization/file_io.cpp b/src/serialization/file_io.cpp index c17767e..18c34f1 100644 --- a/src/serialization/file_io.cpp +++ b/src/serialization/file_io.cpp @@ -38,7 +38,6 @@ #include #include -#include "spark_dsg/dynamic_scene_graph.h" #include "spark_dsg/logging.h" #include "spark_dsg/serialization/graph_binary_serialization.h" #include "spark_dsg/serialization/graph_json_serialization.h" @@ -96,7 +95,7 @@ void saveDsgBinary(const DynamicSceneGraph& graph, out.write(reinterpret_cast(graph_buffer.data()), graph_buffer.size()); } -DynamicSceneGraph::Ptr loadDsgBinary(const std::string& filepath) { +DynamicSceneGraphPtr loadDsgBinary(const std::string& filepath) { // Read the file into a buffer. std::ifstream infile(filepath, std::ios::in | std::ios::binary); std::vector buffer((std::istreambuf_iterator(infile)), @@ -122,7 +121,7 @@ void saveDsgJson(const DynamicSceneGraph& graph, outfile << json::writeGraph(graph, include_mesh); } -DynamicSceneGraph::Ptr loadDsgJson(const std::string& filepath) { +DynamicSceneGraphPtr loadDsgJson(const std::string& filepath) { std::ifstream infile(filepath); std::stringstream ss; ss << infile.rdbuf(); diff --git a/src/serialization/graph_binary_serialization.cpp b/src/serialization/graph_binary_serialization.cpp index dc4a7d1..30b33ab 100644 --- a/src/serialization/graph_binary_serialization.cpp +++ b/src/serialization/graph_binary_serialization.cpp @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #include "spark_dsg/serialization/graph_binary_serialization.h" +#include "spark_dsg/dynamic_scene_graph.h" #include "spark_dsg/serialization/attribute_registry.h" #include "spark_dsg/serialization/attribute_serialization.h" #include "spark_dsg/serialization/binary_conversions.h" diff --git a/src/serialization/graph_json_serialization.cpp b/src/serialization/graph_json_serialization.cpp index e401914..d0fb7c6 100644 --- a/src/serialization/graph_json_serialization.cpp +++ b/src/serialization/graph_json_serialization.cpp @@ -36,6 +36,7 @@ #include +#include "spark_dsg/dynamic_scene_graph.h" #include "spark_dsg/logging.h" #include "spark_dsg/serialization/attribute_registry.h" #include "spark_dsg/serialization/attribute_serialization.h" @@ -47,9 +48,7 @@ namespace spark_dsg { using nlohmann::json; void to_json(json& record, const SceneGraphNode& node) { - record = {{"id", node.id}, - {"layer", node.layer}, - {"attributes", node.attributes()}}; + record = {{"id", node.id}, {"layer", node.layer}, {"attributes", node.attributes()}}; if (node.timestamp) { record["timestamp"] = node.timestamp->count(); } diff --git a/tests/utest_zmq_interface.cpp b/tests/utest_zmq_interface.cpp index 283cbc8..a402954 100644 --- a/tests/utest_zmq_interface.cpp +++ b/tests/utest_zmq_interface.cpp @@ -33,6 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include +#include #include namespace spark_dsg { From 4ece615e3efa0c7de40c0881ebe0a0ef23c9a12f Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 28 Jun 2024 22:58:31 +0000 Subject: [PATCH 02/18] rework NodeSymbol and hide eigen in new file --- CMakeLists.txt | 1 + include/spark_dsg/node_symbol.h | 53 ++++------------------- include/spark_dsg/printing.h | 60 +++++++++++++++++++++++++++ include/spark_dsg/scene_graph_types.h | 14 ++----- src/bounding_box.cpp | 1 + src/node_attributes.cpp | 1 + src/node_symbol.cpp | 56 +++++++++++++++++++++++++ src/printing.cpp | 44 ++++++++++++++++++++ tests/utest_graph_utilities_layer.cpp | 1 + tests/utest_scene_graph_utilities.cpp | 1 + 10 files changed, 176 insertions(+), 56 deletions(-) create mode 100644 include/spark_dsg/printing.h create mode 100644 src/printing.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index dff4d37..6afe944 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -41,6 +41,7 @@ add_library( 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 diff --git a/include/spark_dsg/node_symbol.h b/include/spark_dsg/node_symbol.h index 9c16847..37981b7 100644 --- a/include/spark_dsg/node_symbol.h +++ b/include/spark_dsg/node_symbol.h @@ -33,9 +33,6 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include -#include - #include "spark_dsg/scene_graph_types.h" namespace spark_dsg { @@ -59,39 +56,28 @@ class NodeSymbol { NodeSymbol(NodeId value); //! cast the symobl directly to a node ID - inline operator NodeId() const { return value_.value; } + operator NodeId() const; /** * @brief Get the index of the node in the specific category * @returns x where (_, x) were the arguments to create the symbol */ - inline NodeId categoryId() const { return value_.symbol.index; } + NodeId categoryId() const; /** * @brief Get the category of the node * @returns c where (c, _) were the arguments to create the symbol */ - inline char category() const { return value_.symbol.key; } + char category() const; //! pre-increment the index portion of the symbol - NodeSymbol& operator++() { - value_.symbol.index++; - return *this; - } + NodeSymbol& operator++(); //! post-increment the index portion of the symbol - NodeSymbol operator++(int) { - NodeSymbol old = *this; - value_.symbol.index++; - return old; - } + NodeSymbol operator++(int); //! get a string representation of the symbol - inline std::string getLabel() const { - std::stringstream ss; - ss << *this; - return ss.str(); - } + std::string getLabel() const; /** * @brief output node symbol information @@ -111,31 +97,6 @@ class NodeSymbol { } value_; }; -inline NodeSymbol operator"" _id(const char* str, size_t size) { - if (size < 1) { - throw std::domain_error("invalid literal: must have at least two characters"); - } - - char prefix = str[0]; - std::string number(str + 1, size - 1); - size_t index = std::stoull(number); - return NodeSymbol(prefix, index); -} - -template -std::string displayNodeSymbolContainer(const Container& set) { - std::stringstream ss; - ss << "["; - auto iter = set.begin(); - while (iter != set.end()) { - ss << NodeSymbol(*iter).getLabel(); - ++iter; - if (iter != set.end()) { - ss << ", "; - } - } - ss << "]"; - return ss.str(); -} +NodeSymbol operator"" _id(const char* str, size_t size); } // namespace spark_dsg diff --git a/include/spark_dsg/printing.h b/include/spark_dsg/printing.h new file mode 100644 index 0000000..451970b --- /dev/null +++ b/include/spark_dsg/printing.h @@ -0,0 +1,60 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once +#include + +#include "spark_dsg/node_symbol.h" + +namespace spark_dsg { + +Eigen::IOFormat getDefaultVectorFormat(); + +template +std::string displayNodeSymbolContainer(const Container& set) { + std::stringstream ss; + ss << "["; + auto iter = set.begin(); + while (iter != set.end()) { + ss << NodeSymbol(*iter).getLabel(); + ++iter; + if (iter != set.end()) { + ss << ", "; + } + } + ss << "]"; + return ss.str(); +} + +} // namespace spark_dsg diff --git a/include/spark_dsg/scene_graph_types.h b/include/spark_dsg/scene_graph_types.h index 01d6808..c666b83 100644 --- a/include/spark_dsg/scene_graph_types.h +++ b/include/spark_dsg/scene_graph_types.h @@ -33,7 +33,6 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include #include #include #include @@ -118,10 +117,10 @@ class LayerPrefix { * @note A higher layer id corresponds to parents for interlayer edges */ struct DsgLayers { - inline const static LayerId SEGMENTS = 1; //< Pre-Object node layer (static) - inline const static LayerId OBJECTS = 2; //< Object node layer (static) - inline const static LayerId AGENTS = 2; //< Agents layer (dynamic) - inline const static LayerId PLACES = 3; //< Places node layer (as well as structure) + inline const static LayerId SEGMENTS = 1; //< Pre-Object node layer (static) + inline const static LayerId OBJECTS = 2; //< Object node layer (static) + inline const static LayerId AGENTS = 2; //< Agents layer (dynamic) + inline const static LayerId PLACES = 3; //< Places node layer (as well as structure) inline const static LayerId STRUCTURE = 3; //< Struct node layer (as well as places) inline const static LayerId ROOMS = 4; //< Room node layer inline const static LayerId BUILDINGS = 5; //< Building node layer @@ -132,9 +131,4 @@ struct DsgLayers { static LayerId StringToLayerId(const std::string& id_str); }; -inline Eigen::IOFormat getDefaultVectorFormat() { - return Eigen::IOFormat( - Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n", "[", "]"); -} - } // namespace spark_dsg diff --git a/src/bounding_box.cpp b/src/bounding_box.cpp index 74dbe97..278793a 100644 --- a/src/bounding_box.cpp +++ b/src/bounding_box.cpp @@ -35,6 +35,7 @@ #include "spark_dsg/bounding_box.h" #include "spark_dsg/bounding_box_extraction.h" +#include "spark_dsg/printing.h" #include "spark_dsg/scene_graph_types.h" namespace spark_dsg { diff --git a/src/node_attributes.cpp b/src/node_attributes.cpp index 306e06d..17963b5 100644 --- a/src/node_attributes.cpp +++ b/src/node_attributes.cpp @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #include "spark_dsg/node_attributes.h" +#include "spark_dsg/printing.h" #include "spark_dsg/serialization/attribute_serialization.h" #include "spark_dsg/serialization/binary_conversions.h" #include "spark_dsg/serialization/json_conversions.h" diff --git a/src/node_symbol.cpp b/src/node_symbol.cpp index ee5a9a8..366965a 100644 --- a/src/node_symbol.cpp +++ b/src/node_symbol.cpp @@ -34,6 +34,9 @@ * -------------------------------------------------------------------------- */ #include "spark_dsg/node_symbol.h" +#include +#include + namespace spark_dsg { NodeSymbol::NodeSymbol(char key, NodeId index) { @@ -43,6 +46,59 @@ NodeSymbol::NodeSymbol(char key, NodeId index) { NodeSymbol::NodeSymbol(NodeId value) { value_.value = value; } + NodeSymbol::operator NodeId() const { return value_.value; } + + NodeId NodeSymbol::categoryId() const { return value_.symbol.index; } + + char NodeSymbol::category() const { return value_.symbol.key; } + + NodeSymbol& NodeSymbol::operator++() { + value_.symbol.index++; + return *this; + } + + NodeSymbol NodeSymbol::operator++(int) { + NodeSymbol old = *this; + value_.symbol.index++; + return old; + } + + std::string NodeSymbol::getLabel() const { + std::stringstream ss; + ss << *this; + return ss.str(); + } + + +NodeSymbol operator"" _id(const char* str, size_t size) { + if (size < 1) { + throw std::domain_error("invalid literal: must have at least two characters"); + } + + char prefix = str[0]; + std::string number(str + 1, size - 1); + size_t index = std::stoull(number); + return NodeSymbol(prefix, index); +} + +/* +template +std::string displayNodeSymbolContainer(const Container& set) { + std::stringstream ss; + ss << "["; + auto iter = set.begin(); + while (iter != set.end()) { + ss << NodeSymbol(*iter).getLabel(); + ++iter; + if (iter != set.end()) { + ss << ", "; + } + } + ss << "]"; + return ss.str(); +} +*/ + std::ostream& operator<<(std::ostream& out, const NodeSymbol& symbol) { if (std::isalpha(symbol.value_.symbol.key)) { out << symbol.value_.symbol.key << "(" << symbol.value_.symbol.index << ")"; diff --git a/src/printing.cpp b/src/printing.cpp new file mode 100644 index 0000000..5c3784c --- /dev/null +++ b/src/printing.cpp @@ -0,0 +1,44 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "spark_dsg/printing.h" + +namespace spark_dsg { + +Eigen::IOFormat getDefaultVectorFormat() { + return Eigen::IOFormat( + Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n", "[", "]"); +} + +} // namespace spark_dsg diff --git a/tests/utest_graph_utilities_layer.cpp b/tests/utest_graph_utilities_layer.cpp index ee791bd..d2cf0df 100644 --- a/tests/utest_graph_utilities_layer.cpp +++ b/tests/utest_graph_utilities_layer.cpp @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #include #include +#include #include namespace spark_dsg { diff --git a/tests/utest_scene_graph_utilities.cpp b/tests/utest_scene_graph_utilities.cpp index d6b5eeb..d4e3293 100644 --- a/tests/utest_scene_graph_utilities.cpp +++ b/tests/utest_scene_graph_utilities.cpp @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #include #include +#include #include "spark_dsg_tests/type_comparisons.h" From c8ff9f881266c54f87d5f7de91f2cd6de7865afc Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 28 Jun 2024 23:31:46 +0000 Subject: [PATCH 03/18] forward declare node attributes and some bounding boxes --- examples/dsg_endpoint.cpp | 1 + examples/dsg_repeater.cpp | 1 + include/spark_dsg/dynamic_scene_graph.h | 13 ++++++++----- include/spark_dsg/dynamic_scene_graph_layer.h | 5 +++-- include/spark_dsg/scene_graph_layer.h | 2 +- include/spark_dsg/scene_graph_logger.h | 2 +- include/spark_dsg/scene_graph_node.h | 13 +++++-------- include/spark_dsg/scene_graph_utilities.h | 2 +- include/spark_dsg/serialization/file_io.h | 2 +- .../graph_binary_serialization.h | 2 +- .../serialization/graph_json_serialization.h | 2 +- .../serialization/json_conversions.h | 13 ++----------- ...amic_scene_graph_fwd.h => spark_dsg_fwd.h} | 2 ++ include/spark_dsg/zmq_interface.h | 2 +- src/dynamic_scene_graph.cpp | 19 +++++++++++-------- src/dynamic_scene_graph_layer.cpp | 7 ++++--- src/scene_graph_layer.cpp | 4 +++- src/scene_graph_node.cpp | 4 ++-- .../graph_binary_serialization.cpp | 1 + .../graph_json_serialization.cpp | 1 + src/serialization/json_conversions.cpp | 11 ++++++++++- tests/utest_adjacency_matrix.cpp | 1 + tests/utest_dynamic_scene_graph.cpp | 1 + tests/utest_dynamic_scene_graph_layer.cpp | 3 ++- tests/utest_graph_utilities_layer.cpp | 1 + tests/utest_scene_graph_layer.cpp | 3 ++- tests/utest_scene_graph_node.cpp | 3 ++- tests/utest_scene_graph_utilities.cpp | 3 ++- 28 files changed, 72 insertions(+), 52 deletions(-) rename include/spark_dsg/{dynamic_scene_graph_fwd.h => spark_dsg_fwd.h} (87%) diff --git a/examples/dsg_endpoint.cpp b/examples/dsg_endpoint.cpp index 2000617..c1931fc 100644 --- a/examples/dsg_endpoint.cpp +++ b/examples/dsg_endpoint.cpp @@ -2,6 +2,7 @@ #include #include +#include #include auto main(int argc, char* argv[]) -> int { diff --git a/examples/dsg_repeater.cpp b/examples/dsg_repeater.cpp index 5e0eaf6..b59abab 100644 --- a/examples/dsg_repeater.cpp +++ b/examples/dsg_repeater.cpp @@ -2,6 +2,7 @@ #include #include +#include #include auto main(int argc, char* argv[]) -> int { diff --git a/include/spark_dsg/dynamic_scene_graph.h b/include/spark_dsg/dynamic_scene_graph.h index 5282165..3329abf 100644 --- a/include/spark_dsg/dynamic_scene_graph.h +++ b/include/spark_dsg/dynamic_scene_graph.h @@ -40,6 +40,7 @@ #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 +108,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&& attrs); /** * @brief construct and add a dynamic node to the specified layer in the graph @@ -121,7 +124,7 @@ class DynamicSceneGraph { bool emplaceNode(LayerId layer_id, LayerPrefix prefix, std::chrono::nanoseconds timestamp, - NodeAttributes::Ptr&& attrs, + std::unique_ptr&& attrs, bool add_edge_to_previous = true); /** @@ -135,7 +138,7 @@ class DynamicSceneGraph { bool emplacePrevDynamicNode(LayerId layer_id, NodeId prev_node_id, std::chrono::nanoseconds timestamp, - NodeAttributes::Ptr&& attrs); + std::unique_ptr&& attrs); /** * @brief add a node to the graph @@ -159,7 +162,7 @@ class DynamicSceneGraph { bool addOrUpdateNode( LayerId layer_id, NodeId node_id, - NodeAttributes::Ptr&& attrs, + std::unique_ptr&& attrs, std::optional timestamp = std::nullopt); /** @@ -213,7 +216,7 @@ class DynamicSceneGraph { * @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&& attrs); /** * @brief Set the attributes of an existing edge diff --git a/include/spark_dsg/dynamic_scene_graph_layer.h b/include/spark_dsg/dynamic_scene_graph_layer.h index 2523c35..9f32b9a 100644 --- a/include/spark_dsg/dynamic_scene_graph_layer.h +++ b/include/spark_dsg/dynamic_scene_graph_layer.h @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #pragma once #include "spark_dsg/base_layer.h" +#include "spark_dsg/spark_dsg_fwd.h" #include "spark_dsg/node_symbol.h" namespace spark_dsg { @@ -114,12 +115,12 @@ class DynamicSceneGraphLayer : public BaseLayer { protected: bool emplaceNode(std::chrono::nanoseconds timestamp, - NodeAttributes::Ptr&& attrs, + std::unique_ptr&& attrs, bool add_edge = true); bool emplaceNodeAtIndex(std::chrono::nanoseconds stamp, size_t index, - NodeAttributes::Ptr&& attrs); + std::unique_ptr&& attrs); bool removeNode(NodeId node) override; diff --git a/include/spark_dsg/scene_graph_layer.h b/include/spark_dsg/scene_graph_layer.h index 2507db8..0bd9252 100644 --- a/include/spark_dsg/scene_graph_layer.h +++ b/include/spark_dsg/scene_graph_layer.h @@ -246,7 +246,7 @@ class SceneGraphLayer : public BaseLayer { * @param attrs node attributes * @returns true if emplace into internal map was successful */ - bool emplaceNode(NodeId node_id, NodeAttributes::Ptr&& attrs); + bool emplaceNode(NodeId node_id, std::unique_ptr&& attrs); /** * @brief add a node to the layer diff --git a/include/spark_dsg/scene_graph_logger.h b/include/spark_dsg/scene_graph_logger.h index 4c24594..72bac55 100644 --- a/include/spark_dsg/scene_graph_logger.h +++ b/include/spark_dsg/scene_graph_logger.h @@ -37,8 +37,8 @@ #include #include -#include "spark_dsg/dynamic_scene_graph_fwd.h" #include "spark_dsg/scene_graph_types.h" +#include "spark_dsg/spark_dsg_fwd.h" namespace spark_dsg { diff --git a/include/spark_dsg/scene_graph_node.h b/include/spark_dsg/scene_graph_node.h index 5c944d1..db55146 100644 --- a/include/spark_dsg/scene_graph_node.h +++ b/include/spark_dsg/scene_graph_node.h @@ -33,17 +33,15 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include #include #include #include #include #include -#include -#include +#include -#include "spark_dsg/node_attributes.h" #include "spark_dsg/scene_graph_types.h" +#include "spark_dsg/spark_dsg_fwd.h" namespace spark_dsg { @@ -65,7 +63,6 @@ enum class NodeStatus { NEW, VISIBLE, MERGED, DELETED, NONEXISTENT }; */ class SceneGraphNode { public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW //! desired pointer type of the node (unique) using Ptr = std::unique_ptr; friend class DynamicSceneGraphLayer; @@ -82,7 +79,7 @@ class SceneGraphNode { * @param layer the layer that the node will belong to * @param attrs attributes for the node */ - SceneGraphNode(NodeId id, LayerId layer, NodeAttributes::Ptr&& attrs); + SceneGraphNode(NodeId id, LayerId layer, std::unique_ptr&& attrs); /** * @brief Make a scene graph node (with a timestamp) @@ -96,7 +93,7 @@ class SceneGraphNode { SceneGraphNode(NodeId id, LayerId layer, std::chrono::nanoseconds timestamp, - NodeAttributes::Ptr&& attrs); + std::unique_ptr&& attrs); SceneGraphNode(const SceneGraphNode& other) = delete; @@ -193,7 +190,7 @@ class SceneGraphNode { protected: //! pointer to attributes - NodeAttributes::Ptr attributes_; + std::unique_ptr attributes_; //! node id of parent (if valid) std::set parents_; diff --git a/include/spark_dsg/scene_graph_utilities.h b/include/spark_dsg/scene_graph_utilities.h index ba36e03..7394022 100644 --- a/include/spark_dsg/scene_graph_utilities.h +++ b/include/spark_dsg/scene_graph_utilities.h @@ -34,8 +34,8 @@ * -------------------------------------------------------------------------- */ #pragma once #include "spark_dsg/bounding_box.h" -#include "spark_dsg/dynamic_scene_graph_fwd.h" #include "spark_dsg/scene_graph_types.h" +#include "spark_dsg/spark_dsg_fwd.h" namespace spark_dsg { diff --git a/include/spark_dsg/serialization/file_io.h b/include/spark_dsg/serialization/file_io.h index a80c9f3..12ee746 100644 --- a/include/spark_dsg/serialization/file_io.h +++ b/include/spark_dsg/serialization/file_io.h @@ -39,7 +39,7 @@ #include #include -#include "spark_dsg/dynamic_scene_graph_fwd.h" +#include "spark_dsg/spark_dsg_fwd.h" namespace spark_dsg::io { diff --git a/include/spark_dsg/serialization/graph_binary_serialization.h b/include/spark_dsg/serialization/graph_binary_serialization.h index e411b3d..e320994 100644 --- a/include/spark_dsg/serialization/graph_binary_serialization.h +++ b/include/spark_dsg/serialization/graph_binary_serialization.h @@ -34,7 +34,7 @@ * -------------------------------------------------------------------------- */ #pragma once -#include "spark_dsg/dynamic_scene_graph_fwd.h" +#include "spark_dsg/spark_dsg_fwd.h" #include "spark_dsg/serialization/versioning.h" namespace spark_dsg::io::binary { diff --git a/include/spark_dsg/serialization/graph_json_serialization.h b/include/spark_dsg/serialization/graph_json_serialization.h index 077e4e8..d010510 100644 --- a/include/spark_dsg/serialization/graph_json_serialization.h +++ b/include/spark_dsg/serialization/graph_json_serialization.h @@ -33,7 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "spark_dsg/dynamic_scene_graph_fwd.h" +#include "spark_dsg/spark_dsg_fwd.h" namespace spark_dsg::io::json { diff --git a/include/spark_dsg/serialization/json_conversions.h b/include/spark_dsg/serialization/json_conversions.h index 84c264a..826ac62 100644 --- a/include/spark_dsg/serialization/json_conversions.h +++ b/include/spark_dsg/serialization/json_conversions.h @@ -34,21 +34,12 @@ * -------------------------------------------------------------------------- */ #pragma once +#include #include -#include "spark_dsg/bounding_box.h" - namespace spark_dsg { - -NLOHMANN_JSON_SERIALIZE_ENUM(BoundingBox::Type, - { - {BoundingBox::Type::INVALID, "INVALID"}, - {BoundingBox::Type::AABB, "AABB"}, - {BoundingBox::Type::RAABB, "RAABB"}, - {BoundingBox::Type::OBB, "OBB"}, - }); - +struct BoundingBox; void to_json(nlohmann::json& j, const BoundingBox& b); void from_json(const nlohmann::json& j, BoundingBox& b); diff --git a/include/spark_dsg/dynamic_scene_graph_fwd.h b/include/spark_dsg/spark_dsg_fwd.h similarity index 87% rename from include/spark_dsg/dynamic_scene_graph_fwd.h rename to include/spark_dsg/spark_dsg_fwd.h index 34d162a..1e6e8f1 100644 --- a/include/spark_dsg/dynamic_scene_graph_fwd.h +++ b/include/spark_dsg/spark_dsg_fwd.h @@ -2,6 +2,8 @@ #include namespace spark_dsg { +struct NodeAttributes; + class DynamicSceneGraph; using DynamicSceneGraphPtr = std::shared_ptr; } // namespace spark_dsg diff --git a/include/spark_dsg/zmq_interface.h b/include/spark_dsg/zmq_interface.h index 3b8cf7a..0115a9b 100644 --- a/include/spark_dsg/zmq_interface.h +++ b/include/spark_dsg/zmq_interface.h @@ -34,7 +34,7 @@ * -------------------------------------------------------------------------- */ #pragma once -#include "spark_dsg/dynamic_scene_graph_fwd.h" +#include "spark_dsg/spark_dsg_fwd.h" namespace spark_dsg { diff --git a/src/dynamic_scene_graph.cpp b/src/dynamic_scene_graph.cpp index 1d068e1..d2248fb 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/node_attributes.h" #include "spark_dsg/serialization/file_io.h" namespace spark_dsg { @@ -98,7 +99,7 @@ bool DynamicSceneGraph::createDynamicLayer(LayerId layer, LayerPrefix layer_pref bool DynamicSceneGraph::emplaceNode(LayerId layer_id, NodeId node_id, - NodeAttributes::Ptr&& attrs) { + std::unique_ptr&& attrs) { if (node_lookup_.count(node_id)) { return false; } @@ -119,7 +120,7 @@ bool DynamicSceneGraph::emplaceNode(LayerId layer_id, bool DynamicSceneGraph::emplaceNode(LayerId layer, LayerPrefix prefix, std::chrono::nanoseconds time, - NodeAttributes::Ptr&& attrs, + std::unique_ptr&& attrs, bool add_edge) { bool has_layer = false; NodeSymbol new_node_id = prefix.makeId(0); @@ -146,10 +147,11 @@ bool DynamicSceneGraph::emplaceNode(LayerId layer, return true; } -bool DynamicSceneGraph::emplacePrevDynamicNode(LayerId layer, - NodeId prev_node_id, - std::chrono::nanoseconds time, - NodeAttributes::Ptr&& attrs) { +bool DynamicSceneGraph::emplacePrevDynamicNode( + LayerId layer, + NodeId prev_node_id, + std::chrono::nanoseconds time, + std::unique_ptr&& attrs) { if (hasNode(prev_node_id)) { SG_LOG(ERROR) << "scene graph already contains node " << NodeSymbol(prev_node_id).getLabel() << std::endl; @@ -198,7 +200,7 @@ bool DynamicSceneGraph::insertNode(Node::Ptr&& node) { bool DynamicSceneGraph::addOrUpdateNode(LayerId layer_id, NodeId node_id, - NodeAttributes::Ptr&& attrs, + std::unique_ptr&& attrs, std::optional stamp) { if (!layers_.count(layer_id)) { SG_LOG(WARNING) << "Invalid layer: " << layer_id << std::endl; @@ -295,7 +297,8 @@ bool DynamicSceneGraph::addOrUpdateEdge(NodeId source, } } -bool DynamicSceneGraph::setNodeAttributes(NodeId node, NodeAttributes::Ptr&& attrs) { +bool DynamicSceneGraph::setNodeAttributes(NodeId node, + std::unique_ptr&& attrs) { auto iter = node_lookup_.find(node); if (iter == node_lookup_.end()) { return false; diff --git a/src/dynamic_scene_graph_layer.cpp b/src/dynamic_scene_graph_layer.cpp index b6d770c..c2108ae 100644 --- a/src/dynamic_scene_graph_layer.cpp +++ b/src/dynamic_scene_graph_layer.cpp @@ -36,6 +36,7 @@ #include "spark_dsg/edge_attributes.h" #include "spark_dsg/logging.h" +#include "spark_dsg/node_attributes.h" namespace spark_dsg { @@ -88,7 +89,7 @@ bool DynamicSceneGraphLayer::mergeLayer(const DynamicSceneGraphLayer& other, } bool DynamicSceneGraphLayer::emplaceNode(std::chrono::nanoseconds stamp, - NodeAttributes::Ptr&& attrs, + std::unique_ptr&& attrs, bool add_edge) { if (times_.count(stamp.count())) { return false; @@ -112,7 +113,7 @@ bool DynamicSceneGraphLayer::emplaceNode(std::chrono::nanoseconds stamp, bool DynamicSceneGraphLayer::emplaceNodeAtIndex(std::chrono::nanoseconds stamp, size_t index, - NodeAttributes::Ptr&& attrs) { + std::unique_ptr&& a) { if (hasNodeByIndex(index)) { return false; } @@ -127,7 +128,7 @@ bool DynamicSceneGraphLayer::emplaceNodeAtIndex(std::chrono::nanoseconds stamp, const NodeId new_id = prefix.makeId(index); times_.insert(stamp.count()); - nodes_[index] = std::make_unique(new_id, id, stamp, std::move(attrs)); + nodes_[index] = std::make_unique(new_id, id, stamp, std::move(a)); node_status_[index] = NodeStatus::NEW; return true; } diff --git a/src/scene_graph_layer.cpp b/src/scene_graph_layer.cpp index d6b37a9..82919da 100644 --- a/src/scene_graph_layer.cpp +++ b/src/scene_graph_layer.cpp @@ -39,6 +39,7 @@ #include "spark_dsg/edge_attributes.h" #include "spark_dsg/logging.h" +#include "spark_dsg/node_attributes.h" namespace spark_dsg { @@ -47,7 +48,8 @@ using Edge = SceneGraphEdge; SceneGraphLayer::SceneGraphLayer(LayerId layer_id) : id(layer_id) {} -bool SceneGraphLayer::emplaceNode(NodeId node_id, NodeAttributes::Ptr&& attrs) { +bool SceneGraphLayer::emplaceNode(NodeId node_id, + std::unique_ptr&& attrs) { nodes_status_[node_id] = NodeStatus::NEW; return nodes_.emplace(node_id, std::make_unique(node_id, id, std::move(attrs))) .second; diff --git a/src/scene_graph_node.cpp b/src/scene_graph_node.cpp index dc7791b..d0e4157 100644 --- a/src/scene_graph_node.cpp +++ b/src/scene_graph_node.cpp @@ -41,13 +41,13 @@ namespace spark_dsg { SceneGraphNode::SceneGraphNode(NodeId node_id, LayerId layer_id, - NodeAttributes::Ptr&& attrs) + std::unique_ptr&& attrs) : id(node_id), layer(layer_id), attributes_(std::move(attrs)) {} SceneGraphNode::SceneGraphNode(NodeId node_id, LayerId layer_id, std::chrono::nanoseconds timestamp, - NodeAttributes::Ptr&& attrs) + std::unique_ptr&& attrs) : id(node_id), layer(layer_id), timestamp(timestamp), diff --git a/src/serialization/graph_binary_serialization.cpp b/src/serialization/graph_binary_serialization.cpp index 30b33ab..eaba40a 100644 --- a/src/serialization/graph_binary_serialization.cpp +++ b/src/serialization/graph_binary_serialization.cpp @@ -35,6 +35,7 @@ #include "spark_dsg/serialization/graph_binary_serialization.h" #include "spark_dsg/dynamic_scene_graph.h" +#include "spark_dsg/node_attributes.h" #include "spark_dsg/serialization/attribute_registry.h" #include "spark_dsg/serialization/attribute_serialization.h" #include "spark_dsg/serialization/binary_conversions.h" diff --git a/src/serialization/graph_json_serialization.cpp b/src/serialization/graph_json_serialization.cpp index d0fb7c6..00460cf 100644 --- a/src/serialization/graph_json_serialization.cpp +++ b/src/serialization/graph_json_serialization.cpp @@ -38,6 +38,7 @@ #include "spark_dsg/dynamic_scene_graph.h" #include "spark_dsg/logging.h" +#include "spark_dsg/node_attributes.h" #include "spark_dsg/serialization/attribute_registry.h" #include "spark_dsg/serialization/attribute_serialization.h" #include "spark_dsg/serialization/json_conversions.h" diff --git a/src/serialization/json_conversions.cpp b/src/serialization/json_conversions.cpp index b59d533..833b2b2 100644 --- a/src/serialization/json_conversions.cpp +++ b/src/serialization/json_conversions.cpp @@ -36,6 +36,7 @@ #include +#include "spark_dsg/bounding_box.h" #include "spark_dsg/edge_attributes.h" #include "spark_dsg/logging.h" #include "spark_dsg/mesh.h" @@ -47,6 +48,14 @@ namespace spark_dsg { using nlohmann::json; +NLOHMANN_JSON_SERIALIZE_ENUM(BoundingBox::Type, + { + {BoundingBox::Type::INVALID, "INVALID"}, + {BoundingBox::Type::AABB, "AABB"}, + {BoundingBox::Type::RAABB, "RAABB"}, + {BoundingBox::Type::OBB, "OBB"}, + }); + void to_json(json& j, const BoundingBox& b) { j = json{{"type", b.type}, {"dimensions", b.dimensions}, @@ -114,7 +123,7 @@ void to_json(json& record, const Color& c) { void from_json(const json& record, Color& c) { // Support scene graphs 1.0.2 and earlier where colors were encoded as vectors. - // TODO(lschmid): Remove this in the future. + // TODO(lschmid): Remove this in the future. if (record.is_array()) { io::warnOutdatedHeader(io::GlobalInfo::loadedHeader()); c.r = record.at(0).get(); diff --git a/tests/utest_adjacency_matrix.cpp b/tests/utest_adjacency_matrix.cpp index 7b5c799..52ca770 100644 --- a/tests/utest_adjacency_matrix.cpp +++ b/tests/utest_adjacency_matrix.cpp @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #include #include +#include #include "spark_dsg/logging.h" diff --git a/tests/utest_dynamic_scene_graph.cpp b/tests/utest_dynamic_scene_graph.cpp index 19a5659..41b64a2 100644 --- a/tests/utest_dynamic_scene_graph.cpp +++ b/tests/utest_dynamic_scene_graph.cpp @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #include #include +#include namespace spark_dsg { diff --git a/tests/utest_dynamic_scene_graph_layer.cpp b/tests/utest_dynamic_scene_graph_layer.cpp index 2800c8b..5e066ca 100644 --- a/tests/utest_dynamic_scene_graph_layer.cpp +++ b/tests/utest_dynamic_scene_graph_layer.cpp @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #include #include +#include namespace spark_dsg { @@ -233,7 +234,7 @@ TEST(DynamicSceneGraphLayerTests, getPositionCorrect) { using namespace std::chrono_literals; Eigen::Vector3d expected; expected << 1.0, 2.0, 3.0; - NodeAttributes::Ptr attrs = std::make_unique(expected); + auto attrs = std::make_unique(expected); TestableDynamicLayer layer(1, 0); layer.emplaceNode(1s, std::move(attrs)); diff --git a/tests/utest_graph_utilities_layer.cpp b/tests/utest_graph_utilities_layer.cpp index d2cf0df..87acfcd 100644 --- a/tests/utest_graph_utilities_layer.cpp +++ b/tests/utest_graph_utilities_layer.cpp @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #include #include +#include #include #include diff --git a/tests/utest_scene_graph_layer.cpp b/tests/utest_scene_graph_layer.cpp index 4d3a439..133f0a2 100644 --- a/tests/utest_scene_graph_layer.cpp +++ b/tests/utest_scene_graph_layer.cpp @@ -33,6 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include +#include #include namespace spark_dsg { @@ -375,7 +376,7 @@ TEST(SceneGraphLayerTests, MergeLayerCorrect) { TEST(SceneGraphLayerTests, getPositionCorrect) { Eigen::Vector3d expected; expected << 1.0, 2.0, 3.0; - NodeAttributes::Ptr attrs = std::make_unique(expected); + auto attrs = std::make_unique(expected); IsolatedSceneGraphLayer layer(1); layer.emplaceNode(NodeSymbol('x', 0), std::move(attrs)); diff --git a/tests/utest_scene_graph_node.cpp b/tests/utest_scene_graph_node.cpp index 9bf87e9..fe6bf27 100644 --- a/tests/utest_scene_graph_node.cpp +++ b/tests/utest_scene_graph_node.cpp @@ -33,13 +33,14 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include +#include #include using namespace spark_dsg; // Check that a default node has no edges TEST(SceneGraphNodeTests, DefaultNodeIvariants) { - NodeAttributes::Ptr attrs = std::make_unique(); + auto attrs = std::make_unique(); SceneGraphNode a(0, 0, std::move(attrs)); EXPECT_FALSE(a.hasSiblings()); EXPECT_FALSE(a.hasChildren()); diff --git a/tests/utest_scene_graph_utilities.cpp b/tests/utest_scene_graph_utilities.cpp index d4e3293..450566a 100644 --- a/tests/utest_scene_graph_utilities.cpp +++ b/tests/utest_scene_graph_utilities.cpp @@ -33,8 +33,9 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include -#include +#include #include +#include #include "spark_dsg_tests/type_comparisons.h" From 946fba3f5bdb2bc7fc9e39885b185900e72a7c37 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 28 Jun 2024 23:42:41 +0000 Subject: [PATCH 04/18] forward declare mesh for bounding box --- include/spark_dsg/bounding_box.h | 3 +-- include/spark_dsg/dynamic_scene_graph_layer.h | 4 +++- include/spark_dsg/scene_graph_layer.h | 1 + include/spark_dsg/serialization/attribute_registry.h | 1 - include/spark_dsg/serialization/attribute_serialization.h | 1 - include/spark_dsg/spark_dsg_fwd.h | 1 + src/bounding_box.cpp | 3 +++ 7 files changed, 9 insertions(+), 5 deletions(-) diff --git a/include/spark_dsg/bounding_box.h b/include/spark_dsg/bounding_box.h index 4ec43c2..657109d 100644 --- a/include/spark_dsg/bounding_box.h +++ b/include/spark_dsg/bounding_box.h @@ -35,9 +35,8 @@ #pragma once #include -#include -#include "spark_dsg/mesh.h" +#include "spark_dsg/spark_dsg_fwd.h" namespace spark_dsg { diff --git a/include/spark_dsg/dynamic_scene_graph_layer.h b/include/spark_dsg/dynamic_scene_graph_layer.h index 9f32b9a..ab0f083 100644 --- a/include/spark_dsg/dynamic_scene_graph_layer.h +++ b/include/spark_dsg/dynamic_scene_graph_layer.h @@ -33,9 +33,11 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once +#include + #include "spark_dsg/base_layer.h" -#include "spark_dsg/spark_dsg_fwd.h" #include "spark_dsg/node_symbol.h" +#include "spark_dsg/spark_dsg_fwd.h" namespace spark_dsg { diff --git a/include/spark_dsg/scene_graph_layer.h b/include/spark_dsg/scene_graph_layer.h index 0bd9252..79677e0 100644 --- a/include/spark_dsg/scene_graph_layer.h +++ b/include/spark_dsg/scene_graph_layer.h @@ -33,6 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once +#include #include #include #include diff --git a/include/spark_dsg/serialization/attribute_registry.h b/include/spark_dsg/serialization/attribute_registry.h index 460f8e2..a43183b 100644 --- a/include/spark_dsg/serialization/attribute_registry.h +++ b/include/spark_dsg/serialization/attribute_registry.h @@ -37,7 +37,6 @@ #include #include #include -#include #include #include diff --git a/include/spark_dsg/serialization/attribute_serialization.h b/include/spark_dsg/serialization/attribute_serialization.h index 5d52d7f..2ca14fd 100644 --- a/include/spark_dsg/serialization/attribute_serialization.h +++ b/include/spark_dsg/serialization/attribute_serialization.h @@ -36,7 +36,6 @@ #include #include -#include #include "spark_dsg/serialization/attribute_registry.h" #include "spark_dsg/serialization/binary_serialization.h" diff --git a/include/spark_dsg/spark_dsg_fwd.h b/include/spark_dsg/spark_dsg_fwd.h index 1e6e8f1..32904fa 100644 --- a/include/spark_dsg/spark_dsg_fwd.h +++ b/include/spark_dsg/spark_dsg_fwd.h @@ -2,6 +2,7 @@ #include namespace spark_dsg { +class Mesh; struct NodeAttributes; class DynamicSceneGraph; diff --git a/src/bounding_box.cpp b/src/bounding_box.cpp index 278793a..c682baa 100644 --- a/src/bounding_box.cpp +++ b/src/bounding_box.cpp @@ -34,7 +34,10 @@ * -------------------------------------------------------------------------- */ #include "spark_dsg/bounding_box.h" +#include + #include "spark_dsg/bounding_box_extraction.h" +#include "spark_dsg/mesh.h" #include "spark_dsg/printing.h" #include "spark_dsg/scene_graph_types.h" From 4ab5092565495d88a286a63d7e7b568f2d6346d2 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Mon, 1 Jul 2024 00:21:09 +0000 Subject: [PATCH 05/18] forward declare edge attributes and remove graph utilities from layer --- CMakeLists.txt | 1 + include/spark_dsg/base_layer.h | 36 ++-------- include/spark_dsg/dynamic_scene_graph.h | 12 ++-- include/spark_dsg/dynamic_scene_graph_layer.h | 6 +- include/spark_dsg/edge_container.h | 34 +++------ include/spark_dsg/graph_utilities.h | 8 +-- include/spark_dsg/scene_graph_layer.h | 3 +- include/spark_dsg/scene_graph_types.h | 6 ++ include/spark_dsg/spark_dsg.h | 41 +++++++++++ include/spark_dsg/spark_dsg_fwd.h | 2 + python/bindings/spark_dsg_bindings.cpp | 4 +- src/base_layer.cpp | 71 +++++++++++++++++++ src/dynamic_scene_graph.cpp | 9 +-- src/dynamic_scene_graph_layer.cpp | 7 +- src/edge_container.cpp | 28 +++++++- src/scene_graph_layer.cpp | 4 +- .../graph_binary_serialization.cpp | 1 + .../graph_json_serialization.cpp | 2 + tests/serialization/utest_file_io.cpp | 2 +- .../utest_graph_serialization.cpp | 1 + tests/spark_dsg_tests/type_comparisons.h | 1 + tests/utest_adjacency_matrix.cpp | 1 + tests/utest_dynamic_scene_graph.cpp | 3 +- tests/utest_dynamic_scene_graph_layer.cpp | 3 +- tests/utest_edge_container.cpp | 1 + tests/utest_graph_utilities_layer.cpp | 1 + tests/utest_scene_graph_layer.cpp | 3 +- 27 files changed, 201 insertions(+), 90 deletions(-) create mode 100644 include/spark_dsg/spark_dsg.h create mode 100644 src/base_layer.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 6afe944..500a8c3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,6 +30,7 @@ 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 diff --git a/include/spark_dsg/base_layer.h b/include/spark_dsg/base_layer.h index 5f07535..083423c 100644 --- a/include/spark_dsg/base_layer.h +++ b/include/spark_dsg/base_layer.h @@ -33,7 +33,6 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "spark_dsg/edge_attributes.h" #include "spark_dsg/edge_container.h" #include "spark_dsg/scene_graph_node.h" @@ -47,14 +46,7 @@ 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; }; class BaseLayer { @@ -67,7 +59,9 @@ 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&& info) = 0; virtual NodeStatus checkNode(NodeId node_id) const = 0; @@ -75,25 +69,9 @@ class BaseLayer { 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 diff --git a/include/spark_dsg/dynamic_scene_graph.h b/include/spark_dsg/dynamic_scene_graph.h index 3329abf..7ad81a1 100644 --- a/include/spark_dsg/dynamic_scene_graph.h +++ b/include/spark_dsg/dynamic_scene_graph.h @@ -181,7 +181,7 @@ class DynamicSceneGraph { */ bool insertEdge(NodeId source, NodeId target, - EdgeAttributes::Ptr&& edge_info = nullptr); + std::unique_ptr&& edge_info = nullptr); /** * @brief Insert a parent edge between two nodes @@ -199,7 +199,7 @@ class DynamicSceneGraph { */ bool insertParentEdge(NodeId source, NodeId target, - EdgeAttributes::Ptr&& edge_info = nullptr); + std::unique_ptr&& edge_info = nullptr); /** * @brief Add an edge to the graph or update an existing edge @@ -208,7 +208,9 @@ 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&& edge_info); /** * @brief Set the attributes of an existing node @@ -225,7 +227,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&& attrs); /** * @brief Check whether the layer exists and is valid diff --git a/include/spark_dsg/dynamic_scene_graph_layer.h b/include/spark_dsg/dynamic_scene_graph_layer.h index ab0f083..3b7f6fc 100644 --- a/include/spark_dsg/dynamic_scene_graph_layer.h +++ b/include/spark_dsg/dynamic_scene_graph_layer.h @@ -36,8 +36,6 @@ #include #include "spark_dsg/base_layer.h" -#include "spark_dsg/node_symbol.h" -#include "spark_dsg/spark_dsg_fwd.h" namespace spark_dsg { @@ -84,11 +82,11 @@ class DynamicSceneGraphLayer : public BaseLayer { bool insertEdge(NodeId source, NodeId target, - EdgeAttributes::Ptr&& edge_info = nullptr) override; + std::unique_ptr&& edge_info = nullptr) override; bool insertEdgeByIndex(size_t source_index, size_t target_index, - EdgeAttributes::Ptr&& edge_info = nullptr); + std::unique_ptr&& edge_info = nullptr); bool removeEdge(NodeId source, NodeId target) override; diff --git a/include/spark_dsg/edge_container.h b/include/spark_dsg/edge_container.h index 9519521..5d1a403 100644 --- a/include/spark_dsg/edge_container.h +++ b/include/spark_dsg/edge_container.h @@ -36,9 +36,8 @@ #include #include -#include "spark_dsg/edge_attributes.h" -#include "spark_dsg/node_symbol.h" #include "spark_dsg/scene_graph_types.h" +#include "spark_dsg/spark_dsg_fwd.h" namespace spark_dsg { @@ -53,11 +52,8 @@ enum class EdgeStatus { NEW, VISIBLE, DELETED, MERGED, NONEXISTENT }; * @brief Edge representation */ struct SceneGraphEdge { - //! attributes of the edge - using AttrPtr = std::unique_ptr; - //! construct and edge from some info - SceneGraphEdge(NodeId source, NodeId target, AttrPtr&& info); + SceneGraphEdge(NodeId source, NodeId target, std::unique_ptr&& info); ~SceneGraphEdge(); @@ -66,7 +62,7 @@ struct SceneGraphEdge { //! end of edge (by convention the child) const NodeId target; //! attributes about the edge - AttrPtr info; + std::unique_ptr info; /** * @brief get a reference to the attributes of the node (with an optional @@ -81,34 +77,24 @@ struct SceneGraphEdge { }; struct EdgeKey { - EdgeKey(NodeId k1, NodeId k2) : k1(std::min(k1, k2)), k2(std::max(k1, k2)) {} - - inline bool operator==(const EdgeKey& other) const { - return k1 == other.k1 && k2 == other.k2; - } - - inline bool operator<(const EdgeKey& other) const { - if (k1 == other.k1) { - return k2 < other.k2; - } - - return k1 < other.k1; - } + EdgeKey(NodeId k1, NodeId k2); + bool operator==(const EdgeKey& other) const; + bool operator<(const EdgeKey& other) const; NodeId k1; NodeId k2; }; -inline std::ostream& operator<<(std::ostream& out, const EdgeKey& key) { - return out << NodeSymbol(key.k1) << " -> " << NodeSymbol(key.k2); -} +std::ostream& operator<<(std::ostream& out, const EdgeKey& key); struct EdgeContainer { using Edge = SceneGraphEdge; using Edges = std::map; using EdgeStatusMap = std::map; - void insert(NodeId source, NodeId target, EdgeAttributes::Ptr&& edge_info); + void insert(NodeId source, + NodeId target, + std::unique_ptr&& edge_info); void remove(NodeId source, NodeId target); diff --git a/include/spark_dsg/graph_utilities.h b/include/spark_dsg/graph_utilities.h index 1c12475..2339aea 100644 --- a/include/spark_dsg/graph_utilities.h +++ b/include/spark_dsg/graph_utilities.h @@ -35,19 +35,15 @@ #pragma once #include #include -#include +#include #include +#include -#include "spark_dsg/node_symbol.h" #include "spark_dsg/scene_graph_types.h" namespace spark_dsg { namespace graph_utilities { -// TODO(nathan) make inheritance work -template -struct graph_traits {}; - template void breadthFirstSearch(const Graph& graph, std::deque& frontier, diff --git a/include/spark_dsg/scene_graph_layer.h b/include/spark_dsg/scene_graph_layer.h index 79677e0..fc2e9c3 100644 --- a/include/spark_dsg/scene_graph_layer.h +++ b/include/spark_dsg/scene_graph_layer.h @@ -39,7 +39,6 @@ #include #include "spark_dsg/base_layer.h" -#include "spark_dsg/graph_utilities.h" namespace spark_dsg { @@ -89,7 +88,7 @@ class SceneGraphLayer : public BaseLayer { */ bool insertEdge(NodeId source, NodeId target, - EdgeAttributes::Ptr&& edge_attributes = nullptr) override; + std::unique_ptr&& edge_attributes = nullptr) override; /** * @brief Check whether the layer has the specified node diff --git a/include/spark_dsg/scene_graph_types.h b/include/spark_dsg/scene_graph_types.h index c666b83..051ba5f 100644 --- a/include/spark_dsg/scene_graph_types.h +++ b/include/spark_dsg/scene_graph_types.h @@ -131,4 +131,10 @@ struct DsgLayers { static LayerId StringToLayerId(const std::string& id_str); }; +namespace graph_utilities { +// TODO(nathan) make inheritance work +template +struct graph_traits {}; +} // namespace graph_utilities + } // namespace spark_dsg diff --git a/include/spark_dsg/spark_dsg.h b/include/spark_dsg/spark_dsg.h new file mode 100644 index 0000000..09f1869 --- /dev/null +++ b/include/spark_dsg/spark_dsg.h @@ -0,0 +1,41 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once + +#include "spark_dsg/dynamic_scene_graph.h" +#include "spark_dsg/edge_attributes.h" +#include "spark_dsg/node_attributes.h" +#include "spark_dsg/node_symbol.h" +#include "spark_dsg/serialization/file_io.h" diff --git a/include/spark_dsg/spark_dsg_fwd.h b/include/spark_dsg/spark_dsg_fwd.h index 32904fa..b289862 100644 --- a/include/spark_dsg/spark_dsg_fwd.h +++ b/include/spark_dsg/spark_dsg_fwd.h @@ -4,7 +4,9 @@ namespace spark_dsg { class Mesh; struct NodeAttributes; +struct EdgeAttributes; class DynamicSceneGraph; using DynamicSceneGraphPtr = std::shared_ptr; + } // namespace spark_dsg diff --git a/python/bindings/spark_dsg_bindings.cpp b/python/bindings/spark_dsg_bindings.cpp index 629cf9e..c6646a8 100644 --- a/python/bindings/spark_dsg_bindings.cpp +++ b/python/bindings/spark_dsg_bindings.cpp @@ -38,11 +38,9 @@ #include #include #include -#include -#include #include -#include #include +#include #include #include diff --git a/src/base_layer.cpp b/src/base_layer.cpp new file mode 100644 index 0000000..39b9b7f --- /dev/null +++ b/src/base_layer.cpp @@ -0,0 +1,71 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "spark_dsg/base_layer.h" +#include "spark_dsg/node_symbol.h" + +#include + +namespace spark_dsg { + +NodeId GraphMergeConfig::getMergedId(NodeId original) const { + if (!previous_merges) { + return original; + } + + auto iter = previous_merges->find(original); + return iter == previous_merges->end() ? original : iter->second; +} + +const SceneGraphNode& BaseLayer::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; +} + +const SceneGraphEdge& BaseLayer::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; +} + +} // namespace spark_dsg diff --git a/src/dynamic_scene_graph.cpp b/src/dynamic_scene_graph.cpp index d2248fb..de4b0b2 100644 --- a/src/dynamic_scene_graph.cpp +++ b/src/dynamic_scene_graph.cpp @@ -39,6 +39,7 @@ #include "spark_dsg/edge_attributes.h" #include "spark_dsg/logging.h" #include "spark_dsg/node_attributes.h" +#include "spark_dsg/node_symbol.h" #include "spark_dsg/serialization/file_io.h" namespace spark_dsg { @@ -227,7 +228,7 @@ bool DynamicSceneGraph::addOrUpdateNode(LayerId layer_id, bool DynamicSceneGraph::insertEdge(NodeId source, NodeId target, - EdgeAttributes::Ptr&& edge_info) { + std::unique_ptr&& edge_info) { LayerKey source_key, target_key; if (hasEdge(source, target, &source_key, &target_key)) { return false; @@ -258,7 +259,7 @@ bool DynamicSceneGraph::insertEdge(NodeId source, bool DynamicSceneGraph::insertParentEdge(NodeId source, NodeId target, - EdgeAttributes::Ptr&& edge_info) { + std::unique_ptr&& edge_info) { LayerKey source_key, target_key; if (hasEdge(source, target, &source_key, &target_key)) { return false; @@ -289,7 +290,7 @@ bool DynamicSceneGraph::insertParentEdge(NodeId source, bool DynamicSceneGraph::addOrUpdateEdge(NodeId source, NodeId target, - EdgeAttributes::Ptr&& edge_info) { + std::unique_ptr&& edge_info) { if (hasEdge(source, target)) { return setEdgeAttributes(source, target, std::move(edge_info)); } else { @@ -310,7 +311,7 @@ bool DynamicSceneGraph::setNodeAttributes(NodeId node, bool DynamicSceneGraph::setEdgeAttributes(NodeId source, NodeId target, - EdgeAttributes::Ptr&& attrs) { + std::unique_ptr&& attrs) { // defer to layers if it is a intralayer edge const auto& source_key = node_lookup_.at(source); const auto& target_key = node_lookup_.at(target); diff --git a/src/dynamic_scene_graph_layer.cpp b/src/dynamic_scene_graph_layer.cpp index c2108ae..61cc25f 100644 --- a/src/dynamic_scene_graph_layer.cpp +++ b/src/dynamic_scene_graph_layer.cpp @@ -37,6 +37,7 @@ #include "spark_dsg/edge_attributes.h" #include "spark_dsg/logging.h" #include "spark_dsg/node_attributes.h" +#include "spark_dsg/node_symbol.h" namespace spark_dsg { @@ -195,7 +196,7 @@ const Edge& DynamicSceneGraphLayer::getEdgeByIndex(size_t source_idx, bool DynamicSceneGraphLayer::insertEdge(NodeId source, NodeId target, - EdgeAttributes::Ptr&& edge_info) { + std::unique_ptr&& edge_info) { if (source == target) { SG_LOG(WARNING) << "Attempted to add a self-edge for " << NodeSymbol(source).getLabel() << std::endl; @@ -219,8 +220,8 @@ bool DynamicSceneGraphLayer::insertEdge(NodeId source, bool DynamicSceneGraphLayer::insertEdgeByIndex(size_t source, size_t target, - EdgeAttributes::Ptr&& edge_info) { - return insertEdge(prefix.makeId(source), prefix.makeId(target), std::move(edge_info)); + std::unique_ptr&& info) { + return insertEdge(prefix.makeId(source), prefix.makeId(target), std::move(info)); } bool DynamicSceneGraphLayer::removeEdge(NodeId source, NodeId target) { diff --git a/src/edge_container.cpp b/src/edge_container.cpp index dcb2c39..ff0b0f7 100644 --- a/src/edge_container.cpp +++ b/src/edge_container.cpp @@ -34,18 +34,41 @@ * -------------------------------------------------------------------------- */ #include "spark_dsg/edge_container.h" +#include "spark_dsg/edge_attributes.h" +#include "spark_dsg/node_symbol.h" + namespace spark_dsg { using Edge = EdgeContainer::Edge; -SceneGraphEdge::SceneGraphEdge(NodeId source, NodeId target, AttrPtr&& info) +SceneGraphEdge::SceneGraphEdge(NodeId source, + NodeId target, + std::unique_ptr&& info) : source(source), target(target), info(std::move(info)) {} SceneGraphEdge::~SceneGraphEdge() = default; +EdgeKey::EdgeKey(NodeId k1, NodeId k2) : k1(std::min(k1, k2)), k2(std::max(k1, k2)) {} + +bool EdgeKey::operator==(const EdgeKey& other) const { + return k1 == other.k1 && k2 == other.k2; +} + +bool EdgeKey::operator<(const EdgeKey& other) const { + if (k1 == other.k1) { + return k2 < other.k2; + } + + return k1 < other.k1; +} + +std::ostream& operator<<(std::ostream& out, const EdgeKey& key) { + return out << NodeSymbol(key.k1) << " -> " << NodeSymbol(key.k2); +} + void EdgeContainer::insert(NodeId source, NodeId target, - EdgeAttributes::Ptr&& edge_info) { + std::unique_ptr&& edge_info) { auto attrs = (edge_info == nullptr) ? std::make_unique() : std::move(edge_info); @@ -152,5 +175,4 @@ Edge* EdgeContainer::find(const EdgeKey& key) const { return const_cast(&iter->second); } - } // namespace spark_dsg diff --git a/src/scene_graph_layer.cpp b/src/scene_graph_layer.cpp index 82919da..13ef665 100644 --- a/src/scene_graph_layer.cpp +++ b/src/scene_graph_layer.cpp @@ -38,8 +38,10 @@ #include #include "spark_dsg/edge_attributes.h" +#include "spark_dsg/graph_utilities.h" #include "spark_dsg/logging.h" #include "spark_dsg/node_attributes.h" +#include "spark_dsg/node_symbol.h" namespace spark_dsg { @@ -80,7 +82,7 @@ bool SceneGraphLayer::insertNode(SceneGraphNode::Ptr&& node) { bool SceneGraphLayer::insertEdge(NodeId source, NodeId target, - EdgeAttributes::Ptr&& edge_info) { + std::unique_ptr&& edge_info) { if (source == target) { SG_LOG(WARNING) << "Attempted to add a self-edge" << std::endl; return false; diff --git a/src/serialization/graph_binary_serialization.cpp b/src/serialization/graph_binary_serialization.cpp index eaba40a..86d7ff7 100644 --- a/src/serialization/graph_binary_serialization.cpp +++ b/src/serialization/graph_binary_serialization.cpp @@ -35,6 +35,7 @@ #include "spark_dsg/serialization/graph_binary_serialization.h" #include "spark_dsg/dynamic_scene_graph.h" +#include "spark_dsg/edge_attributes.h" #include "spark_dsg/node_attributes.h" #include "spark_dsg/serialization/attribute_registry.h" #include "spark_dsg/serialization/attribute_serialization.h" diff --git a/src/serialization/graph_json_serialization.cpp b/src/serialization/graph_json_serialization.cpp index 00460cf..477a165 100644 --- a/src/serialization/graph_json_serialization.cpp +++ b/src/serialization/graph_json_serialization.cpp @@ -37,8 +37,10 @@ #include #include "spark_dsg/dynamic_scene_graph.h" +#include "spark_dsg/edge_attributes.h" #include "spark_dsg/logging.h" #include "spark_dsg/node_attributes.h" +#include "spark_dsg/node_symbol.h" #include "spark_dsg/serialization/attribute_registry.h" #include "spark_dsg/serialization/attribute_serialization.h" #include "spark_dsg/serialization/json_conversions.h" diff --git a/tests/serialization/utest_file_io.cpp b/tests/serialization/utest_file_io.cpp index 2599def..323e648 100644 --- a/tests/serialization/utest_file_io.cpp +++ b/tests/serialization/utest_file_io.cpp @@ -34,8 +34,8 @@ * -------------------------------------------------------------------------- */ #include -#include "spark_dsg/serialization/file_io.h" #include "spark_dsg/serialization/versioning.h" +#include "spark_dsg/spark_dsg.h" #include "spark_dsg_tests/temp_file.h" #include "spark_dsg_tests/type_comparisons.h" diff --git a/tests/serialization/utest_graph_serialization.cpp b/tests/serialization/utest_graph_serialization.cpp index 543800b..ed03b6c 100644 --- a/tests/serialization/utest_graph_serialization.cpp +++ b/tests/serialization/utest_graph_serialization.cpp @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #include +#include "spark_dsg/node_symbol.h" #include "spark_dsg/serialization/graph_binary_serialization.h" #include "spark_dsg/serialization/graph_json_serialization.h" #include "spark_dsg_tests/type_comparisons.h" diff --git a/tests/spark_dsg_tests/type_comparisons.h b/tests/spark_dsg_tests/type_comparisons.h index 4c77eff..cc0bfe7 100644 --- a/tests/spark_dsg_tests/type_comparisons.h +++ b/tests/spark_dsg_tests/type_comparisons.h @@ -35,6 +35,7 @@ #pragma once #include #include +#include namespace spark_dsg { namespace test { diff --git a/tests/utest_adjacency_matrix.cpp b/tests/utest_adjacency_matrix.cpp index 52ca770..2b74bca 100644 --- a/tests/utest_adjacency_matrix.cpp +++ b/tests/utest_adjacency_matrix.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include "spark_dsg/logging.h" diff --git a/tests/utest_dynamic_scene_graph.cpp b/tests/utest_dynamic_scene_graph.cpp index 41b64a2..cddbef2 100644 --- a/tests/utest_dynamic_scene_graph.cpp +++ b/tests/utest_dynamic_scene_graph.cpp @@ -33,8 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include -#include -#include +#include namespace spark_dsg { diff --git a/tests/utest_dynamic_scene_graph_layer.cpp b/tests/utest_dynamic_scene_graph_layer.cpp index 5e066ca..3556dd4 100644 --- a/tests/utest_dynamic_scene_graph_layer.cpp +++ b/tests/utest_dynamic_scene_graph_layer.cpp @@ -33,8 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include -#include -#include +#include namespace spark_dsg { diff --git a/tests/utest_edge_container.cpp b/tests/utest_edge_container.cpp index 0f905a5..128cf11 100644 --- a/tests/utest_edge_container.cpp +++ b/tests/utest_edge_container.cpp @@ -34,6 +34,7 @@ * -------------------------------------------------------------------------- */ #include #include +#include namespace spark_dsg { diff --git a/tests/utest_graph_utilities_layer.cpp b/tests/utest_graph_utilities_layer.cpp index 87acfcd..224fdee 100644 --- a/tests/utest_graph_utilities_layer.cpp +++ b/tests/utest_graph_utilities_layer.cpp @@ -33,6 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include +#include #include #include #include diff --git a/tests/utest_scene_graph_layer.cpp b/tests/utest_scene_graph_layer.cpp index 133f0a2..4856022 100644 --- a/tests/utest_scene_graph_layer.cpp +++ b/tests/utest_scene_graph_layer.cpp @@ -33,8 +33,7 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include -#include -#include +#include namespace spark_dsg { From ede8a31c6a13c5c3b43a628118e39aca1870da63 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Mon, 1 Jul 2024 01:24:14 +0000 Subject: [PATCH 06/18] split bindings into separate files --- python/CMakeLists.txt | 17 +- .../include/spark_dsg/python/bounding_box.h | 43 + .../spark_dsg/python/dynamic_scene_graph.h | 43 + .../spark_dsg/python/edge_attributes.h | 43 + .../bindings/include/spark_dsg/python/mesh.h | 43 + .../include/spark_dsg/python/misc_types.h | 43 + .../spark_dsg/python/node_attributes.h | 43 + .../spark_dsg/python/scene_graph_edge.h | 43 + .../spark_dsg/python}/scene_graph_iterators.h | 0 .../spark_dsg/python/scene_graph_layer.h | 43 + .../spark_dsg/python/scene_graph_node.h | 43 + .../spark_dsg/python/scene_graph_utilities.h | 43 + .../spark_dsg/python/zmq_interface.h.in} | 9 +- python/bindings/spark_dsg_bindings.cpp | 830 ------------------ python/bindings/src/bounding_box.cpp | 88 ++ python/bindings/src/dynamic_scene_graph.cpp | 304 +++++++ python/bindings/src/edge_attributes.cpp | 53 ++ python/bindings/src/mesh.cpp | 175 ++++ python/bindings/src/misc_types.cpp | 85 ++ python/bindings/src/node_attributes.cpp | 253 ++++++ python/bindings/src/scene_graph_edge.cpp | 99 +++ python/bindings/src/scene_graph_layer.cpp | 104 +++ python/bindings/src/scene_graph_node.cpp | 76 ++ python/bindings/src/scene_graph_utilities.cpp | 53 ++ python/bindings/src/spark_dsg_bindings.cpp | 67 ++ .../zmq_interface.cpp} | 15 +- 26 files changed, 1815 insertions(+), 843 deletions(-) create mode 100644 python/bindings/include/spark_dsg/python/bounding_box.h create mode 100644 python/bindings/include/spark_dsg/python/dynamic_scene_graph.h create mode 100644 python/bindings/include/spark_dsg/python/edge_attributes.h create mode 100644 python/bindings/include/spark_dsg/python/mesh.h create mode 100644 python/bindings/include/spark_dsg/python/misc_types.h create mode 100644 python/bindings/include/spark_dsg/python/node_attributes.h create mode 100644 python/bindings/include/spark_dsg/python/scene_graph_edge.h rename python/bindings/{ => include/spark_dsg/python}/scene_graph_iterators.h (100%) create mode 100644 python/bindings/include/spark_dsg/python/scene_graph_layer.h create mode 100644 python/bindings/include/spark_dsg/python/scene_graph_node.h create mode 100644 python/bindings/include/spark_dsg/python/scene_graph_utilities.h rename python/bindings/{zmq_bindings.h.in => include/spark_dsg/python/zmq_interface.h.in} (94%) delete mode 100644 python/bindings/spark_dsg_bindings.cpp create mode 100644 python/bindings/src/bounding_box.cpp create mode 100644 python/bindings/src/dynamic_scene_graph.cpp create mode 100644 python/bindings/src/edge_attributes.cpp create mode 100644 python/bindings/src/mesh.cpp create mode 100644 python/bindings/src/misc_types.cpp create mode 100644 python/bindings/src/node_attributes.cpp create mode 100644 python/bindings/src/scene_graph_edge.cpp create mode 100644 python/bindings/src/scene_graph_layer.cpp create mode 100644 python/bindings/src/scene_graph_node.cpp create mode 100644 python/bindings/src/scene_graph_utilities.cpp create mode 100644 python/bindings/src/spark_dsg_bindings.cpp rename python/bindings/{zmq_bindings.cpp => src/zmq_interface.cpp} (94%) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 50088e6..e99c641 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -15,12 +15,21 @@ set(ZMQ_INTERFACE_ENABLED 0) if(SPARK_DSG_BUILD_ZMQ AND zmq_FOUND) set(ZMQ_INTERFACE_ENABLED 1) endif() -configure_file(bindings/zmq_bindings.h.in bindings/zmq_bindings.h) +configure_file( + bindings/include/spark_dsg/python/zmq_interface.h.in + bindings/include/spark_dsg/python/zmq_interface.h +) pybind11_add_module( - _dsg_bindings bindings/spark_dsg_bindings.cpp bindings/zmq_bindings.cpp + _dsg_bindings + bindings/src/bounding_box.cpp + bindings/src/edge_attributes.cpp + bindings/src/mesh.cpp + bindings/src/node_attributes.cpp + bindings/src/spark_dsg_bindings.cpp + bindings/src/zmq_interface.cpp ) target_link_libraries(_dsg_bindings PRIVATE spark_dsg) target_include_directories( - _dsg_bindings PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/bindings - ${CMAKE_CURRENT_BINARY_DIR}/bindings + _dsg_bindings PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/bindings/include + ${CMAKE_CURRENT_BINARY_DIR}/bindings/include ) diff --git a/python/bindings/include/spark_dsg/python/bounding_box.h b/python/bindings/include/spark_dsg/python/bounding_box.h new file mode 100644 index 0000000..354a6df --- /dev/null +++ b/python/bindings/include/spark_dsg/python/bounding_box.h @@ -0,0 +1,43 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once + +namespace pybind11 { +class module_; +} + +namespace spark_dsg::python::bounding_box { +void addBindings(pybind11::module_& m); +} diff --git a/python/bindings/include/spark_dsg/python/dynamic_scene_graph.h b/python/bindings/include/spark_dsg/python/dynamic_scene_graph.h new file mode 100644 index 0000000..f2fae64 --- /dev/null +++ b/python/bindings/include/spark_dsg/python/dynamic_scene_graph.h @@ -0,0 +1,43 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once + +namespace pybind11 { +class module_; +} + +namespace spark_dsg::python::dynamic_scene_graph { +void addBindings(pybind11::module_& m); +} diff --git a/python/bindings/include/spark_dsg/python/edge_attributes.h b/python/bindings/include/spark_dsg/python/edge_attributes.h new file mode 100644 index 0000000..0e4e83a --- /dev/null +++ b/python/bindings/include/spark_dsg/python/edge_attributes.h @@ -0,0 +1,43 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once + +namespace pybind11 { +class module_; +} + +namespace spark_dsg::python::edge_attributes { +void addBindings(pybind11::module_& m); +} diff --git a/python/bindings/include/spark_dsg/python/mesh.h b/python/bindings/include/spark_dsg/python/mesh.h new file mode 100644 index 0000000..b49d26d --- /dev/null +++ b/python/bindings/include/spark_dsg/python/mesh.h @@ -0,0 +1,43 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once + +namespace pybind11 { +class module_; +} + +namespace spark_dsg::python::mesh { +void addBindings(pybind11::module_& m); +} diff --git a/python/bindings/include/spark_dsg/python/misc_types.h b/python/bindings/include/spark_dsg/python/misc_types.h new file mode 100644 index 0000000..c6ccfc7 --- /dev/null +++ b/python/bindings/include/spark_dsg/python/misc_types.h @@ -0,0 +1,43 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once + +namespace pybind11 { +class module_; +} + +namespace spark_dsg::python::misc_types { +void addBindings(pybind11::module_& m); +} diff --git a/python/bindings/include/spark_dsg/python/node_attributes.h b/python/bindings/include/spark_dsg/python/node_attributes.h new file mode 100644 index 0000000..c2edca3 --- /dev/null +++ b/python/bindings/include/spark_dsg/python/node_attributes.h @@ -0,0 +1,43 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once + +namespace pybind11 { +class module_; +} + +namespace spark_dsg::python::node_attributes { +void addBindings(pybind11::module_& m); +} diff --git a/python/bindings/include/spark_dsg/python/scene_graph_edge.h b/python/bindings/include/spark_dsg/python/scene_graph_edge.h new file mode 100644 index 0000000..ff2a440 --- /dev/null +++ b/python/bindings/include/spark_dsg/python/scene_graph_edge.h @@ -0,0 +1,43 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once + +namespace pybind11 { +class module_; +} + +namespace spark_dsg::python::scene_graph_edge { +void addBindings(pybind11::module_& m); +} diff --git a/python/bindings/scene_graph_iterators.h b/python/bindings/include/spark_dsg/python/scene_graph_iterators.h similarity index 100% rename from python/bindings/scene_graph_iterators.h rename to python/bindings/include/spark_dsg/python/scene_graph_iterators.h diff --git a/python/bindings/include/spark_dsg/python/scene_graph_layer.h b/python/bindings/include/spark_dsg/python/scene_graph_layer.h new file mode 100644 index 0000000..fe8f038 --- /dev/null +++ b/python/bindings/include/spark_dsg/python/scene_graph_layer.h @@ -0,0 +1,43 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once + +namespace pybind11 { +class module_; +} + +namespace spark_dsg::python::scene_graph_layer { +void addBindings(pybind11::module_& m); +} diff --git a/python/bindings/include/spark_dsg/python/scene_graph_node.h b/python/bindings/include/spark_dsg/python/scene_graph_node.h new file mode 100644 index 0000000..f67ea55 --- /dev/null +++ b/python/bindings/include/spark_dsg/python/scene_graph_node.h @@ -0,0 +1,43 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once + +namespace pybind11 { +class module_; +} + +namespace spark_dsg::python::scene_graph_node { +void addBindings(pybind11::module_& m); +} diff --git a/python/bindings/include/spark_dsg/python/scene_graph_utilities.h b/python/bindings/include/spark_dsg/python/scene_graph_utilities.h new file mode 100644 index 0000000..876183b --- /dev/null +++ b/python/bindings/include/spark_dsg/python/scene_graph_utilities.h @@ -0,0 +1,43 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once + +namespace pybind11 { +class module_; +} + +namespace spark_dsg::python::scene_graph_utilities { +void addBindings(pybind11::module_& m); +} diff --git a/python/bindings/zmq_bindings.h.in b/python/bindings/include/spark_dsg/python/zmq_interface.h.in similarity index 94% rename from python/bindings/zmq_bindings.h.in rename to python/bindings/include/spark_dsg/python/zmq_interface.h.in index 8fc5dd6..a30974a 100644 --- a/python/bindings/zmq_bindings.h.in +++ b/python/bindings/include/spark_dsg/python/zmq_interface.h.in @@ -33,8 +33,13 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include #define INCLUDE_ZMQ() @ZMQ_INTERFACE_ENABLED@ -void add_zmq_bindings(pybind11::module_& module); +namespace pybind11 { +class module_; +} + +namespace spark_dsg::python::zmq_interface { +void addBindings(pybind11::module_& m); +} diff --git a/python/bindings/spark_dsg_bindings.cpp b/python/bindings/spark_dsg_bindings.cpp deleted file mode 100644 index c6646a8..0000000 --- a/python/bindings/spark_dsg_bindings.cpp +++ /dev/null @@ -1,830 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "scene_graph_iterators.h" -#include "zmq_bindings.h" - -namespace py = pybind11; -using namespace py::literals; - -using namespace spark_dsg; - -Eigen::MatrixXd getEigenVertices(const Mesh& mesh) { - const auto num_vertices = mesh.numVertices(); - Eigen::MatrixXd to_return(6, num_vertices); - for (size_t i = 0; i < num_vertices; ++i) { - const auto& pos = mesh.pos(i); - to_return(0, i) = pos.x(); - to_return(1, i) = pos.y(); - to_return(2, i) = pos.z(); - if (i < mesh.colors.size()) { - const auto c = mesh.color(i); - to_return(3, i) = c.r / 255.0; - to_return(4, i) = c.g / 255.0; - to_return(5, i) = c.b / 255.0; - } else { - to_return(3, i) = 0.0; - to_return(4, i) = 0.0; - to_return(5, i) = 0.0; - } - } - return to_return; -} - -void setEigenVertices(Mesh& mesh, const Eigen::MatrixXd& points) { - if (points.rows() != 6) { - std::stringstream ss; - ss << "point rows do not match expected: " << points.rows() << " != 6"; - throw std::invalid_argument(ss.str()); - } - - mesh.resizeVertices(points.cols()); - for (int i = 0; i < points.cols(); ++i) { - Eigen::Vector3f pos = points.col(i).head<3>().cast(); - mesh.setPos(i, pos); - if (mesh.has_colors) { - Color color{static_cast(points(3, i) * 255), - static_cast(points(4, i) * 255), - static_cast(points(5, i) * 255), - 255}; - mesh.setColor(i, color); - } - } -} - -Eigen::MatrixXi getEigenFaces(const Mesh& mesh) { - const auto num_faces = mesh.numFaces(); - Eigen::MatrixXi to_return(3, num_faces); - for (size_t i = 0; i < num_faces; ++i) { - const auto& face = mesh.face(i); - to_return(0, i) = face[0]; - to_return(1, i) = face[1]; - to_return(2, i) = face[2]; - } - return to_return; -} - -void setEigenFaces(Mesh& mesh, const Eigen::MatrixXi& indices) { - if (indices.rows() != 3) { - std::stringstream ss; - ss << "index rows do not match expected: " << indices.rows() << " != 3"; - throw std::invalid_argument(ss.str()); - } - - mesh.resizeFaces(indices.cols()); - for (int i = 0; i < indices.cols(); ++i) { - Mesh::Face face{{static_cast(indices(0, i)), - static_cast(indices(1, i)), - static_cast(indices(2, i))}}; - mesh.face(i) = face; - } -} - -template -struct Quaternion { - Quaternion() : w(1.0f), x(0.0f), y(0.0f), z(0.0f) {} - - Quaternion(Scalar w, Scalar x, Scalar y, Scalar z) : w(w), x(x), y(y), z(z) {} - - explicit Quaternion(const Eigen::Quaternion other) { - w = other.w(); - x = other.x(); - y = other.y(); - z = other.z(); - } - - operator Eigen::Quaternion() const { - return Eigen::Quaternion(w, x, y, z); - } - - Scalar w; - Scalar x; - Scalar y; - Scalar z; -}; - -template -std::ostream& operator<<(std::ostream& out, const Quaternion& q) { - out << "Quaternion"; - return out; -} - -PYBIND11_MODULE(_dsg_bindings, module) { - py::options options; - // options.disable_function_signatures(); - - add_zmq_bindings(module); - - py::enum_(module, "BoundingBoxType") - .value("INVALID", BoundingBox::Type::INVALID) - .value("AABB", BoundingBox::Type::AABB) - .value("OBB", BoundingBox::Type::OBB) - .value("RAABB", BoundingBox::Type::RAABB); - - py::class_(module, "DsgLayers") - .def_readonly_static("SEGMENTS", &DsgLayers::SEGMENTS) - .def_readonly_static("OBJECTS", &DsgLayers::OBJECTS) - .def_readonly_static("AGENTS", &DsgLayers::AGENTS) - .def_readonly_static("PLACES", &DsgLayers::PLACES) - .def_readonly_static("MESH_PLACES", &DsgLayers::MESH_PLACES) - .def_readonly_static("STRUCTURE", &DsgLayers::STRUCTURE) - .def_readonly_static("ROOMS", &DsgLayers::ROOMS) - .def_readonly_static("BUILDINGS", &DsgLayers::BUILDINGS); - - py::class_>(module, "Quaternionf") - .def(py::init<>()) - .def(py::init()) - .def_readwrite("w", &Quaternion::w) - .def_readwrite("x", &Quaternion::x) - .def_readwrite("y", &Quaternion::y) - .def_readwrite("z", &Quaternion::z) - .def("__repr__", [](const Quaternion& q) { - std::stringstream ss; - ss << q; - return ss.str(); - }); - - py::class_>(module, "Quaterniond") - .def(py::init<>()) - .def(py::init()) - .def_readwrite("w", &Quaternion::w) - .def_readwrite("x", &Quaternion::x) - .def_readwrite("y", &Quaternion::y) - .def_readwrite("z", &Quaternion::z) - .def("__repr__", [](const Quaternion& q) { - std::stringstream ss; - ss << q; - return ss.str(); - }); - - py::class_(module, "BoundingBox") - .def(py::init<>()) - .def(py::init()) - .def(py::init()) - .def(py::init()) - .def(py::init([](BoundingBox::Type type, - const Eigen::Vector3f& dim, - const Eigen::Vector3f& pos, - const Eigen::Matrix3f& trans) { - return BoundingBox(type, dim, pos, trans); - })) - .def_readwrite("type", &BoundingBox::type) - .def_readwrite("dimensions", &BoundingBox::dimensions) - .def_readwrite("world_P_center", &BoundingBox::world_P_center) - .def_readwrite("world_R_center", &BoundingBox::world_R_center) - .def("is_valid", &BoundingBox::isValid) - .def("volume", &BoundingBox::volume) - .def("has_rotation", &BoundingBox::hasRotation) - .def("corners", &BoundingBox::corners) - .def("contains", - static_cast( - &BoundingBox::contains)) - .def("intersects", - static_cast( - &BoundingBox::intersects)) - .def("compute_iou", - static_cast( - &BoundingBox::computeIoU)) - .def("__repr__", [](const BoundingBox& box) { - std::stringstream ss; - ss << box; - return ss.str(); - }); - - py::class_(module, "NodeAttributes") - .def(py::init<>()) - .def_readwrite("position", &NodeAttributes::position) - .def_readwrite("last_update_time_ns", &NodeAttributes::last_update_time_ns) - .def_readwrite("is_predicted", &NodeAttributes::is_predicted) - .def("__repr__", [](const NodeAttributes& attrs) { - std::stringstream ss; - ss << attrs; - return ss.str(); - }); - - py::class_(module, "SemanticNodeAttributes") - .def(py::init<>()) - .def_readwrite("name", &SemanticNodeAttributes::name) - .def_property( - "color", - [](const SemanticNodeAttributes& attrs) { - Eigen::Matrix color; - color << attrs.color.r, attrs.color.g, attrs.color.b; - return color; - }, - [](SemanticNodeAttributes& attrs, const Eigen::Matrix color) { - attrs.color.r = color(0); - attrs.color.g = color(1); - attrs.color.b = color(2); - }) - .def_readwrite("bounding_box", &SemanticNodeAttributes::bounding_box) - .def_readwrite("semantic_label", &SemanticNodeAttributes::semantic_label) - .def_readwrite("semantic_feature", &SemanticNodeAttributes::semantic_feature) - .def_readonly_static("NO_SEMANTIC_LABEL", - &SemanticNodeAttributes::NO_SEMANTIC_LABEL); - - py::class_(module, - "ObjectNodeAttributes") - .def(py::init<>()) - .def_readwrite("registered", &ObjectNodeAttributes::registered) - .def_property( - "world_R_object", - [](const ObjectNodeAttributes& attrs) { - return Quaternion(attrs.world_R_object); - }, - [](ObjectNodeAttributes& attrs, const Quaternion& rot) { - attrs.world_R_object = rot; - }); - - py::class_(module, "RoomNodeAttributes") - .def(py::init<>()) - .def_readwrite("semantic_class_probabilities", - &RoomNodeAttributes::semantic_class_probabilities); - - py::class_(module, "NearestVertexInfo") - .def(py::init<>()) - .def_property( - "block", - [](const NearestVertexInfo& info) -> std::array { - return {info.block[0], info.block[1], info.block[2]}; - }, - [](NearestVertexInfo& info, const std::array& array) { - info.block[0] = array[0]; - info.block[1] = array[1]; - info.block[2] = array[2]; - }) - .def_property( - "voxel_pos", - [](const NearestVertexInfo& info) -> std::array { - return {info.voxel_pos[0], info.voxel_pos[1], info.voxel_pos[2]}; - }, - [](NearestVertexInfo& info, const std::array& array) { - info.voxel_pos[0] = array[0]; - info.voxel_pos[1] = array[1]; - info.voxel_pos[2] = array[2]; - }) - .def_readwrite("vertex", &NearestVertexInfo::vertex) - .def_readwrite("label", &NearestVertexInfo::label) - .def("__repr__", [](const NearestVertexInfo& info) { - std::stringstream ss; - ss << std::setprecision(6) << "VertexInfo"; - return ss.str(); - }); - - py::class_(module, "PlaceNodeAttributes") - .def(py::init<>()) - .def_readwrite("distance", &PlaceNodeAttributes::distance) - .def_readwrite("num_basis_points", &PlaceNodeAttributes::num_basis_points) - .def_readwrite("voxblox_mesh_connections", - &PlaceNodeAttributes::voxblox_mesh_connections) - .def_readwrite("pcl_mesh_connections", &PlaceNodeAttributes::pcl_mesh_connections) - .def_readwrite("mesh_vertex_labels", &PlaceNodeAttributes::mesh_vertex_labels) - .def_readwrite("deformation_connections", - &PlaceNodeAttributes::deformation_connections) - .def_readwrite("is_active", &PlaceNodeAttributes::is_active) - .def_readwrite("real_place", &PlaceNodeAttributes::real_place) - .def_readwrite("active_frontier", &PlaceNodeAttributes::active_frontier) - .def_readwrite("frontier_scale", &PlaceNodeAttributes::frontier_scale) - .def_property( - "orientation", - [](const PlaceNodeAttributes& attrs) { - return Quaternion(attrs.orientation); - }, - [](PlaceNodeAttributes& attrs, const Quaternion& rot) { - attrs.orientation = rot; - }) - .def_readwrite("num_frontier_voxels", &PlaceNodeAttributes::num_frontier_voxels) - .def_readwrite("need_cleanup", &PlaceNodeAttributes::need_cleanup); - - py::class_(module, - "Place2dNodeAttributes") - .def(py::init<>()) - .def_readwrite("boundary", &Place2dNodeAttributes::boundary) - .def_readwrite("ellipse_matrix_compress", - &Place2dNodeAttributes::ellipse_matrix_compress) - .def_readwrite("ellipse_matrix_expand", - &Place2dNodeAttributes::ellipse_matrix_expand) - .def_readwrite("ellipse_centroid", &Place2dNodeAttributes::ellipse_centroid) - .def_readwrite("pcl_boundary_connections", - &Place2dNodeAttributes::pcl_boundary_connections) - .def_readwrite("voxblox_mesh_connections", - &Place2dNodeAttributes::voxblox_mesh_connections) - .def_readwrite("pcl_mesh_connections", - &Place2dNodeAttributes::pcl_mesh_connections) - .def_readwrite("mesh_vertex_labels", &Place2dNodeAttributes::mesh_vertex_labels) - .def_readwrite("deformation_connections", - &Place2dNodeAttributes::deformation_connections) - .def_readwrite("is_active", &Place2dNodeAttributes::is_active); - - py::class_(module, "AgentNodeAttributes") - .def(py::init<>()) - .def_property( - "world_R_body", - [](const AgentNodeAttributes& attrs) { - return Quaternion(attrs.world_R_body); - }, - [](AgentNodeAttributes& attrs, const Quaternion& rot) { - attrs.world_R_body = rot; - }) - .def_readwrite("external_key", &AgentNodeAttributes::external_key) - .def_readwrite("dbow_ids", &AgentNodeAttributes::dbow_ids) - .def_readwrite("dbow_values", &AgentNodeAttributes::dbow_values); - - py::class_(module, "NodeSymbol") - .def(py::init([](char key, size_t index) { return NodeSymbol(key, index); })) - .def(py::init([](size_t value) { return NodeSymbol(value); })) - .def_property("category_id", &NodeSymbol::categoryId, nullptr) - .def_property("category", &NodeSymbol::category, nullptr) - .def_property( - "value", - [](const NodeSymbol& symbol) { return static_cast(symbol); }, - nullptr) - .def("__repr__", &NodeSymbol::getLabel) - .def("__hash__", - [](const NodeSymbol& symbol) { return static_cast(symbol); }) - .def(pybind11::self == pybind11::self) - .def(pybind11::self != pybind11::self); - - py::class_(module, "LayerPrefix") - .def(py::init([](char key) { return LayerPrefix(key); })) - .def(py::init([](char key, uint32_t index) { return LayerPrefix(key, index); })) - .def(py::init([](uint32_t index) { return LayerPrefix(index); })) - .def_property( - "value", - [](const LayerPrefix& prefix) { return static_cast(prefix); }, - nullptr) - .def("__repr__", [](const LayerPrefix& prefix) { return prefix.str(true); }); - - py::class_(module, "SceneGraphNode") - .def("has_parent", &SceneGraphNode::hasParent) - .def("has_siblings", &SceneGraphNode::hasSiblings) - .def("has_children", &SceneGraphNode::hasChildren) - .def("get_parent", &SceneGraphNode::getParent) - .def("siblings", &SceneGraphNode::siblings) - .def("children", &SceneGraphNode::children) - .def_property_readonly("timestamp", - [](const SceneGraphNode& node) -> std::optional { - if (node.timestamp) { - return node.timestamp.value().count(); - } else { - return std::nullopt; - } - }) - .def_property("attributes", - &SceneGraphNode::getAttributesPtr, - &SceneGraphNode::getAttributesPtr, - py::return_value_policy::reference_internal) - .def_property_readonly( - "id", [](const SceneGraphNode& node) { return NodeSymbol(node.id); }) - .def_readonly("layer", &SceneGraphNode::layer) - .def("__repr__", [](const SceneGraphNode& node) { - std::stringstream ss; - ss << node; - return ss.str(); - }); - - py::class_(module, "EdgeAttributes") - .def(py::init<>()) - .def_readwrite("weighted", &EdgeAttributes::weighted) - .def_readwrite("weight", &EdgeAttributes::weight); - - py::class_(module, "SceneGraphEdge") - .def_readonly("source", &SceneGraphEdge::source) - .def_readonly("target", &SceneGraphEdge::target) - .def_property( - "info", - [](const SceneGraphEdge& edge) { return *(edge.info); }, - [](SceneGraphEdge& edge, const EdgeAttributes& info) { *edge.info = info; }) - .def("__repr__", [](const SceneGraphEdge& edge) { - std::stringstream ss; - ss << "Edge"; - return ss.str(); - }); - - // TODO(nathan) iterator over nodes and edges - py::class_>( - module, "SceneGraphLayer") - .def(py::init()) - .def( - "add_node", - [](IsolatedSceneGraphLayer& layer, NodeId node, const NodeAttributes& attrs) { - layer.emplaceNode(node, attrs.clone()); - }) - .def("insert_edge", - [](IsolatedSceneGraphLayer& layer, NodeId source, NodeId target) { - return layer.insertEdge(source, target); - }) - .def("insert_edge", - [](IsolatedSceneGraphLayer& layer, - NodeId source, - NodeId target, - const EdgeAttributes& info) { - return layer.insertEdge(source, target, info.clone()); - }) - .def("has_node", &IsolatedSceneGraphLayer::hasNode) - .def("has_edge", &IsolatedSceneGraphLayer::hasEdge) - .def("get_node", - &IsolatedSceneGraphLayer::getNode, - py::return_value_policy::reference_internal) - .def("find_node", - &IsolatedSceneGraphLayer::findNode, - py::return_value_policy::reference_internal) - .def("get_edge", - &IsolatedSceneGraphLayer::getEdge, - py::return_value_policy::reference_internal) - .def("find_edge", - &IsolatedSceneGraphLayer::findEdge, - py::return_value_policy::reference_internal) - .def("remove_edge", &IsolatedSceneGraphLayer::removeEdge) - .def("num_nodes", &IsolatedSceneGraphLayer::numNodes) - .def("num_edges", &IsolatedSceneGraphLayer::numEdges) - .def("get_position", &IsolatedSceneGraphLayer::getPosition) - .def_readonly("id", &IsolatedSceneGraphLayer::id) - .def_property( - "nodes", - [](const IsolatedSceneGraphLayer& view) { - return py::make_iterator(NodeIter(view.nodes()), IterSentinel()); - }, - nullptr, - py::return_value_policy::reference_internal) - .def_property( - "edges", - [](const IsolatedSceneGraphLayer& view) { - return py::make_iterator(EdgeIter(view.edges()), IterSentinel()); - }, - nullptr, - py::return_value_policy::reference_internal); - - py::class_(module, "LayerView") - .def("has_node", &LayerView::hasNode) - .def("has_edge", &LayerView::hasEdge) - .def("get_node", &LayerView::getNode, py::return_value_policy::reference_internal) - .def("get_edge", &LayerView::getEdge, py::return_value_policy::reference_internal) - .def("num_nodes", &LayerView::numNodes) - .def("num_edges", &LayerView::numEdges) - .def("get_position", &LayerView::getPosition) - .def_readonly("id", &LayerView::id) - .def_property( - "nodes", - [](const LayerView& view) { - return py::make_iterator(view.nodes(), IterSentinel()); - }, - nullptr, - py::return_value_policy::reference_internal) - .def_property( - "edges", - [](const LayerView& view) { - return py::make_iterator(view.edges(), IterSentinel()); - }, - nullptr, - py::return_value_policy::reference_internal); - - py::class_(module, "DynamicLayerView") - .def_readonly("id", &DynamicLayerView::id) - .def_readonly("prefix", &DynamicLayerView::prefix) - .def("num_nodes", &DynamicLayerView::numNodes) - .def("num_edges", &DynamicLayerView::numEdges) - .def_property( - "nodes", - [](const DynamicLayerView& view) { - return py::make_iterator(view.nodes(), IterSentinel()); - }, - nullptr, - py::return_value_policy::reference_internal) - .def_property( - "edges", - [](const DynamicLayerView& view) { - return py::make_iterator(view.edges(), IterSentinel()); - }, - nullptr, - py::return_value_policy::reference_internal); - - py::class_(module, "Color") - .def_readwrite("r", &Color::r) - .def_readwrite("g", &Color::g) - .def_readwrite("b", &Color::b) - .def_readwrite("a", &Color::a); - - py::class_>(module, "Mesh") - .def(py::init(), - "has_colors"_a = true, - "has_timestamps"_a = true, - "has_labels"_a = "true") - .def("empty", &Mesh::empty) - .def("num_vertices", &Mesh::numVertices) - .def("num_faces", &Mesh::numFaces) - .def("resize_vertices", &Mesh::resizeVertices) - .def("resize_faces", &Mesh::resizeFaces) - .def("clone", &Mesh::clone) - .def("pos", &Mesh::pos) - .def("set_pos", &Mesh::setPos) - .def("color", &Mesh::color) - .def("set_color", &Mesh::setColor) - .def("timestamp", &Mesh::timestamp) - .def("set_timestamp", &Mesh::setTimestamp) - .def("label", &Mesh::label) - .def("set_label", &Mesh::setLabel) - .def("face", py::overload_cast(&Mesh::face, py::const_)) - .def("set_face", - [](Mesh& mesh, size_t index, const Mesh::Face& face) { - mesh.face(index) = face; - }) - .def("to_json", &Mesh::serializeToJson) - .def("to_binary", - [](const Mesh& mesh) { - std::vector buffer; - mesh.serializeToBinary(buffer); - return py::bytes(reinterpret_cast(buffer.data()), buffer.size()); - }) - .def("save", &Mesh::save) - .def("save", - [](const Mesh& mesh, const std::filesystem::path& path) { mesh.save(path); }) - .def_static("from_json", &Mesh::deserializeFromJson) - .def_static("from_binary", - [](const py::bytes& contents) { - const auto& view = static_cast(contents); - return Mesh::deserializeFromBinary( - reinterpret_cast(view.data()), view.size()); - }) - .def_static("load", &Mesh::load) - .def_static("load", - [](const std::filesystem::path& path) { return Mesh::load(path); }) - .def("get_vertices", [](const Mesh& mesh) { return getEigenVertices(mesh); }) - .def("get_faces", [](const Mesh& mesh) { return getEigenFaces(mesh); }) - .def("get_labels", [](const Mesh& mesh) { return mesh.labels; }) - .def("set_vertices", - [](Mesh& mesh, const Eigen::MatrixXd& points) { - setEigenVertices(mesh, points); - }) - .def("set_faces", [](Mesh& mesh, const Eigen::MatrixXi& faces) { - setEigenFaces(mesh, faces); - }); - - py::class_>( - module, "DynamicSceneGraph", py::dynamic_attr()) - .def(py::init<>()) - .def(py::init()) - .def("clear", &DynamicSceneGraph::clear) - .def("create_dynamic_layer", &DynamicSceneGraph::createDynamicLayer) - .def("add_node", - [](DynamicSceneGraph& graph, - LayerId layer_id, - NodeId node_id, - const NodeAttributes& attrs) { - graph.emplaceNode(layer_id, node_id, attrs.clone()); - }) - .def( - "add_node", - [](DynamicSceneGraph& graph, - LayerId layer_id, - LayerPrefix prefix, - std::chrono::nanoseconds timestamp, - const NodeAttributes& attrs, - bool add_edge_to_previous) { - graph.emplaceNode( - layer_id, prefix, timestamp, attrs.clone(), add_edge_to_previous); - }, - "layer_id"_a, - "prefix"_a, - "timestamp"_a, - "attrs"_a, - "add_edge_to_previous"_a = true) - .def( - "insert_edge", - [](DynamicSceneGraph& graph, - NodeId source, - NodeId target, - bool enforce_single_parent) { - return enforce_single_parent ? graph.insertParentEdge(source, target) - : graph.insertEdge(source, target); - }, - "source"_a, - "target"_a, - "enforce_single_parent"_a = false) - .def( - "insert_edge", - [](DynamicSceneGraph& graph, - NodeId source, - NodeId target, - const EdgeAttributes& info, - bool enforce_single_parent) { - return enforce_single_parent - ? graph.insertParentEdge(source, target, info.clone()) - : graph.insertEdge(source, target, info.clone()); - }, - "source"_a, - "target"_a, - "info"_a, - "enforce_single_parent"_a = false) - .def("has_layer", - static_cast( - &DynamicSceneGraph::hasLayer)) - .def("has_layer", - static_cast( - &DynamicSceneGraph::hasLayer)) - .def("has_node", &DynamicSceneGraph::hasNode) - .def("has_edge", - py::overload_cast(&DynamicSceneGraph::hasEdge, py::const_)) - .def("has_mesh", &DynamicSceneGraph::hasMesh) - .def( - "get_layer", - [](const DynamicSceneGraph& graph, LayerId layer_id) { - if (!graph.hasLayer(layer_id)) { - throw std::out_of_range("layer doesn't exist"); - } - return LayerView(graph.getLayer(layer_id)); - }, - py::return_value_policy::reference_internal) - .def( - "get_dynamic_layer", - [](const DynamicSceneGraph& graph, LayerId layer_id, LayerPrefix prefix) { - if (!graph.hasLayer(layer_id, prefix)) { - throw std::out_of_range("layer doesn't exist"); - } - return DynamicLayerView(graph.getLayer(layer_id, prefix)); - }, - py::return_value_policy::reference_internal) - .def("get_node", - &DynamicSceneGraph::getNode, - py::return_value_policy::reference_internal) - .def("find_node", - &DynamicSceneGraph::findNode, - py::return_value_policy::reference_internal) - .def("get_edge", - &DynamicSceneGraph::getEdge, - py::return_value_policy::reference_internal) - .def("find_edge", - &DynamicSceneGraph::findEdge, - py::return_value_policy::reference_internal) - .def("remove_node", &DynamicSceneGraph::removeNode) - .def("remove_edge", &DynamicSceneGraph::removeEdge) - .def("is_dynamic", &DynamicSceneGraph::isDynamic) - .def("num_layers", &DynamicSceneGraph::numLayers) - .def("num_dynamic_layers_of_type", &DynamicSceneGraph::numDynamicLayersOfType) - .def("num_dynamic_layers", &DynamicSceneGraph::numDynamicLayers) - .def("num_nodes", &DynamicSceneGraph::numNodes, "include_mesh"_a = false) - .def("num_static_nodes", &DynamicSceneGraph::numStaticNodes) - .def("num_dynamic_nodes", &DynamicSceneGraph::numDynamicNodes) - .def("empty", &DynamicSceneGraph::empty) - .def("num_edges", &DynamicSceneGraph::numEdges) - .def("num_static_edges", &DynamicSceneGraph::numStaticEdges) - .def("num_dynamic_edges", &DynamicSceneGraph::numDynamicEdges) - .def("get_position", &DynamicSceneGraph::getPosition) - .def( - "save", - [](const DynamicSceneGraph& graph, - const std::string& filepath, - bool include_mesh) { graph.save(filepath, include_mesh); }, - "filepath"_a, - "include_mesh"_a = true) - .def( - "save", - [](const DynamicSceneGraph& graph, - const std::filesystem::path& filepath, - bool include_mesh) { graph.save(filepath, include_mesh); }, - "filepath"_a, - "include_mesh"_a = true) - .def_static("load", &DynamicSceneGraph::load) - .def_static("load", - [](const std::filesystem::path& filepath) { - return DynamicSceneGraph::load(filepath); - }) - .def_property( - "layers", - [](const DynamicSceneGraph& graph) { - return py::make_iterator(LayerIter(graph.layers()), IterSentinel()); - }, - nullptr, - py::return_value_policy::reference_internal) - .def_property( - "dynamic_layers", - [](const DynamicSceneGraph& graph) { - return py::make_iterator(DynamicLayerIter(graph.dynamicLayers()), - IterSentinel()); - }, - nullptr, - py::return_value_policy::reference_internal) - .def_property( - "nodes", - [](const DynamicSceneGraph& graph) { - return py::make_iterator(GlobalNodeIter(graph), IterSentinel()); - }, - nullptr, - py::return_value_policy::reference_internal) - .def_property( - "edges", - [](const DynamicSceneGraph& graph) { - return py::make_iterator(GlobalEdgeIter(graph), IterSentinel()); - }, - nullptr, - py::return_value_policy::reference_internal) - .def_property( - "interlayer_edges", - [](const DynamicSceneGraph& graph) { - return py::make_iterator(EdgeIter(graph.interlayer_edges()), - IterSentinel()); - }, - nullptr, - py::return_value_policy::reference_internal) - .def_property( - "dynamic_interlayer_edges", - [](const DynamicSceneGraph& graph) { - return py::make_iterator(EdgeIter(graph.dynamic_interlayer_edges()), - IterSentinel()); - }, - nullptr, - py::return_value_policy::reference_internal) - .def_property( - "mesh", - [](const DynamicSceneGraph& graph) { return graph.mesh(); }, - [](DynamicSceneGraph& graph, const Mesh::Ptr& mesh) { graph.setMesh(mesh); }) - .def("clone", &DynamicSceneGraph::clone) - .def("__deepcopy__", - [](const DynamicSceneGraph& G, py::object) { return G.clone(); }) - .def( - "to_binary", - [](const DynamicSceneGraph& graph, bool include_mesh) { - std::vector buffer; - io::binary::writeGraph(graph, buffer, include_mesh); - return py::bytes(reinterpret_cast(buffer.data()), buffer.size()); - }, - "include_mesh"_a = false) - .def( - "update_from_binary", - [](DynamicSceneGraph& graph, const py::bytes& contents) { - const auto& view = static_cast(contents); - return io::binary::updateGraph( - graph, reinterpret_cast(view.data()), view.size()); - }, - "contents"_a) - .def_static("from_binary", [](const py::bytes& contents) { - const auto& view = static_cast(contents); - return io::binary::readGraph(reinterpret_cast(view.data()), - view.size()); - }); - - module.def("compute_ancestor_bounding_box", - &computeAncestorBoundingBox, - "G"_a, - "node_id"_a, - "child_layer"_a = DsgLayers::PLACES, - "bbox_type"_a = BoundingBox::Type::AABB); - - py::implicitly_convertible(); -} diff --git a/python/bindings/src/bounding_box.cpp b/python/bindings/src/bounding_box.cpp new file mode 100644 index 0000000..94e993e --- /dev/null +++ b/python/bindings/src/bounding_box.cpp @@ -0,0 +1,88 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "spark_dsg/python/bounding_box.h" + +#include +#include +#include + +namespace py = pybind11; +using namespace py::literals; + +namespace spark_dsg::python::bounding_box { + +void addBindings(pybind11::module_& module) { + py::enum_(module, "BoundingBoxType") + .value("INVALID", BoundingBox::Type::INVALID) + .value("AABB", BoundingBox::Type::AABB) + .value("OBB", BoundingBox::Type::OBB) + .value("RAABB", BoundingBox::Type::RAABB); + + py::class_(module, "BoundingBox") + .def(py::init<>()) + .def(py::init()) + .def(py::init()) + .def(py::init()) + .def(py::init([](BoundingBox::Type type, + const Eigen::Vector3f& dim, + const Eigen::Vector3f& pos, + const Eigen::Matrix3f& trans) { + return BoundingBox(type, dim, pos, trans); + })) + .def_readwrite("type", &BoundingBox::type) + .def_readwrite("dimensions", &BoundingBox::dimensions) + .def_readwrite("world_P_center", &BoundingBox::world_P_center) + .def_readwrite("world_R_center", &BoundingBox::world_R_center) + .def("is_valid", &BoundingBox::isValid) + .def("volume", &BoundingBox::volume) + .def("has_rotation", &BoundingBox::hasRotation) + .def("corners", &BoundingBox::corners) + .def("contains", + static_cast( + &BoundingBox::contains)) + .def("intersects", + static_cast( + &BoundingBox::intersects)) + .def("compute_iou", + static_cast( + &BoundingBox::computeIoU)) + .def("__repr__", [](const BoundingBox& box) { + std::stringstream ss; + ss << box; + return ss.str(); + }); +} + +} // namespace spark_dsg::python::bounding_box diff --git a/python/bindings/src/dynamic_scene_graph.cpp b/python/bindings/src/dynamic_scene_graph.cpp new file mode 100644 index 0000000..91577c2 --- /dev/null +++ b/python/bindings/src/dynamic_scene_graph.cpp @@ -0,0 +1,304 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "spark_dsg/python/dynamic_scene_graph.h" + +#include +#include +#include +#include + +#include "spark_dsg/python/scene_graph_iterators.h" + +namespace py = pybind11; +using namespace py::literals; + +namespace spark_dsg::python::dynamic_scene_graph { + +void addBindings(pybind11::module_& module) { + py::class_(module, "LayerView") + .def("has_node", &LayerView::hasNode) + .def("has_edge", &LayerView::hasEdge) + .def("get_node", &LayerView::getNode, py::return_value_policy::reference_internal) + .def("get_edge", &LayerView::getEdge, py::return_value_policy::reference_internal) + .def("num_nodes", &LayerView::numNodes) + .def("num_edges", &LayerView::numEdges) + .def("get_position", &LayerView::getPosition) + .def_readonly("id", &LayerView::id) + .def_property( + "nodes", + [](const LayerView& view) { + return py::make_iterator(view.nodes(), IterSentinel()); + }, + nullptr, + py::return_value_policy::reference_internal) + .def_property( + "edges", + [](const LayerView& view) { + return py::make_iterator(view.edges(), IterSentinel()); + }, + nullptr, + py::return_value_policy::reference_internal); + + py::class_(module, "DynamicLayerView") + .def_readonly("id", &DynamicLayerView::id) + .def_readonly("prefix", &DynamicLayerView::prefix) + .def("num_nodes", &DynamicLayerView::numNodes) + .def("num_edges", &DynamicLayerView::numEdges) + .def_property( + "nodes", + [](const DynamicLayerView& view) { + return py::make_iterator(view.nodes(), IterSentinel()); + }, + nullptr, + py::return_value_policy::reference_internal) + .def_property( + "edges", + [](const DynamicLayerView& view) { + return py::make_iterator(view.edges(), IterSentinel()); + }, + nullptr, + py::return_value_policy::reference_internal); + + py::class_(module, "Color") + .def_readwrite("r", &Color::r) + .def_readwrite("g", &Color::g) + .def_readwrite("b", &Color::b) + .def_readwrite("a", &Color::a); + + py::class_>( + module, "DynamicSceneGraph", py::dynamic_attr()) + .def(py::init<>()) + .def(py::init()) + .def("clear", &DynamicSceneGraph::clear) + .def("create_dynamic_layer", &DynamicSceneGraph::createDynamicLayer) + .def("add_node", + [](DynamicSceneGraph& graph, + LayerId layer_id, + NodeId node_id, + const NodeAttributes& attrs) { + graph.emplaceNode(layer_id, node_id, attrs.clone()); + }) + .def( + "add_node", + [](DynamicSceneGraph& graph, + LayerId layer_id, + LayerPrefix prefix, + std::chrono::nanoseconds timestamp, + const NodeAttributes& attrs, + bool add_edge_to_previous) { + graph.emplaceNode( + layer_id, prefix, timestamp, attrs.clone(), add_edge_to_previous); + }, + "layer_id"_a, + "prefix"_a, + "timestamp"_a, + "attrs"_a, + "add_edge_to_previous"_a = true) + .def( + "insert_edge", + [](DynamicSceneGraph& graph, + NodeId source, + NodeId target, + bool enforce_single_parent) { + return enforce_single_parent ? graph.insertParentEdge(source, target) + : graph.insertEdge(source, target); + }, + "source"_a, + "target"_a, + "enforce_single_parent"_a = false) + .def( + "insert_edge", + [](DynamicSceneGraph& graph, + NodeId source, + NodeId target, + const EdgeAttributes& info, + bool enforce_single_parent) { + return enforce_single_parent + ? graph.insertParentEdge(source, target, info.clone()) + : graph.insertEdge(source, target, info.clone()); + }, + "source"_a, + "target"_a, + "info"_a, + "enforce_single_parent"_a = false) + .def("has_layer", + static_cast( + &DynamicSceneGraph::hasLayer)) + .def("has_layer", + static_cast( + &DynamicSceneGraph::hasLayer)) + .def("has_node", &DynamicSceneGraph::hasNode) + .def("has_edge", + py::overload_cast(&DynamicSceneGraph::hasEdge, py::const_)) + .def("has_mesh", &DynamicSceneGraph::hasMesh) + .def( + "get_layer", + [](const DynamicSceneGraph& graph, LayerId layer_id) { + if (!graph.hasLayer(layer_id)) { + throw std::out_of_range("layer doesn't exist"); + } + return LayerView(graph.getLayer(layer_id)); + }, + py::return_value_policy::reference_internal) + .def( + "get_dynamic_layer", + [](const DynamicSceneGraph& graph, LayerId layer_id, LayerPrefix prefix) { + if (!graph.hasLayer(layer_id, prefix)) { + throw std::out_of_range("layer doesn't exist"); + } + return DynamicLayerView(graph.getLayer(layer_id, prefix)); + }, + py::return_value_policy::reference_internal) + .def("get_node", + &DynamicSceneGraph::getNode, + py::return_value_policy::reference_internal) + .def("find_node", + &DynamicSceneGraph::findNode, + py::return_value_policy::reference_internal) + .def("get_edge", + &DynamicSceneGraph::getEdge, + py::return_value_policy::reference_internal) + .def("find_edge", + &DynamicSceneGraph::findEdge, + py::return_value_policy::reference_internal) + .def("remove_node", &DynamicSceneGraph::removeNode) + .def("remove_edge", &DynamicSceneGraph::removeEdge) + .def("is_dynamic", &DynamicSceneGraph::isDynamic) + .def("num_layers", &DynamicSceneGraph::numLayers) + .def("num_dynamic_layers_of_type", &DynamicSceneGraph::numDynamicLayersOfType) + .def("num_dynamic_layers", &DynamicSceneGraph::numDynamicLayers) + .def("num_nodes", &DynamicSceneGraph::numNodes, "include_mesh"_a = false) + .def("num_static_nodes", &DynamicSceneGraph::numStaticNodes) + .def("num_dynamic_nodes", &DynamicSceneGraph::numDynamicNodes) + .def("empty", &DynamicSceneGraph::empty) + .def("num_edges", &DynamicSceneGraph::numEdges) + .def("num_static_edges", &DynamicSceneGraph::numStaticEdges) + .def("num_dynamic_edges", &DynamicSceneGraph::numDynamicEdges) + .def("get_position", &DynamicSceneGraph::getPosition) + .def( + "save", + [](const DynamicSceneGraph& graph, + const std::string& filepath, + bool include_mesh) { graph.save(filepath, include_mesh); }, + "filepath"_a, + "include_mesh"_a = true) + .def( + "save", + [](const DynamicSceneGraph& graph, + const std::filesystem::path& filepath, + bool include_mesh) { graph.save(filepath, include_mesh); }, + "filepath"_a, + "include_mesh"_a = true) + .def_static("load", &DynamicSceneGraph::load) + .def_static("load", + [](const std::filesystem::path& filepath) { + return DynamicSceneGraph::load(filepath); + }) + .def_property( + "layers", + [](const DynamicSceneGraph& graph) { + return py::make_iterator(LayerIter(graph.layers()), IterSentinel()); + }, + nullptr, + py::return_value_policy::reference_internal) + .def_property( + "dynamic_layers", + [](const DynamicSceneGraph& graph) { + return py::make_iterator(DynamicLayerIter(graph.dynamicLayers()), + IterSentinel()); + }, + nullptr, + py::return_value_policy::reference_internal) + .def_property( + "nodes", + [](const DynamicSceneGraph& graph) { + return py::make_iterator(GlobalNodeIter(graph), IterSentinel()); + }, + nullptr, + py::return_value_policy::reference_internal) + .def_property( + "edges", + [](const DynamicSceneGraph& graph) { + return py::make_iterator(GlobalEdgeIter(graph), IterSentinel()); + }, + nullptr, + py::return_value_policy::reference_internal) + .def_property( + "interlayer_edges", + [](const DynamicSceneGraph& graph) { + return py::make_iterator(EdgeIter(graph.interlayer_edges()), + IterSentinel()); + }, + nullptr, + py::return_value_policy::reference_internal) + .def_property( + "dynamic_interlayer_edges", + [](const DynamicSceneGraph& graph) { + return py::make_iterator(EdgeIter(graph.dynamic_interlayer_edges()), + IterSentinel()); + }, + nullptr, + py::return_value_policy::reference_internal) + .def_property( + "mesh", + [](const DynamicSceneGraph& graph) { return graph.mesh(); }, + [](DynamicSceneGraph& graph, const Mesh::Ptr& mesh) { graph.setMesh(mesh); }) + .def("clone", &DynamicSceneGraph::clone) + .def("__deepcopy__", + [](const DynamicSceneGraph& G, py::object) { return G.clone(); }) + .def( + "to_binary", + [](const DynamicSceneGraph& graph, bool include_mesh) { + std::vector buffer; + io::binary::writeGraph(graph, buffer, include_mesh); + return py::bytes(reinterpret_cast(buffer.data()), buffer.size()); + }, + "include_mesh"_a = false) + .def( + "update_from_binary", + [](DynamicSceneGraph& graph, const py::bytes& contents) { + const auto& view = static_cast(contents); + return io::binary::updateGraph( + graph, reinterpret_cast(view.data()), view.size()); + }, + "contents"_a) + .def_static("from_binary", [](const py::bytes& contents) { + const auto& view = static_cast(contents); + return io::binary::readGraph(reinterpret_cast(view.data()), + view.size()); + }); +} + +} // namespace spark_dsg::python::dynamic_scene_graph diff --git a/python/bindings/src/edge_attributes.cpp b/python/bindings/src/edge_attributes.cpp new file mode 100644 index 0000000..da1bc2a --- /dev/null +++ b/python/bindings/src/edge_attributes.cpp @@ -0,0 +1,53 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "spark_dsg/python/edge_attributes.h" + +#include +#include +#include + +namespace py = pybind11; +using namespace py::literals; + +namespace spark_dsg::python::edge_attributes { + +void addBindings(pybind11::module_& module) { + py::class_(module, "EdgeAttributes") + .def(py::init<>()) + .def_readwrite("weighted", &EdgeAttributes::weighted) + .def_readwrite("weight", &EdgeAttributes::weight); +} + +} // namespace spark_dsg::python::edge_attributes diff --git a/python/bindings/src/mesh.cpp b/python/bindings/src/mesh.cpp new file mode 100644 index 0000000..201ce2d --- /dev/null +++ b/python/bindings/src/mesh.cpp @@ -0,0 +1,175 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "spark_dsg/python/mesh.h" + +#include +#include +#include +#include + +namespace py = pybind11; +using namespace py::literals; + +namespace spark_dsg::python::mesh { + +Eigen::MatrixXd getEigenVertices(const Mesh& mesh) { + const auto num_vertices = mesh.numVertices(); + Eigen::MatrixXd to_return(6, num_vertices); + for (size_t i = 0; i < num_vertices; ++i) { + const auto& pos = mesh.pos(i); + to_return(0, i) = pos.x(); + to_return(1, i) = pos.y(); + to_return(2, i) = pos.z(); + if (i < mesh.colors.size()) { + const auto c = mesh.color(i); + to_return(3, i) = c.r / 255.0; + to_return(4, i) = c.g / 255.0; + to_return(5, i) = c.b / 255.0; + } else { + to_return(3, i) = 0.0; + to_return(4, i) = 0.0; + to_return(5, i) = 0.0; + } + } + return to_return; +} + +void setEigenVertices(Mesh& mesh, const Eigen::MatrixXd& points) { + if (points.rows() != 6) { + std::stringstream ss; + ss << "point rows do not match expected: " << points.rows() << " != 6"; + throw std::invalid_argument(ss.str()); + } + + mesh.resizeVertices(points.cols()); + for (int i = 0; i < points.cols(); ++i) { + Eigen::Vector3f pos = points.col(i).head<3>().cast(); + mesh.setPos(i, pos); + if (mesh.has_colors) { + Color color{static_cast(points(3, i) * 255), + static_cast(points(4, i) * 255), + static_cast(points(5, i) * 255), + 255}; + mesh.setColor(i, color); + } + } +} + +Eigen::MatrixXi getEigenFaces(const Mesh& mesh) { + const auto num_faces = mesh.numFaces(); + Eigen::MatrixXi to_return(3, num_faces); + for (size_t i = 0; i < num_faces; ++i) { + const auto& face = mesh.face(i); + to_return(0, i) = face[0]; + to_return(1, i) = face[1]; + to_return(2, i) = face[2]; + } + return to_return; +} + +void setEigenFaces(Mesh& mesh, const Eigen::MatrixXi& indices) { + if (indices.rows() != 3) { + std::stringstream ss; + ss << "index rows do not match expected: " << indices.rows() << " != 3"; + throw std::invalid_argument(ss.str()); + } + + mesh.resizeFaces(indices.cols()); + for (int i = 0; i < indices.cols(); ++i) { + Mesh::Face face{{static_cast(indices(0, i)), + static_cast(indices(1, i)), + static_cast(indices(2, i))}}; + mesh.face(i) = face; + } +} + +void addBindings(pybind11::module_& module) { + py::class_>(module, "Mesh") + .def(py::init(), + "has_colors"_a = true, + "has_timestamps"_a = true, + "has_labels"_a = "true") + .def("empty", &Mesh::empty) + .def("num_vertices", &Mesh::numVertices) + .def("num_faces", &Mesh::numFaces) + .def("resize_vertices", &Mesh::resizeVertices) + .def("resize_faces", &Mesh::resizeFaces) + .def("clone", &Mesh::clone) + .def("pos", &Mesh::pos) + .def("set_pos", &Mesh::setPos) + .def("color", &Mesh::color) + .def("set_color", &Mesh::setColor) + .def("timestamp", &Mesh::timestamp) + .def("set_timestamp", &Mesh::setTimestamp) + .def("label", &Mesh::label) + .def("set_label", &Mesh::setLabel) + .def("face", py::overload_cast(&Mesh::face, py::const_)) + .def("set_face", + [](Mesh& mesh, size_t index, const Mesh::Face& face) { + mesh.face(index) = face; + }) + .def("to_json", &Mesh::serializeToJson) + .def("to_binary", + [](const Mesh& mesh) { + std::vector buffer; + mesh.serializeToBinary(buffer); + return py::bytes(reinterpret_cast(buffer.data()), buffer.size()); + }) + .def("save", &Mesh::save) + .def("save", + [](const Mesh& mesh, const std::filesystem::path& path) { mesh.save(path); }) + .def_static("from_json", &Mesh::deserializeFromJson) + .def_static("from_binary", + [](const py::bytes& contents) { + const auto& view = static_cast(contents); + return Mesh::deserializeFromBinary( + reinterpret_cast(view.data()), view.size()); + }) + .def_static("load", &Mesh::load) + .def_static("load", + [](const std::filesystem::path& path) { return Mesh::load(path); }) + .def("get_vertices", [](const Mesh& mesh) { return getEigenVertices(mesh); }) + .def("get_faces", [](const Mesh& mesh) { return getEigenFaces(mesh); }) + .def("get_labels", [](const Mesh& mesh) { return mesh.labels; }) + .def("set_vertices", + [](Mesh& mesh, const Eigen::MatrixXd& points) { + setEigenVertices(mesh, points); + }) + .def("set_faces", [](Mesh& mesh, const Eigen::MatrixXi& faces) { + setEigenFaces(mesh, faces); + }); +} + +} // namespace spark_dsg::python::mesh diff --git a/python/bindings/src/misc_types.cpp b/python/bindings/src/misc_types.cpp new file mode 100644 index 0000000..5c126e6 --- /dev/null +++ b/python/bindings/src/misc_types.cpp @@ -0,0 +1,85 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "spark_dsg/python/misc_types.h" + +#include +#include +#include + +namespace py = pybind11; +using namespace py::literals; + +namespace spark_dsg::python::misc_types { + +void addBindings(pybind11::module_& module) { + py::class_(module, "DsgLayers") + .def_readonly_static("SEGMENTS", &DsgLayers::SEGMENTS) + .def_readonly_static("OBJECTS", &DsgLayers::OBJECTS) + .def_readonly_static("AGENTS", &DsgLayers::AGENTS) + .def_readonly_static("PLACES", &DsgLayers::PLACES) + .def_readonly_static("MESH_PLACES", &DsgLayers::MESH_PLACES) + .def_readonly_static("STRUCTURE", &DsgLayers::STRUCTURE) + .def_readonly_static("ROOMS", &DsgLayers::ROOMS) + .def_readonly_static("BUILDINGS", &DsgLayers::BUILDINGS); + + py::class_(module, "LayerPrefix") + .def(py::init([](char key) { return LayerPrefix(key); })) + .def(py::init([](char key, uint32_t index) { return LayerPrefix(key, index); })) + .def(py::init([](uint32_t index) { return LayerPrefix(index); })) + .def_property( + "value", + [](const LayerPrefix& prefix) { return static_cast(prefix); }, + nullptr) + .def("__repr__", [](const LayerPrefix& prefix) { return prefix.str(true); }); + + py::class_(module, "NodeSymbol") + .def(py::init([](char key, size_t index) { return NodeSymbol(key, index); })) + .def(py::init([](size_t value) { return NodeSymbol(value); })) + .def_property("category_id", &NodeSymbol::categoryId, nullptr) + .def_property("category", &NodeSymbol::category, nullptr) + .def_property( + "value", + [](const NodeSymbol& symbol) { return static_cast(symbol); }, + nullptr) + .def("__repr__", &NodeSymbol::getLabel) + .def("__hash__", + [](const NodeSymbol& symbol) { return static_cast(symbol); }) + .def(pybind11::self == pybind11::self) + .def(pybind11::self != pybind11::self); + + py::implicitly_convertible(); +} + +} // namespace spark_dsg::python::misc_types diff --git a/python/bindings/src/node_attributes.cpp b/python/bindings/src/node_attributes.cpp new file mode 100644 index 0000000..b4b8983 --- /dev/null +++ b/python/bindings/src/node_attributes.cpp @@ -0,0 +1,253 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "spark_dsg/python/node_attributes.h" + +#include +#include +#include +#include + +#include + +namespace py = pybind11; +using namespace py::literals; + +namespace spark_dsg::python::node_attributes { + +template +struct Quaternion { + Quaternion() : w(1.0f), x(0.0f), y(0.0f), z(0.0f) {} + + Quaternion(Scalar w, Scalar x, Scalar y, Scalar z) : w(w), x(x), y(y), z(z) {} + + explicit Quaternion(const Eigen::Quaternion other) { + w = other.w(); + x = other.x(); + y = other.y(); + z = other.z(); + } + + operator Eigen::Quaternion() const { + return Eigen::Quaternion(w, x, y, z); + } + + Scalar w; + Scalar x; + Scalar y; + Scalar z; +}; + +template +std::ostream& operator<<(std::ostream& out, const Quaternion& q) { + out << "Quaternion"; + return out; +} + +void addBindings(pybind11::module_& module) { + py::class_>(module, "Quaternionf") + .def(py::init<>()) + .def(py::init()) + .def_readwrite("w", &Quaternion::w) + .def_readwrite("x", &Quaternion::x) + .def_readwrite("y", &Quaternion::y) + .def_readwrite("z", &Quaternion::z) + .def("__repr__", [](const Quaternion& q) { + std::stringstream ss; + ss << q; + return ss.str(); + }); + + py::class_>(module, "Quaterniond") + .def(py::init<>()) + .def(py::init()) + .def_readwrite("w", &Quaternion::w) + .def_readwrite("x", &Quaternion::x) + .def_readwrite("y", &Quaternion::y) + .def_readwrite("z", &Quaternion::z) + .def("__repr__", [](const Quaternion& q) { + std::stringstream ss; + ss << q; + return ss.str(); + }); + + py::class_(module, "NodeAttributes") + .def(py::init<>()) + .def_readwrite("position", &NodeAttributes::position) + .def_readwrite("last_update_time_ns", &NodeAttributes::last_update_time_ns) + .def_readwrite("is_active", &NodeAttributes::is_active) + .def_readwrite("is_predicted", &NodeAttributes::is_predicted) + .def("__repr__", [](const NodeAttributes& attrs) { + std::stringstream ss; + ss << attrs; + return ss.str(); + }); + + py::class_(module, "SemanticNodeAttributes") + .def(py::init<>()) + .def_readwrite("name", &SemanticNodeAttributes::name) + .def_property( + "color", + [](const SemanticNodeAttributes& attrs) { + Eigen::Matrix color; + color << attrs.color.r, attrs.color.g, attrs.color.b; + return color; + }, + [](SemanticNodeAttributes& attrs, const Eigen::Matrix color) { + attrs.color.r = color(0); + attrs.color.g = color(1); + attrs.color.b = color(2); + }) + .def_readwrite("bounding_box", &SemanticNodeAttributes::bounding_box) + .def_readwrite("semantic_label", &SemanticNodeAttributes::semantic_label) + .def_readwrite("semantic_feature", &SemanticNodeAttributes::semantic_feature) + .def_readonly_static("NO_SEMANTIC_LABEL", + &SemanticNodeAttributes::NO_SEMANTIC_LABEL); + + py::class_(module, + "ObjectNodeAttributes") + .def(py::init<>()) + .def_readwrite("registered", &ObjectNodeAttributes::registered) + .def_property( + "world_R_object", + [](const ObjectNodeAttributes& attrs) { + return Quaternion(attrs.world_R_object); + }, + [](ObjectNodeAttributes& attrs, const Quaternion& rot) { + attrs.world_R_object = rot; + }); + + py::class_(module, "RoomNodeAttributes") + .def(py::init<>()) + .def_readwrite("semantic_class_probabilities", + &RoomNodeAttributes::semantic_class_probabilities); + + py::class_(module, "NearestVertexInfo") + .def(py::init<>()) + .def_property( + "block", + [](const NearestVertexInfo& info) -> std::array { + return {info.block[0], info.block[1], info.block[2]}; + }, + [](NearestVertexInfo& info, const std::array& array) { + info.block[0] = array[0]; + info.block[1] = array[1]; + info.block[2] = array[2]; + }) + .def_property( + "voxel_pos", + [](const NearestVertexInfo& info) -> std::array { + return {info.voxel_pos[0], info.voxel_pos[1], info.voxel_pos[2]}; + }, + [](NearestVertexInfo& info, const std::array& array) { + info.voxel_pos[0] = array[0]; + info.voxel_pos[1] = array[1]; + info.voxel_pos[2] = array[2]; + }) + .def_readwrite("vertex", &NearestVertexInfo::vertex) + .def_readwrite("label", &NearestVertexInfo::label) + .def("__repr__", [](const NearestVertexInfo& info) { + std::stringstream ss; + ss << std::setprecision(6) << "VertexInfo"; + return ss.str(); + }); + + py::class_(module, "PlaceNodeAttributes") + .def(py::init<>()) + .def_readwrite("distance", &PlaceNodeAttributes::distance) + .def_readwrite("num_basis_points", &PlaceNodeAttributes::num_basis_points) + .def_readwrite("voxblox_mesh_connections", + &PlaceNodeAttributes::voxblox_mesh_connections) + .def_readwrite("pcl_mesh_connections", &PlaceNodeAttributes::pcl_mesh_connections) + .def_readwrite("mesh_vertex_labels", &PlaceNodeAttributes::mesh_vertex_labels) + .def_readwrite("deformation_connections", + &PlaceNodeAttributes::deformation_connections) + .def_readwrite("real_place", &PlaceNodeAttributes::real_place) + .def_readwrite("active_frontier", &PlaceNodeAttributes::active_frontier) + .def_readwrite("frontier_scale", &PlaceNodeAttributes::frontier_scale) + .def_property( + "orientation", + [](const PlaceNodeAttributes& attrs) { + return Quaternion(attrs.orientation); + }, + [](PlaceNodeAttributes& attrs, const Quaternion& rot) { + attrs.orientation = rot; + }) + .def_readwrite("num_frontier_voxels", &PlaceNodeAttributes::num_frontier_voxels) + .def_readwrite("need_cleanup", &PlaceNodeAttributes::need_cleanup); + + py::class_(module, + "Place2dNodeAttributes") + .def(py::init<>()) + .def_readwrite("boundary", &Place2dNodeAttributes::boundary) + .def_readwrite("ellipse_matrix_compress", + &Place2dNodeAttributes::ellipse_matrix_compress) + .def_readwrite("ellipse_matrix_expand", + &Place2dNodeAttributes::ellipse_matrix_expand) + .def_readwrite("ellipse_centroid", &Place2dNodeAttributes::ellipse_centroid) + .def_readwrite("pcl_boundary_connections", + &Place2dNodeAttributes::pcl_boundary_connections) + .def_readwrite("voxblox_mesh_connections", + &Place2dNodeAttributes::voxblox_mesh_connections) + .def_readwrite("pcl_mesh_connections", + &Place2dNodeAttributes::pcl_mesh_connections) + .def_readwrite("mesh_vertex_labels", &Place2dNodeAttributes::mesh_vertex_labels) + .def_readwrite("deformation_connections", + &Place2dNodeAttributes::deformation_connections); + + py::class_(module, "AgentNodeAttributes") + .def(py::init<>()) + .def_property( + "world_R_body", + [](const AgentNodeAttributes& attrs) { + return Quaternion(attrs.world_R_body); + }, + [](AgentNodeAttributes& attrs, const Quaternion& rot) { + attrs.world_R_body = rot; + }) + .def_readwrite("external_key", &AgentNodeAttributes::external_key) + .def_readwrite("dbow_ids", &AgentNodeAttributes::dbow_ids) + .def_readwrite("dbow_values", &AgentNodeAttributes::dbow_values); +} + +} // namespace spark_dsg::python::node_attributes diff --git a/python/bindings/src/scene_graph_edge.cpp b/python/bindings/src/scene_graph_edge.cpp new file mode 100644 index 0000000..f722600 --- /dev/null +++ b/python/bindings/src/scene_graph_edge.cpp @@ -0,0 +1,99 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include + +#include "spark_dsg/edge_attributes.h" + +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include +#include + +#include "spark_dsg/python/scene_graph_edge.h" + +namespace py = pybind11; +using namespace py::literals; + +namespace spark_dsg::python::scene_graph_edge { + +void addBindings(pybind11::module_& module) { + py::class_(module, "SceneGraphEdge") + .def_readonly("source", &SceneGraphEdge::source) + .def_readonly("target", &SceneGraphEdge::target) + .def_property( + "info", + [](const SceneGraphEdge& edge) { return *(edge.info); }, + [](SceneGraphEdge& edge, const EdgeAttributes& info) { *edge.info = info; }) + .def("__repr__", [](const SceneGraphEdge& edge) { + std::stringstream ss; + ss << "Edge"; + return ss.str(); + }); +} + +} // namespace spark_dsg::python::scene_graph_edge diff --git a/python/bindings/src/scene_graph_layer.cpp b/python/bindings/src/scene_graph_layer.cpp new file mode 100644 index 0000000..1b9f385 --- /dev/null +++ b/python/bindings/src/scene_graph_layer.cpp @@ -0,0 +1,104 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "spark_dsg/python/scene_graph_layer.h" + +#include +#include +#include + +#include "spark_dsg/python/scene_graph_iterators.h" + +namespace py = pybind11; +using namespace py::literals; + +namespace spark_dsg::python::scene_graph_layer { + +void addBindings(pybind11::module_& module) { + // TODO(nathan) iterator over nodes and edges + py::class_>( + module, "SceneGraphLayer") + .def(py::init()) + .def( + "add_node", + [](IsolatedSceneGraphLayer& layer, NodeId node, const NodeAttributes& attrs) { + layer.emplaceNode(node, attrs.clone()); + }) + .def("insert_edge", + [](IsolatedSceneGraphLayer& layer, NodeId source, NodeId target) { + return layer.insertEdge(source, target); + }) + .def("insert_edge", + [](IsolatedSceneGraphLayer& layer, + NodeId source, + NodeId target, + const EdgeAttributes& info) { + return layer.insertEdge(source, target, info.clone()); + }) + .def("has_node", &IsolatedSceneGraphLayer::hasNode) + .def("has_edge", &IsolatedSceneGraphLayer::hasEdge) + .def("get_node", + &IsolatedSceneGraphLayer::getNode, + py::return_value_policy::reference_internal) + .def("find_node", + &IsolatedSceneGraphLayer::findNode, + py::return_value_policy::reference_internal) + .def("get_edge", + &IsolatedSceneGraphLayer::getEdge, + py::return_value_policy::reference_internal) + .def("find_edge", + &IsolatedSceneGraphLayer::findEdge, + py::return_value_policy::reference_internal) + .def("remove_edge", &IsolatedSceneGraphLayer::removeEdge) + .def("num_nodes", &IsolatedSceneGraphLayer::numNodes) + .def("num_edges", &IsolatedSceneGraphLayer::numEdges) + .def("get_position", &IsolatedSceneGraphLayer::getPosition) + .def_readonly("id", &IsolatedSceneGraphLayer::id) + .def_property( + "nodes", + [](const IsolatedSceneGraphLayer& view) { + return py::make_iterator(NodeIter(view.nodes()), IterSentinel()); + }, + nullptr, + py::return_value_policy::reference_internal) + .def_property( + "edges", + [](const IsolatedSceneGraphLayer& view) { + return py::make_iterator(EdgeIter(view.edges()), IterSentinel()); + }, + nullptr, + py::return_value_policy::reference_internal); +} + +} // namespace spark_dsg::python::scene_graph_layer diff --git a/python/bindings/src/scene_graph_node.cpp b/python/bindings/src/scene_graph_node.cpp new file mode 100644 index 0000000..ed16bcc --- /dev/null +++ b/python/bindings/src/scene_graph_node.cpp @@ -0,0 +1,76 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "spark_dsg/python/scene_graph_node.h" + +#include +#include +#include + +namespace py = pybind11; +using namespace py::literals; + +namespace spark_dsg::python::scene_graph_node { + +void addBindings(pybind11::module_& module) { + py::class_(module, "SceneGraphNode") + .def("has_parent", &SceneGraphNode::hasParent) + .def("has_siblings", &SceneGraphNode::hasSiblings) + .def("has_children", &SceneGraphNode::hasChildren) + .def("get_parent", &SceneGraphNode::getParent) + .def("siblings", &SceneGraphNode::siblings) + .def("children", &SceneGraphNode::children) + .def_property_readonly("timestamp", + [](const SceneGraphNode& node) -> std::optional { + if (node.timestamp) { + return node.timestamp.value().count(); + } else { + return std::nullopt; + } + }) + .def_property("attributes", + &SceneGraphNode::getAttributesPtr, + &SceneGraphNode::getAttributesPtr, + py::return_value_policy::reference_internal) + .def_property_readonly( + "id", [](const SceneGraphNode& node) { return NodeSymbol(node.id); }) + .def_readonly("layer", &SceneGraphNode::layer) + .def("__repr__", [](const SceneGraphNode& node) { + std::stringstream ss; + ss << node; + return ss.str(); + }); +} + +} // namespace spark_dsg::python::scene_graph_node diff --git a/python/bindings/src/scene_graph_utilities.cpp b/python/bindings/src/scene_graph_utilities.cpp new file mode 100644 index 0000000..0f8339e --- /dev/null +++ b/python/bindings/src/scene_graph_utilities.cpp @@ -0,0 +1,53 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "spark_dsg/python/scene_graph_utilities.h" + +#include +#include +#include + +namespace py = pybind11; +using namespace py::literals; + +namespace spark_dsg::python::scene_graph_utilities { + +module.def("compute_ancestor_bounding_box", + &computeAncestorBoundingBox, + "G"_a, + "node_id"_a, + "child_layer"_a = DsgLayers::PLACES, + "bbox_type"_a = BoundingBox::Type::AABB); + +} // namespace spark_dsg::python::scene_graph_utilities diff --git a/python/bindings/src/spark_dsg_bindings.cpp b/python/bindings/src/spark_dsg_bindings.cpp new file mode 100644 index 0000000..70d0c49 --- /dev/null +++ b/python/bindings/src/spark_dsg_bindings.cpp @@ -0,0 +1,67 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include + +#include "spark_dsg/python/bounding_box.h" +#include "spark_dsg/python/dynamic_scene_graph.h" +#include "spark_dsg/python/edge_attributes.h" +#include "spark_dsg/python/mesh.h" +#include "spark_dsg/python/misc_types.h" +#include "spark_dsg/python/node_attributes.h" +#include "spark_dsg/python/scene_graph_edge.h" +#include "spark_dsg/python/scene_graph_layer.h" +#include "spark_dsg/python/scene_graph_node.h" +#include "spark_dsg/python/scene_graph_utilities.h" +#include "spark_dsg/python/zmq_interface.h" + +namespace py = pybind11; +using namespace py::literals; + +PYBIND11_MODULE(_dsg_bindings, module) { + py::options options; + // options.disable_function_signatures(); + + spark_dsg::python::bounding_box::addBindings(module); + spark_dsg::python::dynamic_scene_graph::addBindings(module); + spark_dsg::python::edge_attributes::addBindings(module); + spark_dsg::python::mesh::addBindings(module); + spark_dsg::python::misc_types::addBindings(module); + spark_dsg::python::node_attributes::addBindings(module); + spark_dsg::python::scene_graph_edge::addBindings(module); + spark_dsg::python::scene_graph_layer::addBindings(module); + spark_dsg::python::scene_graph_node::addBindings(module); + spark_dsg::python::scene_graph_utilities::addBindings(module); + spark_dsg::python::zmq_interface::addBindings(module); +} diff --git a/python/bindings/zmq_bindings.cpp b/python/bindings/src/zmq_interface.cpp similarity index 94% rename from python/bindings/zmq_bindings.cpp rename to python/bindings/src/zmq_interface.cpp index 68b01b6..f58342d 100644 --- a/python/bindings/zmq_bindings.cpp +++ b/python/bindings/src/zmq_interface.cpp @@ -32,18 +32,19 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include "zmq_bindings.h" +#include "spark_dsg/python/zmq_interface.h" +#include #include #include -using namespace spark_dsg; using namespace pybind11::literals; - namespace py = pybind11; +namespace spark_dsg::python::zmq_interface { + +void addBindings(pybind11::module_& module) { #if INCLUDE_ZMQ() -void add_zmq_bindings(pybind11::module_& module) { py::class_(module, "DsgSender") .def(py::init(), "url"_a, "num_threads"_a = 1) .def("send", &ZmqSender::send, "graph"_a, "include_mesh"_a = false); @@ -67,7 +68,7 @@ void add_zmq_bindings(pybind11::module_& module) { "has_change", [](const ZmqGraph& zmq_graph) { return zmq_graph.hasChange(); }) .def_property_readonly( "graph", [](const ZmqGraph& zmq_graph) { return zmq_graph.graph(); }); -} -#else -void add_zmq_bindings(pybind11::module_&) {} #endif +} + +} // namespace spark_dsg::python::zmq_interface From a02b69f037d577409f299835411a0e932c8f9117 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Mon, 1 Jul 2024 01:38:42 +0000 Subject: [PATCH 07/18] push edges towards derived layers --- include/spark_dsg/base_layer.h | 4 ++- include/spark_dsg/dynamic_scene_graph_layer.h | 1 + include/spark_dsg/edge_container.h | 11 ------- include/spark_dsg/scene_graph_layer.h | 1 + include/spark_dsg/scene_graph_types.h | 20 ++++++++----- include/spark_dsg/spark_dsg_fwd.h | 2 ++ src/edge_container.cpp | 18 ------------ src/scene_graph_types.cpp | 29 +++++++++++++++++++ 8 files changed, 48 insertions(+), 38 deletions(-) diff --git a/include/spark_dsg/base_layer.h b/include/spark_dsg/base_layer.h index 083423c..5237022 100644 --- a/include/spark_dsg/base_layer.h +++ b/include/spark_dsg/base_layer.h @@ -33,8 +33,10 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #pragma once -#include "spark_dsg/edge_container.h" +#include + #include "spark_dsg/scene_graph_node.h" +#include "spark_dsg/spark_dsg_fwd.h" namespace spark_dsg { diff --git a/include/spark_dsg/dynamic_scene_graph_layer.h b/include/spark_dsg/dynamic_scene_graph_layer.h index 3b7f6fc..6b877bb 100644 --- a/include/spark_dsg/dynamic_scene_graph_layer.h +++ b/include/spark_dsg/dynamic_scene_graph_layer.h @@ -36,6 +36,7 @@ #include #include "spark_dsg/base_layer.h" +#include "spark_dsg/edge_container.h" namespace spark_dsg { diff --git a/include/spark_dsg/edge_container.h b/include/spark_dsg/edge_container.h index 5d1a403..1d4ef1f 100644 --- a/include/spark_dsg/edge_container.h +++ b/include/spark_dsg/edge_container.h @@ -76,17 +76,6 @@ struct SceneGraphEdge { } }; -struct EdgeKey { - EdgeKey(NodeId k1, NodeId k2); - bool operator==(const EdgeKey& other) const; - bool operator<(const EdgeKey& other) const; - - NodeId k1; - NodeId k2; -}; - -std::ostream& operator<<(std::ostream& out, const EdgeKey& key); - struct EdgeContainer { using Edge = SceneGraphEdge; using Edges = std::map; diff --git a/include/spark_dsg/scene_graph_layer.h b/include/spark_dsg/scene_graph_layer.h index fc2e9c3..8a57cf3 100644 --- a/include/spark_dsg/scene_graph_layer.h +++ b/include/spark_dsg/scene_graph_layer.h @@ -39,6 +39,7 @@ #include #include "spark_dsg/base_layer.h" +#include "spark_dsg/edge_container.h" namespace spark_dsg { diff --git a/include/spark_dsg/scene_graph_types.h b/include/spark_dsg/scene_graph_types.h index 051ba5f..097f7dc 100644 --- a/include/spark_dsg/scene_graph_types.h +++ b/include/spark_dsg/scene_graph_types.h @@ -46,6 +46,17 @@ namespace spark_dsg { using NodeId = uint64_t; //!< Node label using LayerId = uint64_t; //!< Layer label +struct EdgeKey { + EdgeKey(NodeId k1, NodeId k2); + bool operator==(const EdgeKey& other) const; + bool operator<(const EdgeKey& other) const; + + NodeId k1; + NodeId k2; +}; + +std::ostream& operator<<(std::ostream& out, const EdgeKey& key); + struct LayerKey { static constexpr LayerId UNKNOWN_LAYER = std::numeric_limits::max(); LayerId layer; @@ -69,14 +80,7 @@ struct LayerKey { inline operator bool() const { return layer != UNKNOWN_LAYER; } }; -inline std::ostream& operator<<(std::ostream& out, const LayerKey& key) { - if (key.dynamic) { - out << key.layer << "(" << key.prefix << ")"; - } else { - out << key.layer; - } - return out; -} +std::ostream& operator<<(std::ostream& out, const LayerKey& key); class LayerPrefix { public: diff --git a/include/spark_dsg/spark_dsg_fwd.h b/include/spark_dsg/spark_dsg_fwd.h index b289862..581dd6c 100644 --- a/include/spark_dsg/spark_dsg_fwd.h +++ b/include/spark_dsg/spark_dsg_fwd.h @@ -5,6 +5,8 @@ namespace spark_dsg { class Mesh; struct NodeAttributes; struct EdgeAttributes; +struct SceneGraphEdge; +struct EdgeContainer; class DynamicSceneGraph; using DynamicSceneGraphPtr = std::shared_ptr; diff --git a/src/edge_container.cpp b/src/edge_container.cpp index ff0b0f7..bb312bd 100644 --- a/src/edge_container.cpp +++ b/src/edge_container.cpp @@ -48,24 +48,6 @@ SceneGraphEdge::SceneGraphEdge(NodeId source, SceneGraphEdge::~SceneGraphEdge() = default; -EdgeKey::EdgeKey(NodeId k1, NodeId k2) : k1(std::min(k1, k2)), k2(std::max(k1, k2)) {} - -bool EdgeKey::operator==(const EdgeKey& other) const { - return k1 == other.k1 && k2 == other.k2; -} - -bool EdgeKey::operator<(const EdgeKey& other) const { - if (k1 == other.k1) { - return k2 < other.k2; - } - - return k1 < other.k1; -} - -std::ostream& operator<<(std::ostream& out, const EdgeKey& key) { - return out << NodeSymbol(key.k1) << " -> " << NodeSymbol(key.k2); -} - void EdgeContainer::insert(NodeId source, NodeId target, std::unique_ptr&& edge_info) { diff --git a/src/scene_graph_types.cpp b/src/scene_graph_types.cpp index e525680..f8d2759 100644 --- a/src/scene_graph_types.cpp +++ b/src/scene_graph_types.cpp @@ -37,8 +37,28 @@ #include #include +#include "spark_dsg/node_symbol.h" + namespace spark_dsg { +EdgeKey::EdgeKey(NodeId k1, NodeId k2) : k1(std::min(k1, k2)), k2(std::max(k1, k2)) {} + +bool EdgeKey::operator==(const EdgeKey& other) const { + return k1 == other.k1 && k2 == other.k2; +} + +bool EdgeKey::operator<(const EdgeKey& other) const { + if (k1 == other.k1) { + return k2 < other.k2; + } + + return k1 < other.k1; +} + +std::ostream& operator<<(std::ostream& out, const EdgeKey& key) { + return out << NodeSymbol(key.k1) << " -> " << NodeSymbol(key.k2); +} + LayerPrefix::LayerPrefix(char key) { value_.symbol.key = key; value_.symbol.index = 0; @@ -105,6 +125,15 @@ bool LayerKey::operator==(const LayerKey& other) const { bool LayerKey::isParent(const LayerKey& other) const { return layer > other.layer; } +std::ostream& operator<<(std::ostream& out, const LayerKey& key) { + if (key.dynamic) { + out << key.layer << "(" << key.prefix << ")"; + } else { + out << key.layer; + } + return out; +} + std::string DsgLayers::LayerIdToString(LayerId id) { switch (id) { case SEGMENTS: From 90fc97b03d0b7e8acec74254a646bcf296817963 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Mon, 1 Jul 2024 13:33:56 +0000 Subject: [PATCH 08/18] (wip) move traits and status --- include/spark_dsg/base_layer.h | 7 +++ include/spark_dsg/dynamic_scene_graph.h | 2 +- include/spark_dsg/dynamic_scene_graph_layer.h | 4 +- include/spark_dsg/scene_graph_layer.h | 52 +++++-------------- include/spark_dsg/scene_graph_node.h | 7 --- src/scene_graph_layer.cpp | 42 ++++++++++++++- 6 files changed, 66 insertions(+), 48 deletions(-) diff --git a/include/spark_dsg/base_layer.h b/include/spark_dsg/base_layer.h index 5237022..cfbf13b 100644 --- a/include/spark_dsg/base_layer.h +++ b/include/spark_dsg/base_layer.h @@ -51,6 +51,13 @@ struct GraphMergeConfig { 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; diff --git a/include/spark_dsg/dynamic_scene_graph.h b/include/spark_dsg/dynamic_scene_graph.h index 7ad81a1..8d2d737 100644 --- a/include/spark_dsg/dynamic_scene_graph.h +++ b/include/spark_dsg/dynamic_scene_graph.h @@ -149,7 +149,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&& node); /** * @brief add a node to the graph or update an existing node diff --git a/include/spark_dsg/dynamic_scene_graph_layer.h b/include/spark_dsg/dynamic_scene_graph_layer.h index 6b877bb..e64ae94 100644 --- a/include/spark_dsg/dynamic_scene_graph_layer.h +++ b/include/spark_dsg/dynamic_scene_graph_layer.h @@ -34,6 +34,8 @@ * -------------------------------------------------------------------------- */ #pragma once #include +#include +#include #include "spark_dsg/base_layer.h" #include "spark_dsg/edge_container.h" @@ -45,7 +47,7 @@ class DynamicSceneGraphLayer : public BaseLayer { //! desired pointer type for the layer using Ptr = std::unique_ptr; //! node container for the layer - using Nodes = std::vector; + using Nodes = std::vector>; //! edge container type for the layer using Edges = EdgeContainer::Edges; diff --git a/include/spark_dsg/scene_graph_layer.h b/include/spark_dsg/scene_graph_layer.h index 8a57cf3..9c22f8c 100644 --- a/include/spark_dsg/scene_graph_layer.h +++ b/include/spark_dsg/scene_graph_layer.h @@ -35,6 +35,7 @@ #pragma once #include #include +#include #include #include @@ -57,7 +58,7 @@ class SceneGraphLayer : public BaseLayer { //! desired pointer type for the layer using Ptr = std::unique_ptr; //! node container for the layer - using Nodes = std::map; + using Nodes = std::map>; //! type tracking the status of nodes using NodeCheckup = std::map; //! edge container type for the layer @@ -258,7 +259,7 @@ class SceneGraphLayer : public BaseLayer { * @param node to add * @returns true if the node was added successfully */ - bool insertNode(SceneGraphNode::Ptr&& node); + bool insertNode(std::unique_ptr&& node); /** * @brief merge a node into the other if both nodes exist @@ -311,12 +312,9 @@ class IsolatedSceneGraphLayer : public SceneGraphLayer { virtual SceneGraphLayer::Ptr clone(const NodeChecker& is_valid = {}) const override; using SceneGraphLayer::emplaceNode; - using SceneGraphLayer::insertNode; - - using SceneGraphLayer::removeNode; - using SceneGraphLayer::mergeNodes; + using SceneGraphLayer::removeNode; }; namespace graph_utilities { @@ -327,38 +325,16 @@ struct graph_traits { using node_valid_func = const std::function&; using edge_valid_func = const std::function&; - static inline std::set neighbors(const SceneGraphLayer& graph, NodeId node) { - return get_node(graph, node).siblings(); - } - - static inline bool contains(const SceneGraphLayer& graph, NodeId node) { - return graph.hasNode(node); - } - - static inline const SceneGraphLayer::Nodes& nodes(const SceneGraphLayer& graph) { - return graph.nodes(); - } - - static inline const SceneGraphNode& unwrap_node( - const SceneGraphLayer::Nodes::value_type& container) { - return *container.second; - } - - static inline NodeId unwrap_node_id( - const SceneGraphLayer::Nodes::value_type& container) { - return container.first; - } - - static inline const SceneGraphNode& get_node(const SceneGraphLayer& graph, - NodeId node_id) { - return graph.getNode(node_id); - } - - static inline const SceneGraphEdge& get_edge(const SceneGraphLayer& graph, - NodeId source, - NodeId target) { - return graph.getEdge(source, target); - } + static std::set neighbors(const SceneGraphLayer& graph, NodeId node); + static bool contains(const SceneGraphLayer& graph, NodeId node); + static const SceneGraphLayer::Nodes& nodes(const SceneGraphLayer& graph); + static const SceneGraphNode& unwrap_node( + const SceneGraphLayer::Nodes::value_type& container); + static NodeId unwrap_node_id(const SceneGraphLayer::Nodes::value_type& container); + static const SceneGraphNode& get_node(const SceneGraphLayer& graph, NodeId node_id); + static const SceneGraphEdge& get_edge(const SceneGraphLayer& graph, + NodeId source, + NodeId target); }; } // namespace graph_utilities diff --git a/include/spark_dsg/scene_graph_node.h b/include/spark_dsg/scene_graph_node.h index db55146..15094a7 100644 --- a/include/spark_dsg/scene_graph_node.h +++ b/include/spark_dsg/scene_graph_node.h @@ -45,13 +45,6 @@ namespace spark_dsg { -/** - * @brief Base node status. - * - * Mostly for keeping history and status of nodes in a graph - */ -enum class NodeStatus { NEW, VISIBLE, MERGED, DELETED, NONEXISTENT }; - /** * @brief Node in the scene graph * diff --git a/src/scene_graph_layer.cpp b/src/scene_graph_layer.cpp index 13ef665..c210e12 100644 --- a/src/scene_graph_layer.cpp +++ b/src/scene_graph_layer.cpp @@ -57,7 +57,7 @@ bool SceneGraphLayer::emplaceNode(NodeId node_id, .second; } -bool SceneGraphLayer::insertNode(SceneGraphNode::Ptr&& node) { +bool SceneGraphLayer::insertNode(std::unique_ptr&& node) { if (!node) { SG_LOG(ERROR) << "Attempted to add an unitialized node to layer " << id << std::endl; @@ -387,4 +387,44 @@ SceneGraphLayer::Ptr IsolatedSceneGraphLayer::clone(const NodeChecker& is_valid) return new_layer; } +namespace graph_utilities { + +using LayerGraphTraits = graph_traits; + +std::set LayerGraphTraits::neighbors(const SceneGraphLayer& graph, + NodeId node) { + return get_node(graph, node).siblings(); +} + +bool LayerGraphTraits::contains(const SceneGraphLayer& graph, NodeId node) { + return graph.hasNode(node); +} + +const SceneGraphLayer::Nodes& LayerGraphTraits::nodes(const SceneGraphLayer& graph) { + return graph.nodes(); +} + +const SceneGraphNode& LayerGraphTraits::unwrap_node( + const SceneGraphLayer::Nodes::value_type& container) { + return *container.second; +} + +NodeId LayerGraphTraits::unwrap_node_id( + const SceneGraphLayer::Nodes::value_type& container) { + return container.first; +} + +const SceneGraphNode& LayerGraphTraits::get_node(const SceneGraphLayer& graph, + NodeId node_id) { + return graph.getNode(node_id); +} + +const SceneGraphEdge& LayerGraphTraits::get_edge(const SceneGraphLayer& graph, + NodeId source, + NodeId target) { + return graph.getEdge(source, target); +} + +} // namespace graph_utilities + } // namespace spark_dsg From 44a4169d5b2e0404a25b2326d9ffda5b1dd79c08 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Mon, 1 Jul 2024 15:07:44 +0000 Subject: [PATCH 09/18] split layer prefix from types --- CMakeLists.txt | 1 + include/spark_dsg/adjacency_matrix.h | 3 +- include/spark_dsg/bounding_box.h | 5 +- include/spark_dsg/dynamic_scene_graph_layer.h | 5 +- include/spark_dsg/layer_prefix.h | 106 ++++++++++++++++ include/spark_dsg/printing.h | 2 + include/spark_dsg/scene_graph_layer.h | 4 +- include/spark_dsg/scene_graph_types.h | 65 +--------- include/spark_dsg/scene_graph_utilities.h | 2 +- include/spark_dsg/spark_dsg_fwd.h | 4 + src/adjacency_matrix.cpp | 2 +- src/base_layer.cpp | 3 +- src/bounding_box.cpp | 8 ++ src/dynamic_scene_graph.cpp | 14 ++- src/dynamic_scene_graph_layer.cpp | 11 +- src/layer_prefix.cpp | 117 ++++++++++++++++++ src/printing.cpp | 4 + src/scene_graph_layer.cpp | 11 +- src/scene_graph_types.cpp | 82 ------------ tests/CMakeLists.txt | 1 + tests/utest_adjacency_matrix.cpp | 3 +- tests/utest_dynamic_scene_graph_layer.cpp | 13 +- tests/utest_layer_prefix.cpp | 104 ++++++++++++++++ tests/utest_scene_graph_layer.cpp | 11 +- tests/utest_scene_graph_types.cpp | 64 ---------- 25 files changed, 394 insertions(+), 251 deletions(-) create mode 100644 include/spark_dsg/layer_prefix.h create mode 100644 src/layer_prefix.cpp create mode 100644 tests/utest_layer_prefix.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 500a8c3..67a5209 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,6 +38,7 @@ add_library( 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 diff --git a/include/spark_dsg/adjacency_matrix.h b/include/spark_dsg/adjacency_matrix.h index 2c83a2d..2b7196b 100644 --- a/include/spark_dsg/adjacency_matrix.h +++ b/include/spark_dsg/adjacency_matrix.h @@ -36,7 +36,8 @@ #include #include -#include "spark_dsg/scene_graph_layer.h" +#include "spark_dsg/scene_graph_types.h" +#include "spark_dsg/spark_dsg_fwd.h" namespace spark_dsg { diff --git a/include/spark_dsg/bounding_box.h b/include/spark_dsg/bounding_box.h index 657109d..f883278 100644 --- a/include/spark_dsg/bounding_box.h +++ b/include/spark_dsg/bounding_box.h @@ -250,8 +250,7 @@ struct BoundingBox { public: // Specialized point adaptors. struct MeshAdaptor : PointAdaptor { - MeshAdaptor(const Mesh& mesh, const std::vector* indices = nullptr) - : mesh(mesh), indices(indices) {} + MeshAdaptor(const Mesh& mesh, const std::vector* indices = nullptr); size_t size() const override; Eigen::Vector3f get(size_t index) const override; const Mesh& mesh; @@ -259,7 +258,7 @@ struct BoundingBox { }; struct PointVectorAdaptor : PointAdaptor { - PointVectorAdaptor(const std::vector& points) : points(points) {} + PointVectorAdaptor(const std::vector& points); size_t size() const override; Eigen::Vector3f get(size_t index) const override; const std::vector& points; diff --git a/include/spark_dsg/dynamic_scene_graph_layer.h b/include/spark_dsg/dynamic_scene_graph_layer.h index e64ae94..8b791b3 100644 --- a/include/spark_dsg/dynamic_scene_graph_layer.h +++ b/include/spark_dsg/dynamic_scene_graph_layer.h @@ -39,6 +39,7 @@ #include "spark_dsg/base_layer.h" #include "spark_dsg/edge_container.h" +#include "spark_dsg/layer_prefix.h" namespace spark_dsg { @@ -103,9 +104,9 @@ class DynamicSceneGraphLayer : public BaseLayer { const LayerPrefix prefix; - bool mergeLayer(const DynamicSceneGraphLayer& other, + void mergeLayer(const DynamicSceneGraphLayer& other, const GraphMergeConfig& config, - std::map* layer_lookup = nullptr); + std::vector* new_nodes = nullptr); void getNewNodes(std::vector& new_nodes, bool clear_new) override; diff --git a/include/spark_dsg/layer_prefix.h b/include/spark_dsg/layer_prefix.h new file mode 100644 index 0000000..aacf14b --- /dev/null +++ b/include/spark_dsg/layer_prefix.h @@ -0,0 +1,106 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#pragma once +#include + +#include "spark_dsg/scene_graph_types.h" + +/** + * @brief spark_dsg namespace + */ +namespace spark_dsg { + +struct LayerKey { + static constexpr LayerId UNKNOWN_LAYER = DsgLayers::UNKNOWN; + LayerId layer; + uint32_t prefix = 0; + bool dynamic = false; + + LayerKey(); + + LayerKey(LayerId layer_id); + + LayerKey(LayerId layer_id, uint32_t prefix); + + bool isParent(const LayerKey& other) const; + + bool operator==(const LayerKey& other) const; + + inline bool operator!=(const LayerKey& other) const { + return !this->operator==(other); + } + + inline operator bool() const { return layer != UNKNOWN_LAYER; } +}; + +std::ostream& operator<<(std::ostream& out, const LayerKey& key); + +class LayerPrefix { + public: + LayerPrefix(char key); + + LayerPrefix(char key, uint32_t index); + + LayerPrefix(uint32_t index); + + static LayerPrefix fromId(NodeId node); + + inline operator uint32_t() const { return value_.value; } + + std::string str(bool with_key = true) const; + + bool matches(NodeId node) const; + + NodeId makeId(size_t index) const; + + size_t index(NodeId node_id) const; + + inline char key() const { return value_.symbol.key; } + + inline uint32_t index() const { return value_.symbol.index; } + + private: + union { + uint32_t value; + struct __attribute__((packed)) { + uint32_t index : 24; + char key : 8; + } symbol; + } value_; +}; + +std::ostream& operator<<(std::ostream& out, const LayerKey& key); + +} // namespace spark_dsg diff --git a/include/spark_dsg/printing.h b/include/spark_dsg/printing.h index 451970b..9596400 100644 --- a/include/spark_dsg/printing.h +++ b/include/spark_dsg/printing.h @@ -39,6 +39,8 @@ namespace spark_dsg { +std::ostream& operator<<(std::ostream& out, const EdgeKey& key); + Eigen::IOFormat getDefaultVectorFormat(); template diff --git a/include/spark_dsg/scene_graph_layer.h b/include/spark_dsg/scene_graph_layer.h index 9c22f8c..6518c4d 100644 --- a/include/spark_dsg/scene_graph_layer.h +++ b/include/spark_dsg/scene_graph_layer.h @@ -157,9 +157,9 @@ class SceneGraphLayer : public BaseLayer { */ bool rewireEdge(NodeId source, NodeId target, NodeId new_source, NodeId new_target); - bool mergeLayer(const SceneGraphLayer& other, + void mergeLayer(const SceneGraphLayer& other, const GraphMergeConfig& config, - std::map* layer_lookup = nullptr); + std::vector* new_nodes); /** * @brief Number of nodes in the layer diff --git a/include/spark_dsg/scene_graph_types.h b/include/spark_dsg/scene_graph_types.h index 097f7dc..e1a6467 100644 --- a/include/spark_dsg/scene_graph_types.h +++ b/include/spark_dsg/scene_graph_types.h @@ -35,7 +35,6 @@ #pragma once #include #include -#include #include /** @@ -55,67 +54,6 @@ struct EdgeKey { NodeId k2; }; -std::ostream& operator<<(std::ostream& out, const EdgeKey& key); - -struct LayerKey { - static constexpr LayerId UNKNOWN_LAYER = std::numeric_limits::max(); - LayerId layer; - uint32_t prefix = 0; - bool dynamic = false; - - LayerKey(); - - LayerKey(LayerId layer_id); - - LayerKey(LayerId layer_id, uint32_t prefix); - - bool isParent(const LayerKey& other) const; - - bool operator==(const LayerKey& other) const; - - inline bool operator!=(const LayerKey& other) const { - return !this->operator==(other); - } - - inline operator bool() const { return layer != UNKNOWN_LAYER; } -}; - -std::ostream& operator<<(std::ostream& out, const LayerKey& key); - -class LayerPrefix { - public: - LayerPrefix(char key); - - LayerPrefix(char key, uint32_t index); - - LayerPrefix(uint32_t index); - - static LayerPrefix fromId(NodeId node); - - inline operator uint32_t() const { return value_.value; } - - std::string str(bool with_key = true) const; - - bool matches(NodeId node) const; - - NodeId makeId(size_t index) const; - - size_t index(NodeId node_id) const; - - inline char key() const { return value_.symbol.key; } - - inline uint32_t index() const { return value_.symbol.index; } - - private: - union { - uint32_t value; - struct __attribute__((packed)) { - uint32_t index : 24; - char key : 8; - } symbol; - } value_; -}; - /** * @brief Layer enum hierarchy corresponding to original DSG paper(s) * @note A higher layer id corresponds to parents for interlayer edges @@ -129,7 +67,8 @@ struct DsgLayers { inline const static LayerId ROOMS = 4; //< Room node layer inline const static LayerId BUILDINGS = 5; //< Building node layer inline const static LayerId MESH_PLACES = 20; //< Mesh (2D) Places node layer - inline const static LayerId UNKNOWN = LayerKey::UNKNOWN_LAYER; //< Catchall layer ID + inline const static LayerId UNKNOWN = + std::numeric_limits::max(); //< Catchall layer ID static std::string LayerIdToString(LayerId id); static LayerId StringToLayerId(const std::string& id_str); diff --git a/include/spark_dsg/scene_graph_utilities.h b/include/spark_dsg/scene_graph_utilities.h index 7394022..d65ccc9 100644 --- a/include/spark_dsg/scene_graph_utilities.h +++ b/include/spark_dsg/scene_graph_utilities.h @@ -34,7 +34,7 @@ * -------------------------------------------------------------------------- */ #pragma once #include "spark_dsg/bounding_box.h" -#include "spark_dsg/scene_graph_types.h" +#include "spark_dsg/layer_prefix.h" #include "spark_dsg/spark_dsg_fwd.h" namespace spark_dsg { diff --git a/include/spark_dsg/spark_dsg_fwd.h b/include/spark_dsg/spark_dsg_fwd.h index 581dd6c..ac19c2a 100644 --- a/include/spark_dsg/spark_dsg_fwd.h +++ b/include/spark_dsg/spark_dsg_fwd.h @@ -2,12 +2,16 @@ #include namespace spark_dsg { + class Mesh; + struct NodeAttributes; + struct EdgeAttributes; struct SceneGraphEdge; struct EdgeContainer; +class SceneGraphLayer; class DynamicSceneGraph; using DynamicSceneGraphPtr = std::shared_ptr; diff --git a/src/adjacency_matrix.cpp b/src/adjacency_matrix.cpp index 0005aee..1abb5e3 100644 --- a/src/adjacency_matrix.cpp +++ b/src/adjacency_matrix.cpp @@ -34,7 +34,7 @@ * -------------------------------------------------------------------------- */ #include "spark_dsg/adjacency_matrix.h" -#include +#include "spark_dsg/scene_graph_layer.h" namespace spark_dsg { diff --git a/src/base_layer.cpp b/src/base_layer.cpp index 39b9b7f..637df90 100644 --- a/src/base_layer.cpp +++ b/src/base_layer.cpp @@ -33,10 +33,11 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include "spark_dsg/base_layer.h" -#include "spark_dsg/node_symbol.h" #include +#include "spark_dsg/printing.h" + namespace spark_dsg { NodeId GraphMergeConfig::getMergedId(NodeId original) const { diff --git a/src/bounding_box.cpp b/src/bounding_box.cpp index c682baa..c5d4170 100644 --- a/src/bounding_box.cpp +++ b/src/bounding_box.cpp @@ -277,6 +277,10 @@ bool BoundingBox::isInside(const Eigen::Vector3f& point_B) const { return (point_B.cwiseAbs() - dimensions / 2).maxCoeff() <= 0; } +BoundingBox::MeshAdaptor::MeshAdaptor(const Mesh& mesh, + const std::vector* indices) + : mesh(mesh), indices(indices) {} + size_t BoundingBox::MeshAdaptor::size() const { return indices ? indices->size() : mesh.numVertices(); } @@ -286,6 +290,10 @@ Eigen::Vector3f BoundingBox::MeshAdaptor::get(size_t index) const { return mesh.pos(global_idx); } +BoundingBox::PointVectorAdaptor::PointVectorAdaptor( + const std::vector& points) + : points(points) {} + size_t BoundingBox::PointVectorAdaptor::size() const { return points.size(); } Eigen::Vector3f BoundingBox::PointVectorAdaptor::get(size_t index) const { diff --git a/src/dynamic_scene_graph.cpp b/src/dynamic_scene_graph.cpp index de4b0b2..b78960f 100644 --- a/src/dynamic_scene_graph.cpp +++ b/src/dynamic_scene_graph.cpp @@ -40,6 +40,7 @@ #include "spark_dsg/logging.h" #include "spark_dsg/node_attributes.h" #include "spark_dsg/node_symbol.h" +#include "spark_dsg/printing.h" #include "spark_dsg/serialization/file_io.h" namespace spark_dsg { @@ -669,7 +670,12 @@ bool DynamicSceneGraph::mergeGraph(const DynamicSceneGraph& other, createDynamicLayer(l_id, prefix); } - dynamic_layers_[l_id][prefix]->mergeLayer(*other_layer, config, &node_lookup_); + const LayerKey layer_key(l_id, prefix); + std::vector new_nodes; + dynamic_layers_[l_id][prefix]->mergeLayer(*other_layer, config, &new_nodes); + for (const auto node_id : new_nodes) { + node_lookup_[node_id] = layer_key; + } } } @@ -690,7 +696,11 @@ bool DynamicSceneGraph::mergeGraph(const DynamicSceneGraph& other, layers_[l_id]->removeEdge(removed_edge.k1, removed_edge.k2); } - layers_[l_id]->mergeLayer(*other_layer, config, &node_lookup_); + std::vector new_nodes; + layers_[l_id]->mergeLayer(*other_layer, config, &new_nodes); + for (const auto node_id : new_nodes) { + node_lookup_[node_id] = l_id; + } } for (const auto& id_edge_pair : other.interlayer_edges()) { diff --git a/src/dynamic_scene_graph_layer.cpp b/src/dynamic_scene_graph_layer.cpp index 61cc25f..ce309bc 100644 --- a/src/dynamic_scene_graph_layer.cpp +++ b/src/dynamic_scene_graph_layer.cpp @@ -47,10 +47,9 @@ using Edge = SceneGraphEdge; DynamicSceneGraphLayer::DynamicSceneGraphLayer(LayerId layer, LayerPrefix node_prefix) : id(layer), prefix(node_prefix), next_node_(0) {} -bool DynamicSceneGraphLayer::mergeLayer(const DynamicSceneGraphLayer& other, +void DynamicSceneGraphLayer::mergeLayer(const DynamicSceneGraphLayer& other, const GraphMergeConfig& config, - std::map* layer_lookup) { - LayerKey layer_key{id, prefix}; + std::vector* new_nodes) { Eigen::Vector3d last_update_delta = Eigen::Vector3d::Zero(); for (size_t i = 0; i < other.nodes_.size(); i++) { @@ -70,8 +69,8 @@ bool DynamicSceneGraphLayer::mergeLayer(const DynamicSceneGraphLayer& other, } else { emplaceNode(other_node.timestamp.value(), other_node.attributes_->clone(), false); nodes_.back()->attributes_->position += last_update_delta; - if (layer_lookup) { - layer_lookup->insert({nodes_.back()->id, layer_key}); + if (new_nodes) { + new_nodes->push_back(nodes_.back()->id); } } } @@ -85,8 +84,6 @@ bool DynamicSceneGraphLayer::mergeLayer(const DynamicSceneGraphLayer& other, insertEdge(edge.source, edge.target, edge.info->clone()); } - - return true; } bool DynamicSceneGraphLayer::emplaceNode(std::chrono::nanoseconds stamp, diff --git a/src/layer_prefix.cpp b/src/layer_prefix.cpp new file mode 100644 index 0000000..e5357f4 --- /dev/null +++ b/src/layer_prefix.cpp @@ -0,0 +1,117 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "spark_dsg/layer_prefix.h" + +#include +#include + +namespace spark_dsg { + +LayerKey::LayerKey() : layer(LayerKey::UNKNOWN_LAYER) {} + +LayerKey::LayerKey(LayerId layer_id) : layer(layer_id) {} + +LayerKey::LayerKey(LayerId layer_id, uint32_t prefix) + : layer(layer_id), prefix(prefix), dynamic(true) {} + +bool LayerKey::operator==(const LayerKey& other) const { + if (dynamic != other.dynamic) { + return false; + } + + const bool same_layer = layer == other.layer; + if (!dynamic && same_layer) { + return true; + } + + return same_layer && prefix == other.prefix; +} + +bool LayerKey::isParent(const LayerKey& other) const { return layer > other.layer; } + +std::ostream& operator<<(std::ostream& out, const LayerKey& key) { + if (key.dynamic) { + out << key.layer << "(" << key.prefix << ")"; + } else { + out << key.layer; + } + return out; +} + +LayerPrefix::LayerPrefix(char key) { + value_.symbol.key = key; + value_.symbol.index = 0; +} + +LayerPrefix::LayerPrefix(char key, uint32_t index) { + value_.symbol.key = key; + value_.symbol.index = index; +} + +LayerPrefix::LayerPrefix(uint32_t index) { value_.value = index; } + +LayerPrefix LayerPrefix::fromId(NodeId node_id) { + // grab the 32 msb portion of the ID + return LayerPrefix(static_cast(node_id >> 32)); +} + +std::string LayerPrefix::str(bool with_key) const { + if (!with_key) { + return std::to_string(value_.value); + } + + std::stringstream ss; + ss << value_.symbol.key; + if (value_.symbol.index) { + ss << "(" << value_.symbol.index << ")"; + } + + return ss.str(); +} + +bool LayerPrefix::matches(NodeId node) const { + return value_.value == static_cast(node >> 32); +} + +NodeId LayerPrefix::makeId(size_t index) const { + return (static_cast(value_.value) << 32) + index; +} + +size_t LayerPrefix::index(NodeId node_id) const { + // grab the 32 lsb portion of the ID + return 0xFFFF'FFFF & node_id; +} + +} // namespace spark_dsg diff --git a/src/printing.cpp b/src/printing.cpp index 5c3784c..b774c29 100644 --- a/src/printing.cpp +++ b/src/printing.cpp @@ -36,6 +36,10 @@ namespace spark_dsg { +std::ostream& operator<<(std::ostream& out, const EdgeKey& key) { + return out << NodeSymbol(key.k1) << " -> " << NodeSymbol(key.k2); +} + Eigen::IOFormat getDefaultVectorFormat() { return Eigen::IOFormat( Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n", "[", "]"); diff --git a/src/scene_graph_layer.cpp b/src/scene_graph_layer.cpp index c210e12..87c0a69 100644 --- a/src/scene_graph_layer.cpp +++ b/src/scene_graph_layer.cpp @@ -214,9 +214,9 @@ bool SceneGraphLayer::rewireEdge(NodeId source, return true; } -bool SceneGraphLayer::mergeLayer(const SceneGraphLayer& other_layer, +void SceneGraphLayer::mergeLayer(const SceneGraphLayer& other_layer, const GraphMergeConfig& config, - std::map* layer_lookup) { + std::vector* new_nodes) { const bool update_attributes = (config.update_layer_attributes && config.update_layer_attributes->count(id)) ? config.update_layer_attributes->at(id) @@ -246,9 +246,8 @@ bool SceneGraphLayer::mergeLayer(const SceneGraphLayer& other_layer, auto attrs = other.attributes_->clone(); nodes_[other.id] = Node::Ptr(new Node(other.id, id, std::move(attrs))); nodes_status_[other.id] = NodeStatus::NEW; - - if (layer_lookup) { - layer_lookup->insert({other.id, id}); + if (new_nodes) { + new_nodes->push_back(other.id); } } @@ -267,8 +266,6 @@ bool SceneGraphLayer::mergeLayer(const SceneGraphLayer& other_layer, insertEdge(new_source, new_target, edge.info->clone()); } - - return true; } Eigen::Vector3d SceneGraphLayer::getPosition(NodeId node) const { diff --git a/src/scene_graph_types.cpp b/src/scene_graph_types.cpp index f8d2759..76124c8 100644 --- a/src/scene_graph_types.cpp +++ b/src/scene_graph_types.cpp @@ -35,9 +35,6 @@ #include "spark_dsg/scene_graph_types.h" #include -#include - -#include "spark_dsg/node_symbol.h" namespace spark_dsg { @@ -55,85 +52,6 @@ bool EdgeKey::operator<(const EdgeKey& other) const { return k1 < other.k1; } -std::ostream& operator<<(std::ostream& out, const EdgeKey& key) { - return out << NodeSymbol(key.k1) << " -> " << NodeSymbol(key.k2); -} - -LayerPrefix::LayerPrefix(char key) { - value_.symbol.key = key; - value_.symbol.index = 0; -} - -LayerPrefix::LayerPrefix(char key, uint32_t index) { - value_.symbol.key = key; - value_.symbol.index = index; -} - -LayerPrefix::LayerPrefix(uint32_t index) { value_.value = index; } - -LayerPrefix LayerPrefix::fromId(NodeId node_id) { - // grab the 32 msb portion of the ID - return LayerPrefix(static_cast(node_id >> 32)); -} - -std::string LayerPrefix::str(bool with_key) const { - if (!with_key) { - return std::to_string(value_.value); - } - - std::stringstream ss; - ss << value_.symbol.key; - if (value_.symbol.index) { - ss << "(" << value_.symbol.index << ")"; - } - - return ss.str(); -} - -bool LayerPrefix::matches(NodeId node) const { - return value_.value == static_cast(node >> 32); -} - -NodeId LayerPrefix::makeId(size_t index) const { - return (static_cast(value_.value) << 32) + index; -} - -size_t LayerPrefix::index(NodeId node_id) const { - // grab the 32 lsb portion of the ID - return 0xFFFF'FFFF & node_id; -} - -LayerKey::LayerKey() : layer(LayerKey::UNKNOWN_LAYER) {} - -LayerKey::LayerKey(LayerId layer_id) : layer(layer_id) {} - -LayerKey::LayerKey(LayerId layer_id, uint32_t prefix) - : layer(layer_id), prefix(prefix), dynamic(true) {} - -bool LayerKey::operator==(const LayerKey& other) const { - if (dynamic != other.dynamic) { - return false; - } - - const bool same_layer = layer == other.layer; - if (!dynamic && same_layer) { - return true; - } - - return same_layer && prefix == other.prefix; -} - -bool LayerKey::isParent(const LayerKey& other) const { return layer > other.layer; } - -std::ostream& operator<<(std::ostream& out, const LayerKey& key) { - if (key.dynamic) { - out << key.layer << "(" << key.prefix << ")"; - } else { - out << key.layer; - } - return out; -} - std::string DsgLayers::LayerIdToString(LayerId id) { switch (id) { case SEGMENTS: diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 47ab0a8..e498dd8 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -12,6 +12,7 @@ add_executable( utest_dynamic_scene_graph.cpp utest_edge_container.cpp utest_graph_utilities_layer.cpp + utest_layer_prefix.cpp utest_mesh.cpp utest_node_attributes.cpp utest_node_symbol.cpp diff --git a/tests/utest_adjacency_matrix.cpp b/tests/utest_adjacency_matrix.cpp index 2b74bca..ebd9542 100644 --- a/tests/utest_adjacency_matrix.cpp +++ b/tests/utest_adjacency_matrix.cpp @@ -34,8 +34,9 @@ * -------------------------------------------------------------------------- */ #include #include -#include #include +#include +#include #include "spark_dsg/logging.h" diff --git a/tests/utest_dynamic_scene_graph_layer.cpp b/tests/utest_dynamic_scene_graph_layer.cpp index 3556dd4..504ccf4 100644 --- a/tests/utest_dynamic_scene_graph_layer.cpp +++ b/tests/utest_dynamic_scene_graph_layer.cpp @@ -283,22 +283,19 @@ TEST(DynamicSceneGraphLayerTests, MergeLayerCorrect) { } GraphMergeConfig config; - std::map node_to_layer; - layer_1.mergeLayer(layer_2, config, &node_to_layer); - - EXPECT_EQ(2u, node_to_layer.size()); + std::vector new_nodes; + layer_1.mergeLayer(layer_2, config, &new_nodes); EXPECT_EQ(5u, layer_1.numNodes()); EXPECT_EQ(4u, layer_1.numEdges()); + std::vector expected_new_nodes{3, 4}; + EXPECT_EQ(new_nodes, expected_new_nodes); + for (size_t i = 0; i < 5; i++) { Eigen::Vector3d result = layer_1.getPosition(i); EXPECT_EQ(static_cast(i), result(0)); EXPECT_EQ(0.0, result(1)); EXPECT_EQ(0.0, result(2)); - if (i > 2) { - EXPECT_EQ(1u, node_to_layer.at(i).layer); - EXPECT_EQ(0, node_to_layer.at(i).prefix); - } } } diff --git a/tests/utest_layer_prefix.cpp b/tests/utest_layer_prefix.cpp new file mode 100644 index 0000000..09e2595 --- /dev/null +++ b/tests/utest_layer_prefix.cpp @@ -0,0 +1,104 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include +#include +#include + +namespace spark_dsg { + +TEST(LayerKeyTests, TestEquality) { + EXPECT_EQ(LayerKey(1), LayerKey(1)); + EXPECT_NE(LayerKey(1), LayerKey(2)); + EXPECT_NE(LayerKey(1), LayerKey(1, 0)); + EXPECT_EQ(LayerKey(2, 0), LayerKey(2, 0)); + EXPECT_NE(LayerKey(2, 0), LayerKey(2, 1)); +} + +TEST(LayerKeyTests, TestIsParent) { + LayerKey key1{1}; + LayerKey key2{1}; + LayerKey key3{2}; + LayerKey key4{2, 0}; + LayerKey key5{3, 0}; + LayerKey key6{2, 1}; + + // static + EXPECT_TRUE(key3.isParent(key1)); + EXPECT_FALSE(key1.isParent(key2)); + EXPECT_FALSE(key1.isParent(key3)); + + // dynamic + EXPECT_TRUE(key4.isParent(key1)); + EXPECT_TRUE(key5.isParent(key6)); + EXPECT_FALSE(key6.isParent(key4)); +} + +TEST(LayerKeyTests, TestKeyTruthValues) { + EXPECT_FALSE(LayerKey()); + EXPECT_TRUE(LayerKey(1)); + EXPECT_TRUE(LayerKey(2, 0)); + EXPECT_TRUE(LayerKey(0)); +} + +TEST(LayerPrefixTests, TestMatches) { + LayerPrefix a('a'); + EXPECT_TRUE(a.matches(NodeSymbol('a', 0))); + EXPECT_TRUE(a.matches(NodeSymbol('a', 5))); + EXPECT_FALSE(a.matches(NodeSymbol('b', 5))); +} + +TEST(LayerPrefixTests, TestFromId) { + EXPECT_EQ(LayerPrefix::fromId("a0"_id), LayerPrefix('a')); +} + +TEST(LayerPrefixTests, TestMakeId) { + LayerPrefix a('a'); + EXPECT_EQ(a.makeId(0), NodeSymbol('a', 0)); + EXPECT_EQ(a.makeId(5), NodeSymbol('a', 5)); + + LayerPrefix b('b'); + EXPECT_EQ(b.makeId(5), NodeSymbol('b', 5)); +} + +TEST(LayerPrefixTests, TestIndex) { + LayerPrefix a('a'); + EXPECT_EQ(a.index(NodeSymbol('a', 0)), 0u); + EXPECT_EQ(a.index(NodeSymbol('a', 5)), 5u); + + LayerPrefix b('b', 1); + EXPECT_EQ(b.index(b.makeId(5)), 5u); +} + +} // namespace spark_dsg diff --git a/tests/utest_scene_graph_layer.cpp b/tests/utest_scene_graph_layer.cpp index 4856022..858fcf5 100644 --- a/tests/utest_scene_graph_layer.cpp +++ b/tests/utest_scene_graph_layer.cpp @@ -353,22 +353,21 @@ TEST(SceneGraphLayerTests, MergeLayerCorrect) { EXPECT_TRUE(layer_2.insertEdge(i - 1, i)); } - std::map node_to_layer; - layer_1.mergeLayer(layer_2, {}, &node_to_layer); + std::vector new_nodes; + layer_1.mergeLayer(layer_2, {}, &new_nodes); - EXPECT_EQ(2u, node_to_layer.size()); EXPECT_EQ(5u, layer_1.numNodes()); EXPECT_EQ(4u, layer_1.numEdges()); + std::vector expected_new_nodes{3, 4}; + EXPECT_EQ(new_nodes, expected_new_nodes); + for (size_t i = 0; i < 5; i++) { Eigen::Vector3d result = layer_1.getPosition(i); 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); EXPECT_EQ(NodeStatus::NEW, layer_1.checkNode(i)); - if (i > 2) { - EXPECT_EQ(LayerKey(1), node_to_layer.at(i)); - } } } diff --git a/tests/utest_scene_graph_types.cpp b/tests/utest_scene_graph_types.cpp index f112fb0..eedb30b 100644 --- a/tests/utest_scene_graph_types.cpp +++ b/tests/utest_scene_graph_types.cpp @@ -33,74 +33,10 @@ * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ #include -#include #include namespace spark_dsg { -TEST(LayerKeyTests, TestEquality) { - EXPECT_EQ(LayerKey(1), LayerKey(1)); - EXPECT_NE(LayerKey(1), LayerKey(2)); - EXPECT_NE(LayerKey(1), LayerKey(1, 0)); - EXPECT_EQ(LayerKey(2, 0), LayerKey(2, 0)); - EXPECT_NE(LayerKey(2, 0), LayerKey(2, 1)); -} - -TEST(LayerKeyTests, TestIsParent) { - LayerKey key1{1}; - LayerKey key2{1}; - LayerKey key3{2}; - LayerKey key4{2, 0}; - LayerKey key5{3, 0}; - LayerKey key6{2, 1}; - - // static - EXPECT_TRUE(key3.isParent(key1)); - EXPECT_FALSE(key1.isParent(key2)); - EXPECT_FALSE(key1.isParent(key3)); - - // dynamic - EXPECT_TRUE(key4.isParent(key1)); - EXPECT_TRUE(key5.isParent(key6)); - EXPECT_FALSE(key6.isParent(key4)); -} - -TEST(LayerKeyTests, TestKeyTruthValues) { - EXPECT_FALSE(LayerKey()); - EXPECT_TRUE(LayerKey(1)); - EXPECT_TRUE(LayerKey(2, 0)); - EXPECT_TRUE(LayerKey(0)); -} - -TEST(LayerPrefixTests, TestMatches) { - LayerPrefix a('a'); - EXPECT_TRUE(a.matches(NodeSymbol('a', 0))); - EXPECT_TRUE(a.matches(NodeSymbol('a', 5))); - EXPECT_FALSE(a.matches(NodeSymbol('b', 5))); -} - -TEST(LayerPrefixTests, TestFromId) { - EXPECT_EQ(LayerPrefix::fromId("a0"_id), LayerPrefix('a')); -} - -TEST(LayerPrefixTests, TestMakeId) { - LayerPrefix a('a'); - EXPECT_EQ(a.makeId(0), NodeSymbol('a', 0)); - EXPECT_EQ(a.makeId(5), NodeSymbol('a', 5)); - - LayerPrefix b('b'); - EXPECT_EQ(b.makeId(5), NodeSymbol('b', 5)); -} - -TEST(LayerPrefixTests, TestIndex) { - LayerPrefix a('a'); - EXPECT_EQ(a.index(NodeSymbol('a', 0)), 0u); - EXPECT_EQ(a.index(NodeSymbol('a', 5)), 5u); - - LayerPrefix b('b', 1); - EXPECT_EQ(b.index(b.makeId(5)), 5u); -} - TEST(LayerHelperTests, TestToString) { EXPECT_EQ("OBJECTS", DsgLayers::LayerIdToString(DsgLayers::OBJECTS)); // layer 2 always maps to OBJECTS From c6fec8d7ba833f0e02bf4b44d4a8c142730be375 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Mon, 1 Jul 2024 15:18:59 +0000 Subject: [PATCH 10/18] minor cleanup of node --- include/spark_dsg/dynamic_scene_graph_layer.h | 2 -- include/spark_dsg/printing.h | 4 ++++ include/spark_dsg/scene_graph_layer.h | 2 -- include/spark_dsg/scene_graph_node.h | 16 ---------------- src/printing.cpp | 11 +++++++++++ src/scene_graph_node.cpp | 13 ------------- 6 files changed, 15 insertions(+), 33 deletions(-) diff --git a/include/spark_dsg/dynamic_scene_graph_layer.h b/include/spark_dsg/dynamic_scene_graph_layer.h index 8b791b3..bf7a939 100644 --- a/include/spark_dsg/dynamic_scene_graph_layer.h +++ b/include/spark_dsg/dynamic_scene_graph_layer.h @@ -34,8 +34,6 @@ * -------------------------------------------------------------------------- */ #pragma once #include -#include -#include #include "spark_dsg/base_layer.h" #include "spark_dsg/edge_container.h" diff --git a/include/spark_dsg/printing.h b/include/spark_dsg/printing.h index 9596400..39ddab2 100644 --- a/include/spark_dsg/printing.h +++ b/include/spark_dsg/printing.h @@ -39,8 +39,12 @@ namespace spark_dsg { +class SceneGraphNode; + std::ostream& operator<<(std::ostream& out, const EdgeKey& key); +std::ostream& operator<<(std::ostream& out, const SceneGraphNode& node); + Eigen::IOFormat getDefaultVectorFormat(); template diff --git a/include/spark_dsg/scene_graph_layer.h b/include/spark_dsg/scene_graph_layer.h index 6518c4d..3d4c26a 100644 --- a/include/spark_dsg/scene_graph_layer.h +++ b/include/spark_dsg/scene_graph_layer.h @@ -35,9 +35,7 @@ #pragma once #include #include -#include #include -#include #include "spark_dsg/base_layer.h" #include "spark_dsg/edge_container.h" diff --git a/include/spark_dsg/scene_graph_node.h b/include/spark_dsg/scene_graph_node.h index 15094a7..d80e52b 100644 --- a/include/spark_dsg/scene_graph_node.h +++ b/include/spark_dsg/scene_graph_node.h @@ -36,7 +36,6 @@ #include #include #include -#include #include #include @@ -166,21 +165,6 @@ class SceneGraphNode { //! Timestamp of node (if dynamic) const std::optional timestamp; - /** - * @brief output node information - * @param out output stream - * @param node node to print - * @returns original output stream - */ - friend std::ostream& operator<<(std::ostream& out, const SceneGraphNode& node); - - protected: - /** - * @brief internal function for outputing information to a ostream - * @param out ostream to output info to - */ - virtual std::ostream& fill_ostream(std::ostream& out) const; - protected: //! pointer to attributes std::unique_ptr attributes_; diff --git a/src/printing.cpp b/src/printing.cpp index b774c29..af48a62 100644 --- a/src/printing.cpp +++ b/src/printing.cpp @@ -34,12 +34,23 @@ * -------------------------------------------------------------------------- */ #include "spark_dsg/printing.h" +#include "spark_dsg/scene_graph_node.h" + namespace spark_dsg { std::ostream& operator<<(std::ostream& out, const EdgeKey& key) { return out << NodeSymbol(key.k1) << " -> " << NodeSymbol(key.k2); } +std::ostream& operator<<(std::ostream& out, const SceneGraphNode& node) { + out << "Node"; + return out; +} + Eigen::IOFormat getDefaultVectorFormat() { return Eigen::IOFormat( Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n", "[", "]"); diff --git a/src/scene_graph_node.cpp b/src/scene_graph_node.cpp index d0e4157..0068047 100644 --- a/src/scene_graph_node.cpp +++ b/src/scene_graph_node.cpp @@ -55,19 +55,6 @@ SceneGraphNode::SceneGraphNode(NodeId node_id, SceneGraphNode::~SceneGraphNode() = default; -std::ostream& SceneGraphNode::fill_ostream(std::ostream& out) const { - out << "Node"; - return out; -} - -std::ostream& operator<<(std::ostream& out, const SceneGraphNode& node) { - return node.fill_ostream(out); -} - bool SceneGraphNode::hasParent() const { return parents_.size() == 1; } bool SceneGraphNode::hasSiblings() const { return not siblings_.empty(); } From 65945812b0c9df3d2e3cc44f920fc8784f188955 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Mon, 1 Jul 2024 15:57:45 +0000 Subject: [PATCH 11/18] simplify eraseVertices --- include/spark_dsg/mesh.h | 6 +++ src/mesh.cpp | 88 ++++++++++++++++++++-------------------- 2 files changed, 50 insertions(+), 44 deletions(-) diff --git a/include/spark_dsg/mesh.h b/include/spark_dsg/mesh.h index a7ce32a..74a705b 100644 --- a/include/spark_dsg/mesh.h +++ b/include/spark_dsg/mesh.h @@ -94,6 +94,12 @@ class Mesh { */ size_t numFaces() const; + /** + * @brief Reserve vertices memory + * @param size New expected size of mesh vertices + */ + void reserveVertices(size_t size); + /** * @brief Set mesh vertex size * @param size New size of mesh vertices diff --git a/src/mesh.cpp b/src/mesh.cpp index d96b5e2..b8b12e8 100644 --- a/src/mesh.cpp +++ b/src/mesh.cpp @@ -90,6 +90,22 @@ size_t Mesh::numVertices() const { return points.size(); } size_t Mesh::numFaces() const { return faces.size(); } +void Mesh::reserveVertices(size_t size) { + points.reserve(size); + if (has_colors) { + colors.reserve(size); + } + if (has_timestamps) { + stamps.reserve(size); + } + if (has_labels) { + labels.reserve(size); + } + if (has_first_seen_stamps) { + first_seen_stamps.reserve(size); + } +} + void Mesh::resizeVertices(size_t size) { points.resize(size); if (has_colors) { @@ -141,77 +157,61 @@ const Mesh::Face& Mesh::face(size_t index) const { return faces.at(index); } Mesh::Face& Mesh::face(size_t index) { return faces.at(index); } void Mesh::eraseVertices(const std::unordered_set& indices) { - // Map old indices to new indices. - std::unordered_map old_to_new; - // Allocating new storage is faster than erasing from all old storages. - Positions new_points; - Colors new_colors; - Timestamps new_stamps; - Timestamps new_first_seen_stamps; - Labels new_labels; - - const size_t num_new_vertices = numVertices() - indices.size(); - new_points.reserve(num_new_vertices); - if (has_colors) { - new_colors.reserve(num_new_vertices); - } - if (has_timestamps) { - new_stamps.reserve(num_new_vertices); - } - if (has_labels) { - new_labels.reserve(num_new_vertices); - } - if (has_first_seen_stamps) { - new_first_seen_stamps.reserve(num_new_vertices); - } + const auto num_new_vertices = numVertices() - indices.size(); + Mesh other(has_colors, has_timestamps, has_labels, has_first_seen_stamps); + other.reserveVertices(num_new_vertices); // Copy over the vertices that are not being removed. size_t new_index = 0; + // Map old indices to new indices. + std::unordered_map old_to_new; for (size_t old_index = 0; old_index < numVertices(); ++old_index) { if (indices.count(old_index)) { continue; } + old_to_new[old_index] = new_index++; - new_points.push_back(points[old_index]); + other.points.push_back(points[old_index]); if (has_colors) { - new_colors.push_back(colors[old_index]); + other.colors.push_back(colors[old_index]); } + if (has_timestamps) { - new_stamps.push_back(stamps[old_index]); + other.stamps.push_back(stamps[old_index]); } + if (has_labels) { - new_labels.push_back(labels[old_index]); + other.labels.push_back(labels[old_index]); } + if (has_first_seen_stamps) { - new_first_seen_stamps.push_back(first_seen_stamps[old_index]); + other.first_seen_stamps.push_back(first_seen_stamps[old_index]); } } - points = std::move(new_points); - colors = std::move(new_colors); - stamps = std::move(new_stamps); - labels = std::move(new_labels); - first_seen_stamps = std::move(new_first_seen_stamps); - - // Update the faces. - auto face_it = faces.begin(); - while (face_it != faces.end()) { + // Update and copy over the faces. + for (const auto& face : faces) { + Face new_face; bool erase_face = false; - for (size_t& index : *face_it) { - const auto new_index = old_to_new.find(index); - if (new_index == old_to_new.end()) { + for (size_t i = 0; i < 3; ++i) { + const auto iter = old_to_new.find(face[i]); + if (iter == old_to_new.end()) { erase_face = true; break; } - index = new_index->second; + + new_face[i] = iter->second; } + if (erase_face) { - face_it = faces.erase(face_it); - } else { - ++face_it; + continue; } + + other.faces.push_back(new_face); } + + *this = std::move(other); } void Mesh::eraseFaces(const std::unordered_set& indices, From ffadcc86d9687b677783f9a6a421cd45b4f0af95 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Mon, 1 Jul 2024 15:57:56 +0000 Subject: [PATCH 12/18] drop getPosition and eigen dep --- include/spark_dsg/dynamic_scene_graph.h | 6 ++-- include/spark_dsg/dynamic_scene_graph_layer.h | 14 +++----- include/spark_dsg/scene_graph_layer.h | 7 +--- src/dynamic_scene_graph.cpp | 15 ++++---- src/dynamic_scene_graph_layer.cpp | 20 ----------- src/scene_graph_layer.cpp | 10 ------ tests/utest_dynamic_scene_graph_layer.cpp | 36 +------------------ tests/utest_scene_graph_layer.cpp | 23 +----------- 8 files changed, 15 insertions(+), 116 deletions(-) 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); From 93c1d6a2c1109169f0fdfa6bc8ef3844a6c0a8bf Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Mon, 1 Jul 2024 17:00:15 +0000 Subject: [PATCH 13/18] drop node symbol include --- src/scene_graph_node.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/scene_graph_node.cpp b/src/scene_graph_node.cpp index 0068047..1efacd5 100644 --- a/src/scene_graph_node.cpp +++ b/src/scene_graph_node.cpp @@ -35,7 +35,6 @@ #include "spark_dsg/scene_graph_node.h" #include "spark_dsg/node_attributes.h" -#include "spark_dsg/node_symbol.h" namespace spark_dsg { From e9f639d513dfec183e7d3e7a7e4baafea75ef4fc Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 19 Jul 2024 18:13:11 +0000 Subject: [PATCH 14/18] remove typedef from --- include/spark_dsg/serialization/file_io.h | 4 ++-- .../spark_dsg/serialization/graph_binary_serialization.h | 8 +++++--- .../spark_dsg/serialization/graph_json_serialization.h | 2 +- include/spark_dsg/spark_dsg_fwd.h | 1 - include/spark_dsg/zmq_interface.h | 4 ++-- src/serialization/file_io.cpp | 4 ++-- 6 files changed, 12 insertions(+), 11 deletions(-) diff --git a/include/spark_dsg/serialization/file_io.h b/include/spark_dsg/serialization/file_io.h index 12ee746..33ea358 100644 --- a/include/spark_dsg/serialization/file_io.h +++ b/include/spark_dsg/serialization/file_io.h @@ -79,7 +79,7 @@ void saveDsgJson(const DynamicSceneGraph& graph, * @param filepath The filepath including extension to load from. * @return A pointer to the loaded graph or nullptr if loading failed. */ -DynamicSceneGraphPtr loadDsgJson(const std::string& filepath); +std::shared_ptr loadDsgJson(const std::string& filepath); /** * @brief Save a DynamicSceneGraph to a file in binary serialization. @@ -96,6 +96,6 @@ void saveDsgBinary(const DynamicSceneGraph& graph, * @param filepath The filepath including extension to load from. * @return A pointer to the loaded graph or nullptr if loading failed. */ -DynamicSceneGraphPtr loadDsgBinary(const std::string& filepath); +std::shared_ptr loadDsgBinary(const std::string& filepath); } // namespace spark_dsg::io diff --git a/include/spark_dsg/serialization/graph_binary_serialization.h b/include/spark_dsg/serialization/graph_binary_serialization.h index e320994..0d5e9ed 100644 --- a/include/spark_dsg/serialization/graph_binary_serialization.h +++ b/include/spark_dsg/serialization/graph_binary_serialization.h @@ -34,8 +34,8 @@ * -------------------------------------------------------------------------- */ #pragma once -#include "spark_dsg/spark_dsg_fwd.h" #include "spark_dsg/serialization/versioning.h" +#include "spark_dsg/spark_dsg_fwd.h" namespace spark_dsg::io::binary { @@ -43,9 +43,11 @@ void writeGraph(const DynamicSceneGraph& graph, std::vector& buffer, bool include_mesh = false); -DynamicSceneGraphPtr readGraph(const uint8_t* const buffer, size_t length); +std::shared_ptr readGraph(const uint8_t* const buffer, + size_t length); -inline DynamicSceneGraphPtr readGraph(const std::vector& buffer) { +inline std::shared_ptr readGraph( + const std::vector& buffer) { return readGraph(buffer.data(), buffer.size()); } diff --git a/include/spark_dsg/serialization/graph_json_serialization.h b/include/spark_dsg/serialization/graph_json_serialization.h index d010510..28a7f3e 100644 --- a/include/spark_dsg/serialization/graph_json_serialization.h +++ b/include/spark_dsg/serialization/graph_json_serialization.h @@ -49,6 +49,6 @@ std::string writeGraph(const DynamicSceneGraph& graph, bool include_mesh = false * @param contents JSON string to parse * @returns Resulting parsed scene graph */ -DynamicSceneGraphPtr readGraph(const std::string& contents); +std::shared_ptr readGraph(const std::string& contents); } // namespace spark_dsg::io::json diff --git a/include/spark_dsg/spark_dsg_fwd.h b/include/spark_dsg/spark_dsg_fwd.h index ac19c2a..853eabf 100644 --- a/include/spark_dsg/spark_dsg_fwd.h +++ b/include/spark_dsg/spark_dsg_fwd.h @@ -13,6 +13,5 @@ struct EdgeContainer; class SceneGraphLayer; class DynamicSceneGraph; -using DynamicSceneGraphPtr = std::shared_ptr; } // namespace spark_dsg diff --git a/include/spark_dsg/zmq_interface.h b/include/spark_dsg/zmq_interface.h index 0115a9b..99fc821 100644 --- a/include/spark_dsg/zmq_interface.h +++ b/include/spark_dsg/zmq_interface.h @@ -60,7 +60,7 @@ class ZmqReceiver { bool recv(size_t timeout_ms, bool recv_all = false); - DynamicSceneGraphPtr graph() const; + std::shared_ptr graph() const; private: struct Detail; @@ -74,7 +74,7 @@ class ZmqGraph { ~ZmqGraph(); bool hasChange() const; - DynamicSceneGraphPtr graph() const; + std::shared_ptr graph() const; private: struct Detail; diff --git a/src/serialization/file_io.cpp b/src/serialization/file_io.cpp index 18c34f1..a8d28ec 100644 --- a/src/serialization/file_io.cpp +++ b/src/serialization/file_io.cpp @@ -95,7 +95,7 @@ void saveDsgBinary(const DynamicSceneGraph& graph, out.write(reinterpret_cast(graph_buffer.data()), graph_buffer.size()); } -DynamicSceneGraphPtr loadDsgBinary(const std::string& filepath) { +std::shared_ptr loadDsgBinary(const std::string& filepath) { // Read the file into a buffer. std::ifstream infile(filepath, std::ios::in | std::ios::binary); std::vector buffer((std::istreambuf_iterator(infile)), @@ -121,7 +121,7 @@ void saveDsgJson(const DynamicSceneGraph& graph, outfile << json::writeGraph(graph, include_mesh); } -DynamicSceneGraphPtr loadDsgJson(const std::string& filepath) { +std::shared_ptr loadDsgJson(const std::string& filepath) { std::ifstream infile(filepath); std::stringstream ss; ss << infile.rdbuf(); From 97a6690a151a56a878bc6161a0043c4e2ec09c60 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 19 Jul 2024 19:15:15 +0000 Subject: [PATCH 15/18] add missing bindings and fix compilation --- CMakeLists.txt | 2 +- python/CMakeLists.txt | 7 + .../spark_dsg/python/scene_graph_iterators.h | 307 ++---------------- python/bindings/src/dynamic_scene_graph.cpp | 138 ++++++++ python/bindings/src/misc_types.cpp | 2 + python/bindings/src/node_attributes.cpp | 1 + python/bindings/src/scene_graph_edge.cpp | 42 +-- python/bindings/src/scene_graph_iterators.cpp | 196 +++++++++++ python/bindings/src/scene_graph_layer.cpp | 7 +- python/bindings/src/scene_graph_node.cpp | 5 + python/bindings/src/scene_graph_utilities.cpp | 15 +- python/tests/test_networkx.py | 2 +- 12 files changed, 394 insertions(+), 330 deletions(-) create mode 100644 python/bindings/src/scene_graph_iterators.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 67a5209..076da19 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -56,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) diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index e99c641..8b6974d 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -22,9 +22,16 @@ configure_file( pybind11_add_module( _dsg_bindings bindings/src/bounding_box.cpp + bindings/src/dynamic_scene_graph.cpp bindings/src/edge_attributes.cpp bindings/src/mesh.cpp + bindings/src/misc_types.cpp bindings/src/node_attributes.cpp + bindings/src/scene_graph_edge.cpp + bindings/src/scene_graph_iterators.cpp + bindings/src/scene_graph_layer.cpp + bindings/src/scene_graph_node.cpp + bindings/src/scene_graph_utilities.cpp bindings/src/spark_dsg_bindings.cpp bindings/src/zmq_interface.cpp ) diff --git a/python/bindings/include/spark_dsg/python/scene_graph_iterators.h b/python/bindings/include/spark_dsg/python/scene_graph_iterators.h index 5082da2..5a7eb18 100644 --- a/python/bindings/include/spark_dsg/python/scene_graph_iterators.h +++ b/python/bindings/include/spark_dsg/python/scene_graph_iterators.h @@ -35,23 +35,16 @@ #pragma once #include -using namespace spark_dsg; +namespace spark_dsg::python { struct IterSentinel {}; class NodeIter { public: - NodeIter(const SceneGraphLayer::Nodes& container) - : curr_iter_(container.begin()), end_iter_(container.end()) {} - - const SceneGraphNode* operator*() const { return curr_iter_->second.get(); } - - NodeIter& operator++() { - ++curr_iter_; - return *this; - } - - bool operator==(const IterSentinel&) { return curr_iter_ == end_iter_; } + NodeIter(const SceneGraphLayer::Nodes& container); + const SceneGraphNode* operator*() const; + NodeIter& operator++(); + bool operator==(const IterSentinel&); private: typename SceneGraphLayer::Nodes::const_iterator curr_iter_; @@ -60,24 +53,10 @@ class NodeIter { class DynamicNodeIter { public: - DynamicNodeIter(const DynamicSceneGraphLayer::Nodes& container) - : curr_iter_(container.begin()), end_iter_(container.end()) { - while (*curr_iter_ == nullptr && curr_iter_ != end_iter_) { - ++curr_iter_; - } - } - - const SceneGraphNode* operator*() const { return curr_iter_->get(); } - - DynamicNodeIter& operator++() { - ++curr_iter_; - while (*curr_iter_ == nullptr && curr_iter_ != end_iter_) { - ++curr_iter_; - } - return *this; - } - - bool operator==(const IterSentinel&) { return curr_iter_ == end_iter_; } + DynamicNodeIter(const DynamicSceneGraphLayer::Nodes& container); + const SceneGraphNode* operator*() const; + DynamicNodeIter& operator++(); + bool operator==(const IterSentinel&); private: typename DynamicSceneGraphLayer::Nodes::const_iterator curr_iter_; @@ -86,17 +65,10 @@ class DynamicNodeIter { class EdgeIter { public: - EdgeIter(const SceneGraphLayer::Edges& container) - : curr_iter_(container.begin()), end_iter_(container.end()) {} - - const SceneGraphEdge* operator*() const { return &(curr_iter_->second); } - - EdgeIter& operator++() { - ++curr_iter_; - return *this; - } - - bool operator==(const IterSentinel&) { return curr_iter_ == end_iter_; } + EdgeIter(const SceneGraphLayer::Edges& container); + const SceneGraphEdge* operator*() const; + EdgeIter& operator++(); + bool operator==(const IterSentinel&); private: typename SceneGraphLayer::Edges::const_iterator curr_iter_; @@ -105,52 +77,11 @@ class EdgeIter { class GlobalNodeIter { public: - GlobalNodeIter(const DynamicSceneGraph& dsg) : valid_(true) { - curr_layer_iter_ = dsg.layers().begin(); - end_layer_iter_ = dsg.layers().end(); - - setNodeIter(); - } - - void setNodeIter() { - if (curr_layer_iter_ == end_layer_iter_) { - valid_ = false; - return; - } - - curr_node_iter_ = curr_layer_iter_->second->nodes().begin(); - end_node_iter_ = curr_layer_iter_->second->nodes().end(); - while (curr_node_iter_ == end_node_iter_) { - ++curr_layer_iter_; - if (curr_layer_iter_ == end_layer_iter_) { - valid_ = false; - return; - } - - curr_node_iter_ = curr_layer_iter_->second->nodes().begin(); - end_node_iter_ = curr_layer_iter_->second->nodes().end(); - } - } - - const SceneGraphNode& operator*() const { return *curr_node_iter_->second; } - - GlobalNodeIter& operator++() { - ++curr_node_iter_; - if (curr_node_iter_ == end_node_iter_) { - ++curr_layer_iter_; - setNodeIter(); - } - - return *this; - } - - bool operator==(const IterSentinel&) { - if (!valid_) { - return true; - } - - return curr_node_iter_ == end_node_iter_ && curr_layer_iter_ == end_layer_iter_; - } + GlobalNodeIter(const DynamicSceneGraph& dsg); + void setNodeIter(); + const SceneGraphNode& operator*() const; + GlobalNodeIter& operator++(); + bool operator==(const IterSentinel&); private: bool valid_; @@ -162,67 +93,11 @@ class GlobalNodeIter { class GlobalEdgeIter { public: - GlobalEdgeIter(const DynamicSceneGraph& dsg) : started_interlayer_(false) { - curr_layer_iter_ = dsg.layers().begin(); - end_layer_iter_ = dsg.layers().end(); - - curr_interlayer_iter_ = dsg.interlayer_edges().begin(); - end_interlayer_iter_ = dsg.interlayer_edges().end(); - - setEdgeIter(); - } - - const SceneGraphEdge* operator*() const { - if (started_interlayer_) { - return &curr_interlayer_iter_->second; - } else { - return &curr_edge_iter_->second; - } - } - - void setEdgeIter() { - if (started_interlayer_ || curr_layer_iter_ == end_layer_iter_) { - started_interlayer_ = true; - return; - } - - curr_edge_iter_ = curr_layer_iter_->second->edges().begin(); - end_edge_iter_ = curr_layer_iter_->second->edges().end(); - - while (curr_edge_iter_ == end_edge_iter_) { - ++curr_layer_iter_; - if (curr_layer_iter_ == end_layer_iter_) { - started_interlayer_ = true; - return; - } - - curr_edge_iter_ = curr_layer_iter_->second->edges().begin(); - end_edge_iter_ = curr_layer_iter_->second->edges().end(); - } - } - - GlobalEdgeIter& operator++() { - if (started_interlayer_) { - ++curr_interlayer_iter_; - return *this; - } - - ++curr_edge_iter_; - if (curr_edge_iter_ == end_edge_iter_) { - ++curr_layer_iter_; - setEdgeIter(); - } - - return *this; - } - - bool operator==(const IterSentinel&) { - if (!started_interlayer_) { - return false; - } - - return curr_interlayer_iter_ == end_interlayer_iter_; - } + GlobalEdgeIter(const DynamicSceneGraph& dsg); + const SceneGraphEdge* operator*() const; + void setEdgeIter(); + GlobalEdgeIter& operator++(); + bool operator==(const IterSentinel&); private: bool started_interlayer_; @@ -234,138 +109,4 @@ class GlobalEdgeIter { typename SceneGraphLayer::Edges::const_iterator end_interlayer_iter_; }; -class LayerView { - public: - LayerView(const SceneGraphLayer& layer) : id(layer.id), layer_ref_(layer) {} - - NodeIter nodes() const { return NodeIter(layer_ref_.nodes()); } - - EdgeIter edges() const { return EdgeIter(layer_ref_.edges()); } - - size_t numNodes() const { return layer_ref_.numNodes(); } - - size_t numEdges() const { return layer_ref_.numEdges(); } - - bool hasNode(NodeId node_id) const { return layer_ref_.hasNode(node_id); } - - bool hasEdge(NodeId source, NodeId target) const { - return layer_ref_.hasEdge(source, target); - } - - const SceneGraphNode& getNode(NodeId node_id) const { - return layer_ref_.getNode(node_id); - } - - const SceneGraphEdge& getEdge(NodeId source, NodeId target) const { - return layer_ref_.getEdge(source, target); - } - - Eigen::Vector3d getPosition(NodeId node_id) const { - return layer_ref_.getPosition(node_id); - } - - const LayerId id; - - private: - const SceneGraphLayer& layer_ref_; -}; - -class LayerIter { - public: - LayerIter(const DynamicSceneGraph::Layers& container) - : curr_iter_(container.begin()), end_iter_(container.end()) {} - - LayerView operator*() const { return LayerView(*(curr_iter_->second)); } - - LayerIter& operator++() { - ++curr_iter_; - return *this; - } - - bool operator==(const IterSentinel&) { return curr_iter_ == end_iter_; } - - private: - typename DynamicSceneGraph::Layers::const_iterator curr_iter_; - typename DynamicSceneGraph::Layers::const_iterator end_iter_; -}; - -class DynamicLayerView { - public: - DynamicLayerView(const DynamicSceneGraphLayer& layer) - : id(layer.id), prefix(layer.prefix), layer_ref_(layer) {} - - DynamicNodeIter nodes() const { return DynamicNodeIter(layer_ref_.nodes()); } - - EdgeIter edges() const { return EdgeIter(layer_ref_.edges()); } - - size_t numNodes() const { return layer_ref_.numNodes(); } - - size_t numEdges() const { return layer_ref_.numEdges(); } - - const LayerId id; - - const LayerPrefix prefix; - - private: - const DynamicSceneGraphLayer& layer_ref_; -}; - -class DynamicLayerIter { - public: - using LayerMap = std::map; - - DynamicLayerIter(const LayerMap& container) - : valid_(true), curr_iter_(container.begin()), end_iter_(container.end()) { - setSubIter(); - } - - void setSubIter() { - if (curr_iter_ == end_iter_) { - valid_ = false; - return; - } - - curr_layer_iter_ = curr_iter_->second.begin(); - end_layer_iter_ = curr_iter_->second.end(); - - while (curr_layer_iter_ == end_layer_iter_) { - ++curr_iter_; - if (curr_iter_ == end_iter_) { - valid_ = false; - return; - } - - curr_layer_iter_ = curr_iter_->second.begin(); - end_layer_iter_ = curr_iter_->second.end(); - } - } - - DynamicLayerView operator*() const { - return DynamicLayerView(*(curr_layer_iter_->second)); - } - - DynamicLayerIter& operator++() { - ++curr_layer_iter_; - if (curr_layer_iter_ == end_layer_iter_) { - ++curr_iter_; - setSubIter(); - } - - return *this; - } - - bool operator==(const IterSentinel&) { - if (!valid_) { - return true; - } - - return curr_layer_iter_ == end_layer_iter_ && curr_iter_ == end_iter_; - } - - private: - bool valid_; - typename LayerMap::const_iterator curr_iter_; - typename LayerMap::const_iterator end_iter_; - typename DynamicSceneGraph::DynamicLayers::const_iterator curr_layer_iter_; - typename DynamicSceneGraph::DynamicLayers::const_iterator end_layer_iter_; -}; +} // namespace spark_dsg::python diff --git a/python/bindings/src/dynamic_scene_graph.cpp b/python/bindings/src/dynamic_scene_graph.cpp index 91577c2..e1acff5 100644 --- a/python/bindings/src/dynamic_scene_graph.cpp +++ b/python/bindings/src/dynamic_scene_graph.cpp @@ -38,6 +38,9 @@ #include #include #include +#include +#include +#include #include "spark_dsg/python/scene_graph_iterators.h" @@ -46,6 +49,141 @@ using namespace py::literals; namespace spark_dsg::python::dynamic_scene_graph { +class LayerView { + public: + LayerView(const SceneGraphLayer& layer) : id(layer.id), layer_ref_(layer) {} + + NodeIter nodes() const { return NodeIter(layer_ref_.nodes()); } + + EdgeIter edges() const { return EdgeIter(layer_ref_.edges()); } + + size_t numNodes() const { return layer_ref_.numNodes(); } + + size_t numEdges() const { return layer_ref_.numEdges(); } + + bool hasNode(NodeId node_id) const { return layer_ref_.hasNode(node_id); } + + bool hasEdge(NodeId source, NodeId target) const { + return layer_ref_.hasEdge(source, target); + } + + const SceneGraphNode& getNode(NodeId node_id) const { + return layer_ref_.getNode(node_id); + } + + const SceneGraphEdge& getEdge(NodeId source, NodeId target) const { + return layer_ref_.getEdge(source, target); + } + + Eigen::Vector3d getPosition(NodeId node_id) const { + return layer_ref_.getNode(node_id).attributes().position; + } + + const LayerId id; + + private: + const SceneGraphLayer& layer_ref_; +}; + +class LayerIter { + public: + LayerIter(const DynamicSceneGraph::Layers& container) + : curr_iter_(container.begin()), end_iter_(container.end()) {} + + LayerView operator*() const { return LayerView(*(curr_iter_->second)); } + + LayerIter& operator++() { + ++curr_iter_; + return *this; + } + + bool operator==(const IterSentinel&) { return curr_iter_ == end_iter_; } + + private: + typename DynamicSceneGraph::Layers::const_iterator curr_iter_; + typename DynamicSceneGraph::Layers::const_iterator end_iter_; +}; + +class DynamicLayerView { + public: + DynamicLayerView(const DynamicSceneGraphLayer& layer) + : id(layer.id), prefix(layer.prefix), layer_ref_(layer) {} + + DynamicNodeIter nodes() const { return DynamicNodeIter(layer_ref_.nodes()); } + + EdgeIter edges() const { return EdgeIter(layer_ref_.edges()); } + + size_t numNodes() const { return layer_ref_.numNodes(); } + + size_t numEdges() const { return layer_ref_.numEdges(); } + + const LayerId id; + + const LayerPrefix prefix; + + private: + const DynamicSceneGraphLayer& layer_ref_; +}; + +class DynamicLayerIter { + public: + using LayerMap = std::map; + + DynamicLayerIter(const LayerMap& container) + : valid_(true), curr_iter_(container.begin()), end_iter_(container.end()) { + setSubIter(); + } + + void setSubIter() { + if (curr_iter_ == end_iter_) { + valid_ = false; + return; + } + + curr_layer_iter_ = curr_iter_->second.begin(); + end_layer_iter_ = curr_iter_->second.end(); + + while (curr_layer_iter_ == end_layer_iter_) { + ++curr_iter_; + if (curr_iter_ == end_iter_) { + valid_ = false; + return; + } + + curr_layer_iter_ = curr_iter_->second.begin(); + end_layer_iter_ = curr_iter_->second.end(); + } + } + + DynamicLayerView operator*() const { + return DynamicLayerView(*(curr_layer_iter_->second)); + } + + DynamicLayerIter& operator++() { + ++curr_layer_iter_; + if (curr_layer_iter_ == end_layer_iter_) { + ++curr_iter_; + setSubIter(); + } + + return *this; + } + + bool operator==(const IterSentinel&) { + if (!valid_) { + return true; + } + + return curr_layer_iter_ == end_layer_iter_ && curr_iter_ == end_iter_; + } + + private: + bool valid_; + typename LayerMap::const_iterator curr_iter_; + typename LayerMap::const_iterator end_iter_; + typename DynamicSceneGraph::DynamicLayers::const_iterator curr_layer_iter_; + typename DynamicSceneGraph::DynamicLayers::const_iterator end_layer_iter_; +}; void addBindings(pybind11::module_& module) { py::class_(module, "LayerView") .def("has_node", &LayerView::hasNode) diff --git a/python/bindings/src/misc_types.cpp b/python/bindings/src/misc_types.cpp index 5c126e6..54c7ae6 100644 --- a/python/bindings/src/misc_types.cpp +++ b/python/bindings/src/misc_types.cpp @@ -35,6 +35,8 @@ #include "spark_dsg/python/misc_types.h" #include +#include +#include #include #include diff --git a/python/bindings/src/node_attributes.cpp b/python/bindings/src/node_attributes.cpp index b4b8983..16d3ec1 100644 --- a/python/bindings/src/node_attributes.cpp +++ b/python/bindings/src/node_attributes.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include diff --git a/python/bindings/src/scene_graph_edge.cpp b/python/bindings/src/scene_graph_edge.cpp index f722600..b1af7c6 100644 --- a/python/bindings/src/scene_graph_edge.cpp +++ b/python/bindings/src/scene_graph_edge.cpp @@ -32,48 +32,14 @@ * Government is authorized to reproduce and distribute reprints for Government * purposes notwithstanding any copyright notation herein. * -------------------------------------------------------------------------- */ -#include - -#include "spark_dsg/edge_attributes.h" +#include "spark_dsg/python/scene_graph_edge.h" -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED - * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ #include #include +#include +#include -#include "spark_dsg/python/scene_graph_edge.h" +#include namespace py = pybind11; using namespace py::literals; diff --git a/python/bindings/src/scene_graph_iterators.cpp b/python/bindings/src/scene_graph_iterators.cpp new file mode 100644 index 0000000..eef3112 --- /dev/null +++ b/python/bindings/src/scene_graph_iterators.cpp @@ -0,0 +1,196 @@ +/* ----------------------------------------------------------------------------- + * Copyright 2022 Massachusetts Institute of Technology. + * All Rights Reserved + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * Research was sponsored by the United States Air Force Research Laboratory and + * the United States Air Force Artificial Intelligence Accelerator and was + * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views + * and conclusions contained in this document are those of the authors and should + * not be interpreted as representing the official policies, either expressed or + * implied, of the United States Air Force or the U.S. Government. The U.S. + * Government is authorized to reproduce and distribute reprints for Government + * purposes notwithstanding any copyright notation herein. + * -------------------------------------------------------------------------- */ +#include "spark_dsg/python/scene_graph_iterators.h" + +namespace spark_dsg::python { + +NodeIter::NodeIter(const SceneGraphLayer::Nodes& container) + : curr_iter_(container.begin()), end_iter_(container.end()) {} + +const SceneGraphNode* NodeIter::operator*() const { return curr_iter_->second.get(); } + +NodeIter& NodeIter::operator++() { + ++curr_iter_; + return *this; +} + +bool NodeIter::operator==(const IterSentinel&) { return curr_iter_ == end_iter_; } + +DynamicNodeIter::DynamicNodeIter(const DynamicSceneGraphLayer::Nodes& container) + : curr_iter_(container.begin()), end_iter_(container.end()) { + while (*curr_iter_ == nullptr && curr_iter_ != end_iter_) { + ++curr_iter_; + } +} + +const SceneGraphNode* DynamicNodeIter::operator*() const { return curr_iter_->get(); } + +DynamicNodeIter& DynamicNodeIter::operator++() { + ++curr_iter_; + while (*curr_iter_ == nullptr && curr_iter_ != end_iter_) { + ++curr_iter_; + } + return *this; +} + +bool DynamicNodeIter::operator==(const IterSentinel&) { + return curr_iter_ == end_iter_; +} + +EdgeIter::EdgeIter(const SceneGraphLayer::Edges& container) + : curr_iter_(container.begin()), end_iter_(container.end()) {} + +const SceneGraphEdge* EdgeIter::operator*() const { return &(curr_iter_->second); } + +EdgeIter& EdgeIter::operator++() { + ++curr_iter_; + return *this; +} + +bool EdgeIter::operator==(const IterSentinel&) { return curr_iter_ == end_iter_; } + +GlobalNodeIter::GlobalNodeIter(const DynamicSceneGraph& dsg) : valid_(true) { + curr_layer_iter_ = dsg.layers().begin(); + end_layer_iter_ = dsg.layers().end(); + + setNodeIter(); +} + +void GlobalNodeIter::setNodeIter() { + if (curr_layer_iter_ == end_layer_iter_) { + valid_ = false; + return; + } + + curr_node_iter_ = curr_layer_iter_->second->nodes().begin(); + end_node_iter_ = curr_layer_iter_->second->nodes().end(); + while (curr_node_iter_ == end_node_iter_) { + ++curr_layer_iter_; + if (curr_layer_iter_ == end_layer_iter_) { + valid_ = false; + return; + } + + curr_node_iter_ = curr_layer_iter_->second->nodes().begin(); + end_node_iter_ = curr_layer_iter_->second->nodes().end(); + } +} + +const SceneGraphNode& GlobalNodeIter::operator*() const { + return *curr_node_iter_->second; +} + +GlobalNodeIter& GlobalNodeIter::operator++() { + ++curr_node_iter_; + if (curr_node_iter_ == end_node_iter_) { + ++curr_layer_iter_; + setNodeIter(); + } + + return *this; +} + +bool GlobalNodeIter::operator==(const IterSentinel&) { + if (!valid_) { + return true; + } + + return curr_node_iter_ == end_node_iter_ && curr_layer_iter_ == end_layer_iter_; +} + +GlobalEdgeIter::GlobalEdgeIter(const DynamicSceneGraph& dsg) + : started_interlayer_(false) { + curr_layer_iter_ = dsg.layers().begin(); + end_layer_iter_ = dsg.layers().end(); + + curr_interlayer_iter_ = dsg.interlayer_edges().begin(); + end_interlayer_iter_ = dsg.interlayer_edges().end(); + + setEdgeIter(); +} + +const SceneGraphEdge* GlobalEdgeIter::operator*() const { + if (started_interlayer_) { + return &curr_interlayer_iter_->second; + } else { + return &curr_edge_iter_->second; + } +} + +void GlobalEdgeIter::setEdgeIter() { + if (started_interlayer_ || curr_layer_iter_ == end_layer_iter_) { + started_interlayer_ = true; + return; + } + + curr_edge_iter_ = curr_layer_iter_->second->edges().begin(); + end_edge_iter_ = curr_layer_iter_->second->edges().end(); + + while (curr_edge_iter_ == end_edge_iter_) { + ++curr_layer_iter_; + if (curr_layer_iter_ == end_layer_iter_) { + started_interlayer_ = true; + return; + } + + curr_edge_iter_ = curr_layer_iter_->second->edges().begin(); + end_edge_iter_ = curr_layer_iter_->second->edges().end(); + } +} + +GlobalEdgeIter& GlobalEdgeIter::operator++() { + if (started_interlayer_) { + ++curr_interlayer_iter_; + return *this; + } + + ++curr_edge_iter_; + if (curr_edge_iter_ == end_edge_iter_) { + ++curr_layer_iter_; + setEdgeIter(); + } + + return *this; +} + +bool GlobalEdgeIter::operator==(const IterSentinel&) { + if (!started_interlayer_) { + return false; + } + + return curr_interlayer_iter_ == end_interlayer_iter_; +} + +} // namespace spark_dsg::python diff --git a/python/bindings/src/scene_graph_layer.cpp b/python/bindings/src/scene_graph_layer.cpp index 1b9f385..af3ca0e 100644 --- a/python/bindings/src/scene_graph_layer.cpp +++ b/python/bindings/src/scene_graph_layer.cpp @@ -36,6 +36,8 @@ #include #include +#include +#include #include #include "spark_dsg/python/scene_graph_iterators.h" @@ -83,7 +85,10 @@ void addBindings(pybind11::module_& module) { .def("remove_edge", &IsolatedSceneGraphLayer::removeEdge) .def("num_nodes", &IsolatedSceneGraphLayer::numNodes) .def("num_edges", &IsolatedSceneGraphLayer::numEdges) - .def("get_position", &IsolatedSceneGraphLayer::getPosition) + .def("get_position", + [](const IsolatedSceneGraphLayer& layer, NodeId node) { + return layer.getNode(node).attributes().position; + }) .def_readonly("id", &IsolatedSceneGraphLayer::id) .def_property( "nodes", diff --git a/python/bindings/src/scene_graph_node.cpp b/python/bindings/src/scene_graph_node.cpp index ed16bcc..1a719f7 100644 --- a/python/bindings/src/scene_graph_node.cpp +++ b/python/bindings/src/scene_graph_node.cpp @@ -36,8 +36,13 @@ #include #include +#include +#include +#include #include +#include + namespace py = pybind11; using namespace py::literals; diff --git a/python/bindings/src/scene_graph_utilities.cpp b/python/bindings/src/scene_graph_utilities.cpp index 0f8339e..d0cb55f 100644 --- a/python/bindings/src/scene_graph_utilities.cpp +++ b/python/bindings/src/scene_graph_utilities.cpp @@ -36,6 +36,7 @@ #include #include +#include #include namespace py = pybind11; @@ -43,11 +44,13 @@ using namespace py::literals; namespace spark_dsg::python::scene_graph_utilities { -module.def("compute_ancestor_bounding_box", - &computeAncestorBoundingBox, - "G"_a, - "node_id"_a, - "child_layer"_a = DsgLayers::PLACES, - "bbox_type"_a = BoundingBox::Type::AABB); +void addBindings(pybind11::module_& module) { + module.def("compute_ancestor_bounding_box", + &computeAncestorBoundingBox, + "G"_a, + "node_id"_a, + "child_layer"_a = DsgLayers::PLACES, + "bbox_type"_a = BoundingBox::Type::AABB); +} } // namespace spark_dsg::python::scene_graph_utilities diff --git a/python/tests/test_networkx.py b/python/tests/test_networkx.py index 385e458..e990344 100644 --- a/python/tests/test_networkx.py +++ b/python/tests/test_networkx.py @@ -76,7 +76,7 @@ def test_full_graph_conversion(resource_dir): assert G_nx is not None assert len(G_nx) == G.num_nodes(include_mesh=False) - assert len(G_nx.edges) == G.num_edges(include_mesh=False) + assert len(G_nx.edges) == G.num_edges() _check_attribute_validity(G_nx) From 1d3e377eeec327c4b75c99a70dc75d1770fe8350 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 19 Jul 2024 19:17:33 +0000 Subject: [PATCH 16/18] add binary serialization for python layers --- .../graph_binary_serialization.h | 10 ++ include/spark_dsg/spark_dsg_fwd.h | 1 + python/bindings/src/scene_graph_layer.cpp | 14 ++- python/src/spark_dsg/io.py | 15 +-- .../graph_binary_serialization.cpp | 91 +++++++++++++++++-- 5 files changed, 116 insertions(+), 15 deletions(-) diff --git a/include/spark_dsg/serialization/graph_binary_serialization.h b/include/spark_dsg/serialization/graph_binary_serialization.h index 0d5e9ed..90f1030 100644 --- a/include/spark_dsg/serialization/graph_binary_serialization.h +++ b/include/spark_dsg/serialization/graph_binary_serialization.h @@ -43,6 +43,8 @@ void writeGraph(const DynamicSceneGraph& graph, std::vector& buffer, bool include_mesh = false); +void writeLayer(const SceneGraphLayer& graph, std::vector& buffer); + std::shared_ptr readGraph(const uint8_t* const buffer, size_t length); @@ -51,6 +53,14 @@ inline std::shared_ptr readGraph( return readGraph(buffer.data(), buffer.size()); } +std::shared_ptr readLayer(const uint8_t* const buffer, + size_t length); + +inline std::shared_ptr readLayer( + const std::vector& buffer) { + return readLayer(buffer.data(), buffer.size()); +} + bool updateGraph(DynamicSceneGraph& graph, const uint8_t* const buffer, size_t length); inline bool updateGraph(DynamicSceneGraph& graph, const std::vector& buffer) { diff --git a/include/spark_dsg/spark_dsg_fwd.h b/include/spark_dsg/spark_dsg_fwd.h index 853eabf..6fa12fb 100644 --- a/include/spark_dsg/spark_dsg_fwd.h +++ b/include/spark_dsg/spark_dsg_fwd.h @@ -12,6 +12,7 @@ struct SceneGraphEdge; struct EdgeContainer; class SceneGraphLayer; +class IsolatedSceneGraphLayer; class DynamicSceneGraph; } // namespace spark_dsg diff --git a/python/bindings/src/scene_graph_layer.cpp b/python/bindings/src/scene_graph_layer.cpp index af3ca0e..af0ca92 100644 --- a/python/bindings/src/scene_graph_layer.cpp +++ b/python/bindings/src/scene_graph_layer.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include "spark_dsg/python/scene_graph_iterators.h" @@ -103,7 +104,18 @@ void addBindings(pybind11::module_& module) { return py::make_iterator(EdgeIter(view.edges()), IterSentinel()); }, nullptr, - py::return_value_policy::reference_internal); + py::return_value_policy::reference_internal) + .def("to_binary", + [](const IsolatedSceneGraphLayer& layer) -> py::bytes { + std::vector buffer; + io::binary::writeLayer(layer, buffer); + return py::bytes(reinterpret_cast(buffer.data()), buffer.size()); + }) + .def_static("from_binary", [](const py::bytes& contents) { + const auto& view = static_cast(contents); + return io::binary::readLayer(reinterpret_cast(view.data()), + view.size()); + }); } } // namespace spark_dsg::python::scene_graph_layer diff --git a/python/src/spark_dsg/io.py b/python/src/spark_dsg/io.py index be86dde..7819d8c 100644 --- a/python/src/spark_dsg/io.py +++ b/python/src/spark_dsg/io.py @@ -33,14 +33,15 @@ # # """Module for reading and writing collections of DSGS.""" -from spark_dsg._dsg_bindings import SceneGraphLayer -from dataclasses import dataclass -from typing import Union, Dict import functools -import pathlib -import mmap import json +import mmap import os +import pathlib +from dataclasses import dataclass +from typing import Dict, Union + +from spark_dsg._dsg_bindings import SceneGraphLayer @dataclass @@ -156,7 +157,7 @@ def _get_graph(self, index): start_byte = info.offset end_byte = info.offset + info.size - return SceneGraphLayer.from_bson(self._mmap[start_byte:end_byte]) + return SceneGraphLayer.from_binary(self._mmap[start_byte:end_byte]) @requires_open def __getitem__(self, index): @@ -200,7 +201,7 @@ def save(collection: Dict[int, SceneGraphLayer], path: Union[str, pathlib.Path]) offset = 0 with graph_path.open("wb") as fout: for index, graph in collection.items(): - contents = graph.to_bson() + contents = graph.to_binary() fout.write(contents) info = IndexInfo( index=index, diff --git a/src/serialization/graph_binary_serialization.cpp b/src/serialization/graph_binary_serialization.cpp index 86d7ff7..70573eb 100644 --- a/src/serialization/graph_binary_serialization.cpp +++ b/src/serialization/graph_binary_serialization.cpp @@ -64,9 +64,16 @@ using spark_dsg::serialization::AttributeFactory; using spark_dsg::serialization::BinaryDeserializer; using spark_dsg::serialization::BinarySerializer; +using NodeCallback = std::function&&, + std::optional)>; +using EdgeCallback = + std::function&&)>; + NodeId parseNode(const AttributeFactory& factory, const BinaryDeserializer& deserializer, - DynamicSceneGraph& graph) { + const NodeCallback& callback) { deserializer.checkFixedArrayLength(4); LayerId layer; deserializer.read(layer); @@ -80,13 +87,13 @@ NodeId parseNode(const AttributeFactory& factory, return node; } - graph.addOrUpdateNode(layer, node, std::move(attrs), stamp); + callback(layer, node, std::move(attrs), stamp); return node; } void parseEdge(const AttributeFactory& factory, const BinaryDeserializer& deserializer, - DynamicSceneGraph& graph) { + const EdgeCallback& callback) { deserializer.checkFixedArrayLength(3); NodeId source; deserializer.read(source); @@ -95,7 +102,28 @@ void parseEdge(const AttributeFactory& factory, // last argument always forces parents to rewire auto attrs = serialization::Visitor::from(factory, deserializer); - graph.addOrUpdateEdge(source, target, std::move(attrs)); + callback(source, target, std::move(attrs)); +} + +void writeLayer(const SceneGraphLayer& graph, std::vector& buffer) { + BinarySerializer serializer(&buffer); + serializer.write(graph.id); + + // saves names to type index mapping + serializer.write(serialization::AttributeRegistry::names()); + serializer.write(serialization::AttributeRegistry::names()); + + serializer.startDynamicArray(); + for (const auto& id_node_pair : graph.nodes()) { + serializer.write(*id_node_pair.second); + } + serializer.endDynamicArray(); + + serializer.startDynamicArray(); + for (const auto& id_edge_pair : graph.edges()) { + serializer.write(id_edge_pair.second); + } + serializer.endDynamicArray(); } void writeGraph(const DynamicSceneGraph& graph, @@ -190,12 +218,22 @@ bool updateGraph(DynamicSceneGraph& graph, const BinaryDeserializer& deserialize deserializer.checkDynamicArray(); while (!deserializer.isDynamicArrayEnd()) { - stale_nodes.erase(parseNode(node_factory, deserializer, graph)); + stale_nodes.erase(parseNode( + node_factory, + deserializer, + [&graph](const auto& layer, const auto& node, auto&& attrs, const auto& stamp) { + graph.addOrUpdateNode(layer, node, std::move(attrs), stamp); + })); } deserializer.checkDynamicArray(); while (!deserializer.isDynamicArrayEnd()) { - stale_nodes.erase(parseNode(node_factory, deserializer, graph)); + stale_nodes.erase(parseNode( + node_factory, + deserializer, + [&graph](const auto& layer, const auto& node, auto&& attrs, const auto& stamp) { + graph.addOrUpdateNode(layer, node, std::move(attrs), stamp); + })); } for (const auto& node_id : stale_nodes) { @@ -205,7 +243,11 @@ bool updateGraph(DynamicSceneGraph& graph, const BinaryDeserializer& deserialize graph.markEdgesAsStale(); deserializer.checkDynamicArray(); while (!deserializer.isDynamicArrayEnd()) { - parseEdge(edge_factory, deserializer, graph); + parseEdge(edge_factory, + deserializer, + [&graph](const auto& source, const auto& target, auto&& attrs) { + graph.addOrUpdateEdge(source, target, std::move(attrs)); + }); } graph.removeAllStaleEdges(); @@ -233,6 +275,41 @@ DynamicSceneGraph::Ptr readGraph(const uint8_t* const buffer, size_t length) { return graph; } +std::shared_ptr readLayer(const uint8_t* const buffer, + size_t length) { + const auto& header = io::GlobalInfo::loadedHeader(); + + BinaryDeserializer deserializer(buffer, length); + LayerId layer_id; + deserializer.read(layer_id); + + auto graph = std::make_shared(layer_id); + + // load name to type index mapping if present + const auto node_factory = loadFactory(header, deserializer); + const auto edge_factory = loadFactory(header, deserializer); + + deserializer.checkDynamicArray(); + while (!deserializer.isDynamicArrayEnd()) { + parseNode(node_factory, + deserializer, + [&graph](const auto&, const auto& node, auto&& attrs, const auto&) { + graph->emplaceNode(node, std::move(attrs)); + }); + } + + deserializer.checkDynamicArray(); + while (!deserializer.isDynamicArrayEnd()) { + parseEdge(edge_factory, + deserializer, + [&](const auto& source, const auto& target, auto&& attrs) { + graph->insertEdge(source, target, std::move(attrs)); + }); + } + + return graph; +} + bool updateGraph(DynamicSceneGraph& graph, const uint8_t* const buffer, size_t length) { BinaryDeserializer deserializer(buffer, length); From 6973a8d51df4c140ef74476c85945a960c8ac91b Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 19 Jul 2024 19:24:49 +0000 Subject: [PATCH 17/18] fix pytorch tests --- python/src/spark_dsg/torch_conversion.py | 26 ++++++++---------------- python/tests/test_torch.py | 14 ++++++------- 2 files changed, 16 insertions(+), 24 deletions(-) diff --git a/python/src/spark_dsg/torch_conversion.py b/python/src/spark_dsg/torch_conversion.py index 12dda39..f730b3a 100644 --- a/python/src/spark_dsg/torch_conversion.py +++ b/python/src/spark_dsg/torch_conversion.py @@ -39,18 +39,14 @@ graphs. Note that `DynamicSceneGraph.to_torch()` calls into the relevant homogeneous or heterogeneous conversion function. """ -from spark_dsg._dsg_bindings import ( - DynamicSceneGraph, - SceneGraphLayer, - SceneGraphNode, - SceneGraphEdge, - DsgLayers, - LayerView, -) -from typing import Callable, Optional, Dict, Union -import numpy as np import importlib +from typing import Callable, Dict, Optional, Union + +import numpy as np +from spark_dsg._dsg_bindings import (DsgLayers, DynamicSceneGraph, LayerView, + SceneGraphEdge, SceneGraphLayer, + SceneGraphNode) NodeConversionFunc = Callable[[DynamicSceneGraph, SceneGraphNode], np.ndarray] EdgeConversionFunc = Callable[[DynamicSceneGraph, SceneGraphEdge], np.ndarray] @@ -136,17 +132,13 @@ def scene_graph_layer_to_torch( scene graph layer. """ torch, torch_geometric = _get_torch() - # output torch tensor data types - if double_precision: - dtype_float = torch.float64 - else: - dtype_float = torch.float32 + dtype_float = torch.float64 if double_precision else torch.float32 N = G.num_nodes() node_features = [] - node_positions = torch.zeros((N, 3), dtype=torch.float64) + node_positions = torch.zeros((N, 3), dtype=dtype_float) id_map = {} for node in G.nodes: @@ -168,7 +160,7 @@ def scene_graph_layer_to_torch( edge_features.append(edge_converter(G, edge)) if edge_converter is not None: - edge_features = torch.tensor(np.array(edge_features), dtype_float) + edge_features = torch.tensor(np.array(edge_features), dtype=dtype_float) if edge_index.size(dim=1) > 0: if edge_converter is None: diff --git a/python/tests/test_torch.py b/python/tests/test_torch.py index 0c27943..e6108ca 100644 --- a/python/tests/test_torch.py +++ b/python/tests/test_torch.py @@ -73,7 +73,7 @@ def _check_interlayer_edges(G, data, to_check, has_edge_attrs=False): edge_name = f"{source}_to_{target}" assert (source, edge_name, target) in metadata[1] assert data[source, edge_name, target].edge_index.size(dim=0) == 2 - assert data[source, edge_name, target].edge_index.size(dim=1) >= 2 + assert data[source, edge_name, target].edge_index.size(dim=1) >= 1 if has_edge_attrs: assert data[source, edge_name, target].edge_attr.size(dim=1) == 20 assert data[source, edge_name, target].edge_attr.size(dim=0) == data[ @@ -86,7 +86,7 @@ def test_torch_layer(resource_dir, has_torch): if not has_torch: return pytest.skip(reason="requires pytorch and pytorch geometric") - G = dsg.DynamicSceneGraph.load(str(resource_dir / "apartment_igx_dsg.json")) + G = dsg.DynamicSceneGraph.load(str(resource_dir / "apartment_dsg.json")) places = G.get_layer(dsg.DsgLayers.PLACES) assert places.num_nodes() > 0 assert places.num_edges() > 0 @@ -104,7 +104,7 @@ def test_torch_layer_edge_features(resource_dir, has_torch): if not has_torch: return pytest.skip(reason="requires pytorch and pytorch geometric") - G = dsg.DynamicSceneGraph.load(str(resource_dir / "apartment_igx_dsg.json")) + G = dsg.DynamicSceneGraph.load(str(resource_dir / "apartment_dsg.json")) places = G.get_layer(dsg.DsgLayers.PLACES) assert places.num_nodes() > 0 assert places.num_edges() > 0 @@ -122,7 +122,7 @@ def test_torch_homogeneous(resource_dir, has_torch): if not has_torch: return pytest.skip(reason="requires pytorch and pytorch geometric") - G = dsg.DynamicSceneGraph.load(str(resource_dir / "apartment_igx_dsg.json")) + G = dsg.DynamicSceneGraph.load(str(resource_dir / "apartment_dsg.json")) assert G.num_nodes() > 0 assert G.num_edges() > 0 @@ -139,7 +139,7 @@ def test_torch_homogeneous_edge_features(resource_dir, has_torch): if not has_torch: return pytest.skip(reason="requires pytorch and pytorch geometric") - G = dsg.DynamicSceneGraph.load(str(resource_dir / "apartment_igx_dsg.json")) + G = dsg.DynamicSceneGraph.load(str(resource_dir / "apartment_dsg.json")) assert G.num_nodes() > 0 assert G.num_edges() > 0 @@ -160,7 +160,7 @@ def test_torch_hetereogeneous(resource_dir, has_torch): if not has_torch: return pytest.skip(reason="requires pytorch and pytorch geometric") - G = dsg.DynamicSceneGraph.load(str(resource_dir / "apartment_igx_dsg.json")) + G = dsg.DynamicSceneGraph.load(str(resource_dir / "apartment_dsg.json")) assert G.num_nodes() > 0 assert G.num_edges() > 0 @@ -190,7 +190,7 @@ def test_torch_hetereogeneous_edge_features(resource_dir, has_torch): if not has_torch: return pytest.skip(reason="requires pytorch and pytorch geometric") - G = dsg.DynamicSceneGraph.load(str(resource_dir / "apartment_igx_dsg.json")) + G = dsg.DynamicSceneGraph.load(str(resource_dir / "apartment_dsg.json")) assert G.num_nodes() > 0 assert G.num_edges() > 0 From a100aa01253eeeb1f911e4b0a8981b186f185069 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Fri, 19 Jul 2024 19:57:13 +0000 Subject: [PATCH 18/18] remove commented code --- src/node_symbol.cpp | 53 +++++++++++++++------------------------------ 1 file changed, 17 insertions(+), 36 deletions(-) diff --git a/src/node_symbol.cpp b/src/node_symbol.cpp index 366965a..7b5423c 100644 --- a/src/node_symbol.cpp +++ b/src/node_symbol.cpp @@ -46,29 +46,28 @@ NodeSymbol::NodeSymbol(char key, NodeId index) { NodeSymbol::NodeSymbol(NodeId value) { value_.value = value; } - NodeSymbol::operator NodeId() const { return value_.value; } +NodeSymbol::operator NodeId() const { return value_.value; } - NodeId NodeSymbol::categoryId() const { return value_.symbol.index; } +NodeId NodeSymbol::categoryId() const { return value_.symbol.index; } - char NodeSymbol::category() const { return value_.symbol.key; } +char NodeSymbol::category() const { return value_.symbol.key; } - NodeSymbol& NodeSymbol::operator++() { - value_.symbol.index++; - return *this; - } - - NodeSymbol NodeSymbol::operator++(int) { - NodeSymbol old = *this; - value_.symbol.index++; - return old; - } +NodeSymbol& NodeSymbol::operator++() { + value_.symbol.index++; + return *this; +} - std::string NodeSymbol::getLabel() const { - std::stringstream ss; - ss << *this; - return ss.str(); - } +NodeSymbol NodeSymbol::operator++(int) { + NodeSymbol old = *this; + value_.symbol.index++; + return old; +} +std::string NodeSymbol::getLabel() const { + std::stringstream ss; + ss << *this; + return ss.str(); +} NodeSymbol operator"" _id(const char* str, size_t size) { if (size < 1) { @@ -81,24 +80,6 @@ NodeSymbol operator"" _id(const char* str, size_t size) { return NodeSymbol(prefix, index); } -/* -template -std::string displayNodeSymbolContainer(const Container& set) { - std::stringstream ss; - ss << "["; - auto iter = set.begin(); - while (iter != set.end()) { - ss << NodeSymbol(*iter).getLabel(); - ++iter; - if (iter != set.end()) { - ss << ", "; - } - } - ss << "]"; - return ss.str(); -} -*/ - std::ostream& operator<<(std::ostream& out, const NodeSymbol& symbol) { if (std::isalpha(symbol.value_.symbol.key)) { out << symbol.value_.symbol.key << "(" << symbol.value_.symbol.index << ")";