ObjectPose.h

#pragma once

#include <IceUtil/Time.h>

#include <Eigen/Core>

#include <RobotAPI/interface/objectpose/types.h>


namespace armarx::objpose
{

    /**
     * @brief An object pose as stored by the ObjectPoseObserver.
     */
    struct ObjectPose
    {
        ObjectPose();
        ObjectPose(const data::ObjectPose& ice);

        void fromIce(const data::ObjectPose& ice);

        data::ObjectPose toIce() const;
        void toIce(data::ObjectPose& ice) const;


        /// Name of the providing component.
        std::string providerName;
        /// Known or unknown object.
        ObjectTypeEnum objectType = AnyObject;

        /// The object ID, i.e. dataset and name.
        ObjectID objectID;

        Eigen::Matrix4f objectPoseRobot;
        Eigen::Matrix4f objectPoseGlobal;
        Eigen::Matrix4f objectPoseOriginal;
        std::string objectPoseOriginalFrame;

        std::map<std::string, float> robotConfig;
        Eigen::Matrix4f robotPose;

        /// Confidence in [0, 1] (1 = full, 0 = none).
        float confidence = 0;
        /// Source timestamp.
        IceUtil::Time timestamp = IceUtil::Time::microSeconds(-1);

        /// Object bounding box in object's local coordinate frame.
        Box localOOBB;
    };
    using ObjectPoseSeq = std::vector<ObjectPose>;


    void fromIce(const data::ObjectPose& ice, ObjectPose& pose);
    ObjectPose fromIce(const data::ObjectPose& ice);

    void fromIce(const data::ObjectPoseSeq& ice, ObjectPoseSeq& poses);
    ObjectPoseSeq fromIce(const data::ObjectPoseSeq& ice);


    void toIce(data::ObjectPose& ice, const ObjectPose& pose);
    data::ObjectPose toIce(const ObjectPose& pose);

    void toIce(data::ObjectPoseSeq& ice, const ObjectPoseSeq& poses);
    data::ObjectPoseSeq toIce(const ObjectPoseSeq& poses);

}