diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp index ae8531edacea24b495fac67c39fd50f51a927835..af808d1de3634821a761e2ae4bb3183aa1e2bcfd 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 78cbfbfc6bbe46bd0dc8c24995dfc33973f68e3b..d00e548b82bc9bdd72a46aeb997a6eb297e89ba4 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>;