diff --git a/include/spark_dsg/bounding_box.h b/include/spark_dsg/bounding_box.h index f883278b..3508c768 100644 --- a/include/spark_dsg/bounding_box.h +++ b/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 diff --git a/include/spark_dsg/dynamic_scene_graph.h b/include/spark_dsg/dynamic_scene_graph.h index 6ad56e29..de8f7e19 100644 --- a/include/spark_dsg/dynamic_scene_graph.h +++ b/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 diff --git a/include/spark_dsg/node_attributes.h b/include/spark_dsg/node_attributes.h index b86d77a6..248051f5 100644 --- a/include/spark_dsg/node_attributes.h +++ b/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 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; diff --git a/include/spark_dsg/scene_graph_layer.h b/include/spark_dsg/scene_graph_layer.h index fccf6361..2e5ce797 100644 --- a/include/spark_dsg/scene_graph_layer.h +++ b/include/spark_dsg/scene_graph_layer.h @@ -234,7 +234,7 @@ class SceneGraphLayer { void mergeLayer(const SceneGraphLayer& other, const GraphMergeConfig& config, std::vector* 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 diff --git a/src/bounding_box.cpp b/src/bounding_box.cpp index ae372c7c..26def179 100644 --- a/src/bounding_box.cpp +++ b/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() * world_P_center; + world_R_center = transform.rotation().cast() * world_R_center; +} + bool BoundingBox::operator==(const BoundingBox& other) const { if (type != other.type) { return false; diff --git a/src/dynamic_scene_graph.cpp b/src/dynamic_scene_graph.cpp index 67ad48ee..72ec3586 100644 --- a/src/dynamic_scene_graph.cpp +++ b/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) { diff --git a/src/node_attributes.cpp b/src/node_attributes.cpp index fd22bc35..7a01149a 100644 --- a/src/node_attributes.cpp +++ b/src/node_attributes.cpp @@ -113,6 +113,10 @@ NodeAttributes::Ptr NodeAttributes::clone() const { return std::make_unique(*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(*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(*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(*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); diff --git a/src/scene_graph_layer.cpp b/src/scene_graph_layer.cpp index 531dafe5..5bb5835b 100644 --- a/src/scene_graph_layer.cpp +++ b/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* 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; diff --git a/tests/utest_scene_graph_layer.cpp b/tests/utest_scene_graph_layer.cpp index 632ee198..ba6c1db7 100644 --- a/tests/utest_scene_graph_layer.cpp +++ b/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 new_nodes; layer_1.mergeLayer(layer_2, {}, &new_nodes, &transform);