transform node attributes

include/spark_dsg/bounding_box.h

@@ -213,6 +213,12 @@ struct BoundingBox {
    */
   float computeIoU(const BoundingBox& other) const;
 
+  /**
+   * @brief Transform the bounding box.
+   * @param transform The transform to apply.
+   */
+  void transform(const Eigen::Isometry3d& transform);
+
   /**
    * @brief output bounding box information
    * @param out output stream

include/spark_dsg/dynamic_scene_graph.h

@@ -399,7 +399,7 @@ class DynamicSceneGraph {
    */
   bool mergeGraph(const DynamicSceneGraph& other,
                   const GraphMergeConfig& config = {},
-                  const Eigen::Affine3d* transform = nullptr);
+                  const Eigen::Isometry3d* transform = nullptr);
 
   /**
    * @brief Get all removed nodes from the graph

include/spark_dsg/node_attributes.h

@@ -106,6 +106,8 @@ struct NodeAttributes {
   virtual ~NodeAttributes() = default;
   virtual NodeAttributes::Ptr clone() const;
 
+  virtual void transform(const Eigen::Isometry3d& transform);
+
   //! Position of the node
   Eigen::Vector3d position;
   //! last time the place was updated (while active)
@@ -167,6 +169,7 @@ struct SemanticNodeAttributes : public NodeAttributes {
   SemanticNodeAttributes();
   virtual ~SemanticNodeAttributes() = default;
   NodeAttributes::Ptr clone() const override;
+  virtual void transform(const Eigen::Isometry3d& transform) override;
 
   bool hasLabel() const;
   bool hasFeature() const;
@@ -207,6 +210,7 @@ struct ObjectNodeAttributes : public SemanticNodeAttributes {
   ObjectNodeAttributes();
   virtual ~ObjectNodeAttributes() = default;
   NodeAttributes::Ptr clone() const override;
+  virtual void transform(const Eigen::Isometry3d& transform);
 
   //! Mesh vertice connections
   std::list<size_t> mesh_connections;
@@ -371,6 +375,7 @@ struct AgentNodeAttributes : public NodeAttributes {
                       NodeId external_key);
   virtual ~AgentNodeAttributes() = default;
   NodeAttributes::Ptr clone() const override;
+  virtual void transform(const Eigen::Isometry3d& transform) override;
 
   std::chrono::nanoseconds timestamp;
   Eigen::Quaterniond world_R_body;

include/spark_dsg/scene_graph_layer.h

@@ -234,7 +234,7 @@ class SceneGraphLayer {
   void mergeLayer(const SceneGraphLayer& other,
                   const GraphMergeConfig& config,
                   std::vector<NodeId>* new_nodes = nullptr,
-                  const Eigen::Affine3d* transform_new_nodes = nullptr);
+                  const Eigen::Isometry3d* transform_new_nodes = nullptr);
 
   /**
    * @brief Get node ids of newly inserted nodes

src/bounding_box.cpp

@@ -241,6 +241,11 @@ float BoundingBox::computeIoU(const BoundingBox& other) const {
   return intersection_volume / union_volume;
 }
 
+void BoundingBox::transform(const Eigen::Isometry3d& transform) {
+  world_P_center = transform.cast<float>() * world_P_center;
+  world_R_center = transform.rotation().cast<float>() * world_R_center;
+}
+
 bool BoundingBox::operator==(const BoundingBox& other) const {
   if (type != other.type) {
     return false;

src/dynamic_scene_graph.cpp

@@ -552,7 +552,7 @@ bool DynamicSceneGraph::updateFromLayer(const SceneGraphLayer& other_layer,
 
 bool DynamicSceneGraph::mergeGraph(const DynamicSceneGraph& other,
                                    const GraphMergeConfig& config,
-                                   const Eigen::Affine3d* transform_new_nodes) {
+                                   const Eigen::Isometry3d* transform_new_nodes) {
   metadata.add(other.metadata());
 
   other.visitLayers([&](LayerKey layer_key, const SceneGraphLayer& other_layer) {

src/node_attributes.cpp

@@ -113,6 +113,10 @@ NodeAttributes::Ptr NodeAttributes::clone() const {
   return std::make_unique<NodeAttributes>(*this);
 }
 
+void NodeAttributes::transform(const Eigen::Isometry3d& transform) {
+  position = transform * position;
+}
+
 bool NodeAttributes::operator==(const NodeAttributes& other) const {
   return is_equal(other);
 }
@@ -160,6 +164,11 @@ NodeAttributes::Ptr SemanticNodeAttributes::clone() const {
   return std::make_unique<SemanticNodeAttributes>(*this);
 }
 
+void SemanticNodeAttributes::transform(const Eigen::Isometry3d& transform) {
+  NodeAttributes::transform(transform);
+  bounding_box.transform(transform);
+}
+
 bool SemanticNodeAttributes::hasLabel() const {
   return semantic_label != NO_SEMANTIC_LABEL;
 }
@@ -227,6 +236,12 @@ NodeAttributes::Ptr ObjectNodeAttributes::clone() const {
   return std::make_unique<ObjectNodeAttributes>(*this);
 }
 
+void ObjectNodeAttributes::transform(const Eigen::Isometry3d& transform) {
+  SemanticNodeAttributes::transform(transform);
+  world_R_object =
+      Eigen::Quaterniond(transform.linear() * world_R_object.toRotationMatrix());
+}
+
 std::ostream& ObjectNodeAttributes::fill_ostream(std::ostream& out) const {
   SemanticNodeAttributes::fill_ostream(out);
   out << "\n  - mesh_connections: " << showIterable(mesh_connections);
@@ -428,6 +443,11 @@ NodeAttributes::Ptr AgentNodeAttributes::clone() const {
   return std::make_unique<AgentNodeAttributes>(*this);
 }
 
+void AgentNodeAttributes::transform(const Eigen::Isometry3d& transform) {
+  NodeAttributes::transform(transform);
+  world_R_body = transform.linear() * world_R_body;
+}
+
 std::ostream& AgentNodeAttributes::fill_ostream(std::ostream& out) const {
   NodeAttributes::fill_ostream(out);
   out << "\n  - orientation: " << quatToString(world_R_body);

src/scene_graph_layer.cpp

@@ -240,7 +240,7 @@ bool SceneGraphLayer::rewireEdge(NodeId source,
 void SceneGraphLayer::mergeLayer(const SceneGraphLayer& other_layer,
                                  const GraphMergeConfig& config,
                                  std::vector<NodeId>* new_nodes,
-                                 const Eigen::Affine3d* transform_new_nodes) {
+                                 const Eigen::Isometry3d* transform_new_nodes) {
   const bool update_attributes = config.shouldUpdateAttributes(id);
   for (const auto& id_node_pair : other_layer.nodes_) {
     const auto siter = nodes_status_.find(id_node_pair.first);
@@ -265,7 +265,7 @@ void SceneGraphLayer::mergeLayer(const SceneGraphLayer& other_layer,
 
     auto attrs = other.attributes_->clone();
     if (transform_new_nodes) {
-      attrs->position = *transform_new_nodes * attrs->position;
+      attrs->transform(*transform_new_nodes);
     }
     nodes_[other.id] = Node::Ptr(new Node(other.id, id, std::move(attrs)));
     nodes_status_[other.id] = NodeStatus::NEW;

tests/utest_scene_graph_layer.cpp

@@ -359,7 +359,7 @@ TEST(SceneGraphLayerTests, MergeLayerTransformCorrect) {
   Eigen::Matrix4d transform_matrix;
   transform_matrix << -1, 0, 0, 0, 0, -1, 0, 2, 0, 0, 1, 3, 0, 0, 0, 1;
 
-  Eigen::Affine3d transform(transform_matrix);
+  Eigen::Isometry3d transform(transform_matrix);
   std::vector<NodeId> new_nodes;
   layer_1.mergeLayer(layer_2, {}, &new_nodes, &transform);