additional bindings and functionalities

include/spark_dsg/dynamic_scene_graph.h

@@ -445,6 +445,9 @@ class DynamicSceneGraph {
   //! @brief Make a copy of the scene graph
   DynamicSceneGraph::Ptr clone() const;
 
+  //! @brief Rigidly transform graph
+  void transform(const Eigen::Isometry3d& transform);
+
   /**
    * @brief Save the DSG to file. By default, this will save a binary version of the
    * graph. To save as JSON, specify the filepath with a .json extension.

include/spark_dsg/scene_graph_layer.h

@@ -261,6 +261,11 @@ class SceneGraphLayer {
    */
   virtual SceneGraphLayer::Ptr clone(const NodeChecker& is_valid = {}) const;
 
+  /**
+   * @brief Rigidly transform layer
+   */
+  void transform(const Eigen::Isometry3d& transform);
+
   /**
    * @brief Get the immediate neighborhood of a node via BFS
    * @param node Node to get the neighborhood of

python/bindings/src/spark_dsg_bindings.cpp

@@ -452,6 +452,7 @@ PYBIND11_MODULE(_dsg_bindings, module) {
           [](AgentNodeAttributes& attrs, const Quaternion<double>& rot) {
             attrs.world_R_body = rot;
           })
+      .def_readwrite("timestamp", &AgentNodeAttributes::timestamp)
       .def_readwrite("external_key", &AgentNodeAttributes::external_key)
       .def_readwrite("dbow_ids", &AgentNodeAttributes::dbow_ids)
       .def_readwrite("dbow_values", &AgentNodeAttributes::dbow_values);
@@ -942,6 +943,11 @@ PYBIND11_MODULE(_dsg_bindings, module) {
           },
           "name"_a)
       .def("clone", &DynamicSceneGraph::clone)
+      .def("transform",
+           [](DynamicSceneGraph& G, const Eigen::Matrix4d& mat) {
+             Eigen::Isometry3d iso(mat);
+             G.transform(iso);
+           })
       .def("__deepcopy__",
            [](const DynamicSceneGraph& G, py::object) { return G.clone(); })
       .def(

src/dynamic_scene_graph.cpp

@@ -688,6 +688,13 @@ DynamicSceneGraph::Ptr DynamicSceneGraph::clone() const {
   return to_return;
 }
 
+void DynamicSceneGraph::transform(const Eigen::Isometry3d& transform) {
+  visitLayers([&](LayerKey, Layer& layer) { layer.transform(transform); });
+  if (mesh_) {
+    mesh_->transform(transform.cast<float>());
+  }
+}
+
 void DynamicSceneGraph::save(std::string filepath, bool include_mesh) const {
   const auto type = io::verifyFileExtension(filepath);
   if (type == io::FileType::JSON) {

src/scene_graph_layer.cpp

@@ -384,6 +384,12 @@ SceneGraphLayer::Ptr SceneGraphLayer::clone(const NodeChecker& is_valid) const {
   return new_layer;
 }
 
+void SceneGraphLayer::transform(const Eigen::Isometry3d& transform) {
+  for (auto&& [id, node] : nodes_) {
+    node->attributes().transform(transform);
+  }
+}
+
 namespace graph_utilities {
 
 using LayerGraphTraits = graph_traits<SceneGraphLayer>;