From f74754723c0887bab3580012bf0644b736c5e4b0 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Fri, 18 Sep 2020 11:10:13 +0200
Subject: [PATCH] Add getters for OOBB in robot and global frame
---
.../ObjectPoseObserver/ObjectPose.cpp | 19 +++++++++++++++++++
.../ObjectPoseObserver/ObjectPose.h | 8 ++++++++
2 files changed, 27 insertions(+)
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
index ae8531eda..af808d1de 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp
@@ -113,6 +113,25 @@ namespace armarx::objpose
objpose::fromIce(provided.localOOBB, localOOBB);
}
+
+ std::optional<simox::OrientedBoxf> ObjectPose::oobbRobot() const
+ {
+ if (localOOBB)
+ {
+ return localOOBB->transformed(objectPoseRobot);
+ }
+ return std::nullopt;
+ }
+
+ std::optional<simox::OrientedBoxf> ObjectPose::oobbGlobal() const
+ {
+ if (localOOBB)
+ {
+ return localOOBB->transformed(objectPoseGlobal);
+ }
+ return std::nullopt;
+ }
+
}
namespace armarx
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
index 78cbfbfc6..d00e548b8 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h
@@ -54,7 +54,15 @@ namespace armarx::objpose
IceUtil::Time timestamp = IceUtil::Time::microSeconds(-1);
/// Object bounding box in object's local coordinate frame.
+ /// @see oobbRobot(), oobbGlobal()
std::optional<simox::OrientedBoxf> localOOBB;
+
+
+ /// Get the OOBB in the robot frame (according to `objectPoseRobot`).
+ std::optional<simox::OrientedBoxf> oobbRobot() const;
+ /// Get the OOBB in the global frame (according to `objectPoseGlobal`).
+ std::optional<simox::OrientedBoxf> oobbGlobal() const;
+
};
using ObjectPoseSeq = std::vector<ObjectPose>;
--
GitLab