diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp index 5ea80e8c773bde92b8d4dae67490a1b5560cefbe..2fd894a53894cd778ceabbedab556f185036aee4 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp +++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.cpp @@ -8,10 +8,24 @@ #include <RobotAPI/libraries/core/Pose.h> #include <RobotAPI/libraries/core/FramedPose.h> +#include "ice_conversions.h" + namespace armarx::objpose { + Eigen::Vector3f toEigen(const Vector3BasePtr pose) + { + auto cast = Vector3Ptr::dynamicCast(pose); + ARMARX_CHECK_NOT_NULL(cast); + return cast->toEigen(); + } + Eigen::Matrix3f toEigen(const QuaternionBasePtr pose) + { + auto cast = QuaternionPtr::dynamicCast(pose); + ARMARX_CHECK_NOT_NULL(cast); + return cast->toEigen(); + } Eigen::Matrix4f toEigen(const PoseBasePtr pose) { auto cast = PosePtr::dynamicCast(pose); @@ -45,7 +59,7 @@ namespace armarx::objpose confidence = ice.confidence; timestamp = IceUtil::Time::microSeconds(ice.timestampMicroSeconds); - localOOBB = ice.localOOBB; + objpose::fromIce(ice.localOOBB, localOOBB); } data::ObjectPose ObjectPose::toIce() const @@ -97,7 +111,7 @@ namespace armarx::objpose confidence = provided.confidence; timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds); - localOOBB = provided.localOOBB; + objpose::fromIce(provided.localOOBB, localOOBB); } } @@ -158,3 +172,7 @@ namespace armarx } + + + + diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h index 23c92b55f6fc1bf261b49cdc06206a391a27ec74..814ba29c50b3021663bf3d50163eea65f8aced9f 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h +++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPose.h @@ -2,6 +2,7 @@ #include <Eigen/Core> +#include <SimoxUtility/shapes/OrientedBox.h> #include <VirtualRobot/VirtualRobot.h> #include <IceUtil/Time.h> @@ -51,7 +52,7 @@ namespace armarx::objpose IceUtil::Time timestamp = IceUtil::Time::microSeconds(-1); /// Object bounding box in object's local coordinate frame. - Box localOOBB; + simox::OrientedBoxf localOOBB; }; using ObjectPoseSeq = std::vector<ObjectPose>; diff --git a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp index d80cbdebf30d8ec1b13bf6fd688e061c1d036045..506fc289280788d783f46455ead20a2ded115454 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp +++ b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.cpp @@ -15,6 +15,26 @@ namespace armarx { + Eigen::Vector3f toEigen(const Vector3BasePtr pose) + { + auto cast = Vector3Ptr::dynamicCast(pose); + ARMARX_CHECK_NOT_NULL(cast); + return cast->toEigen(); + } + Eigen::Matrix3f toEigen(const QuaternionBasePtr pose) + { + auto cast = QuaternionPtr::dynamicCast(pose); + ARMARX_CHECK_NOT_NULL(cast); + return cast->toEigen(); + } + Eigen::Matrix4f toEigen(const PoseBasePtr pose) + { + auto cast = PosePtr::dynamicCast(pose); + ARMARX_CHECK_NOT_NULL(cast); + return cast->toEigen(); + } + + std::string objpose::getID(const ObjectID& id) { return id.dataset + "/" + id.name; @@ -41,15 +61,50 @@ namespace armarx return ice; } - objpose::Box objpose::toIce(const simox::OrientedBox<float>& oobb) + + void objpose::fromIce(const Box& box, simox::OrientedBoxf& oobb) { - objpose::Box ice; - ice.position = new Vector3(oobb.center()); - ice.orientation = new Quaternion(oobb.rotation().eval()); - ice.extents = new Vector3(oobb.dimensions()); - return ice; + try + { + Eigen::Vector3f pos = toEigen(box.position); + Eigen::Matrix3f ori = toEigen(box.orientation); + Eigen::Vector3f extents = toEigen(box.extents); + Eigen::Vector3f corner = pos - ori * extents / 2; + + oobb = simox::OrientedBox<float> (corner, + ori.col(0) * extents(0), + ori.col(1) * extents(1), + ori.col(2) * extents(2)); + } + catch (const armarx::LocalException&) + { + // No OOBB information. + oobb = {}; + } } + simox::OrientedBoxf objpose::fromIce(const Box& box) + { + simox::OrientedBoxf oobb; + fromIce(box, oobb); + return oobb; + } + + void objpose::toIce(Box& box, const simox::OrientedBoxf& oobb) + { + box.position = new Vector3(oobb.center()); + box.orientation = new Quaternion(oobb.rotation().eval()); + box.extents = new Vector3(oobb.dimensions()); + } + + objpose::Box objpose::toIce(const simox::OrientedBoxf& oobb) + { + objpose::Box box; + toIce(box, oobb); + return box; + } + + } diff --git a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h index 21d78c947a5211e9220e6f75fd86942632161ddd..b459a01cd9a7411cdbbb1c32dc2a15394c0dee25 100644 --- a/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h +++ b/source/RobotAPI/components/ObjectPoseObserver/ice_conversions.h @@ -26,6 +26,12 @@ namespace armarx::objpose objpose::AABB toIce(const simox::AxisAlignedBoundingBox& aabb); - objpose::Box toIce(const simox::OrientedBox<float>& oobb); + + + void fromIce(const Box& box, simox::OrientedBox<float>& oobb); + simox::OrientedBox<float> fromIce(const Box& box); + + void toIce(Box& box, const simox::OrientedBox<float>& oobb); + Box toIce(const simox::OrientedBox<float>& oobb); }