diff --git a/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h b/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h new file mode 100644 index 0000000000000000000000000000000000000000..c9dc89e13f97e47e40bb9303612c475c5e962d52 --- /dev/null +++ b/source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h @@ -0,0 +1,31 @@ +#pragma once + +#include <map> +#include <vector> + + +namespace armarx +{ + class ObjectID; + class ObjectInfo; + class ObjectFinder; +} +namespace armarx::objpose +{ + struct ObjectAttachmentInfo; + struct PoseCovariance; + + struct PoseCovariance; + + struct ObjectPose; + + using ObjectPoseSeq = std::vector<ObjectPose>; + using ObjectPoseMap = std::map<ObjectID, ObjectPose>; + + class ProvidedObjectPose; + + using ProvidedObjectPoseSeq = std::vector<ProvidedObjectPose>; + using ProvidedObjectPoseMap = std::map<ObjectID, ProvidedObjectPose>; + + class ObjectPoseClient; +} diff --git a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp index ccbae7e21c426f92d946dd6744168a04f2c70e7c..cac3a2f2b7e7d64ca521fe3e80d92fc27faed25a 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp +++ b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.cpp @@ -158,4 +158,64 @@ namespace armarx toIce(box, oobb); return box; } + + + + void objpose::fromIce(const data::PoseCovariance& ice, PoseCovariance& cov) + { + ARMARX_CHECK_EQUAL(static_cast<Eigen::Index>(ice.translation3x3.size()), cov.translation.size()); + cov.translation = Eigen::MatrixXf::Map(ice.translation3x3.data(), + cov.translation.rows(), + cov.translation.cols()); + } + + void objpose::fromIce(const data::PoseCovariancePtr& ice, std::optional<PoseCovariance>& cov) + { + if (ice) + { + cov = PoseCovariance(); + fromIce(*ice, cov.value()); + } + else + { + cov = std::nullopt; + } + } + + std::optional<objpose::PoseCovariance> objpose::fromIce(const data::PoseCovariancePtr& ice) + { + std::optional<objpose::PoseCovariance> cov; + fromIce(ice, cov); + return cov; + } + + + void objpose::toIce(data::PoseCovariance& ice, const PoseCovariance& cov) + { + ice.translation3x3.resize(cov.translation.size()); + Eigen::MatrixXf::Map(ice.translation3x3.data(), + cov.translation.rows(), + cov.translation.cols()) = cov.translation; + } + + void objpose::toIce(data::PoseCovariancePtr& ice, const std::optional<PoseCovariance>& cov) + { + if (cov.has_value()) + { + ice = new data::PoseCovariance; + toIce(*ice, cov.value()); + } + else + { + ice = nullptr; + } + } + + objpose::data::PoseCovariancePtr objpose::toIce(const std::optional<PoseCovariance>& cov) + { + data::PoseCovariancePtr ice; + toIce(ice, cov); + return ice; + } + } diff --git a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h index 429d6246c18f7a36769d6c3d686c12c175cd8b8f..e26a70d0f7de93785b8a359a76ff3007e0fc4147 100644 --- a/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h +++ b/source/RobotAPI/libraries/ArmarXObjects/ice_conversions.h @@ -6,9 +6,11 @@ #include <RobotAPI/interface/objectpose/object_pose_types.h> #include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h> +#include "forward_declarations.h" #include "ObjectPose.h" #include "ObjectID.h" + namespace simox { // #include <SimoxUtility/shapes/AxisAlignedBoundingBox.h> @@ -52,4 +54,14 @@ namespace armarx::objpose void toIce(BoxPtr& box, const std::optional<simox::OrientedBox<float>>& oobb); void toIce(Box& box, const simox::OrientedBox<float>& oobb); Box toIce(const simox::OrientedBox<float>& oobb); + + + void fromIce(const data::PoseCovariance& ice, PoseCovariance& cov); + void fromIce(const data::PoseCovariancePtr& ice, std::optional<PoseCovariance>& cov); + std::optional<PoseCovariance> fromIce(const data::PoseCovariancePtr& ice); + + void toIce(data::PoseCovariance& ice, const PoseCovariance& cov); + void toIce(data::PoseCovariancePtr& ice, const std::optional<PoseCovariance>& cov); + data::PoseCovariancePtr toIce(const std::optional<PoseCovariance>& ice); + }