Add forward_declarations header and ice conversions

source/RobotAPI/libraries/ArmarXObjects/forward_declarations.h

+#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;
+}

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;
+    }
+
 }

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);
+
 }