ObjectPose.h

Go to the documentation of this file.

#pragma once
 
#include <map>
#include <optional>
#include <vector>
 
#include <Eigen/Core>
 
#include <SimoxUtility/shapes/OrientedBox.h>
#include <VirtualRobot/VirtualRobot.h>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/time/DateTime.h>
 
#include <RobotAPI/interface/objectpose/object_pose_types.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectID.h>
#include <RobotAPI/libraries/ArmarXObjects/PoseManifoldGaussian.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
 
namespace armarx::objpose
{
 
    struct ObjectAttachmentInfo
    {
        std::string frameName;
        std::string agentName;
        Eigen::Matrix4f poseInFrame = Eigen::Matrix4f::Identity();
    };
 
    /**
     * @brief An object pose as stored by the ObjectPoseStorage.
     */
    struct ObjectPose
    {
    public:
        ObjectPose();
        ObjectPose(const data::ObjectPose& ice);
 
        void fromIce(const data::ObjectPose& ice);
 
        data::ObjectPose toIce() const;
        void toIce(data::ObjectPose& ice) const;
 
        void fromProvidedPose(const data::ProvidedObjectPose& provided,
                              VirtualRobot::RobotPtr robot = nullptr);
        objpose::ProvidedObjectPose toProvidedObjectPoseGlobal() const;
 
        void setObjectPoseRobot(const Eigen::Matrix4f& objectPoseRobot,
                                bool updateObjectPoseGlobal = true);
        void setObjectPoseGlobal(const Eigen::Matrix4f& objectPoseGlobal,
                                 bool updateObjectPoseRobot = true);
 
 
    public:
        /// The object ID, i.e. dataset, class name and instance name.
        armarx::ObjectID objectID;
 
        /// Name of the providing component.
        std::string providerName;
        /// Known or unknown object.
        ObjectType objectType = AnyObject;
        /// Whether object is static. Static objects don't decay.
        bool isStatic = false;
 
        /// The object pose in the robot root frame.
        Eigen::Matrix4f objectPoseRobot = Eigen::Matrix4f::Identity();
        /// ... and with uncertainty.
        std::optional<PoseManifoldGaussian> objectPoseRobotGaussian;
 
        /// The object pose in the global frame.
        Eigen::Matrix4f objectPoseGlobal = Eigen::Matrix4f::Identity();
        /// ... and with uncertainty.
        std::optional<PoseManifoldGaussian> objectPoseGlobalGaussian;
 
        /// The object pose in the frame it was originally localized in.
        Eigen::Matrix4f objectPoseOriginal = Eigen::Matrix4f::Identity();
        /// The frame the object was originally localized in.
        std::string objectPoseOriginalFrame;
        /// ... and with uncertainty.
        std::optional<PoseManifoldGaussian> objectPoseOriginalGaussian;
 
        /// The object's joint values if it is articulated.
        std::map<std::string, float> objectJointValues;
 
        /// The robot config when the object was observed.
        std::map<std::string, float> robotConfig;
        /// The robot pose when the object was observed.
        Eigen::Matrix4f robotPose = Eigen::Matrix4f::Identity();
        /// The name of the robot.
        std::string robotName;
 
        /// Attachment information.
        std::optional<ObjectAttachmentInfo> attachment;
 
        /// Confidence in [0, 1] (1 = full, 0 = none).
        float confidence = 0;
        /// Source timestamp.
        DateTime timestamp = DateTime::Invalid();
 
        /// 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;
 
        /**
         * @brief Get a copy of `*this` with updated attachment.
         * @see `updateAttached()`
         */
        objpose::ObjectPose getAttached(VirtualRobot::RobotPtr agent) const;
        /**
         * @brief Update an the object pose and robot state of an attached
         * object pose according to the given agent state (in-place).
         * @param agent The agent/robot.
         */
        void updateAttached(VirtualRobot::RobotPtr agent);
 
 
        std::optional<PackagePath> articulatedSimoxXmlPath;
    };
 
    void fromIce(const data::ObjectAttachmentInfo& ice, ObjectAttachmentInfo& attachment);
    void fromIce(const data::ObjectAttachmentInfoPtr& ice,
                 std::optional<ObjectAttachmentInfo>& attachment);
    std::optional<ObjectAttachmentInfo> fromIce(const data::ObjectAttachmentInfoPtr& ice);
 
    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::ObjectAttachmentInfo& ice, const ObjectAttachmentInfo& attachment);
    void toIce(data::ObjectAttachmentInfoPtr& ice,
               const std::optional<ObjectAttachmentInfo>& attachment);
    data::ObjectAttachmentInfoPtr toIce(const std::optional<ObjectAttachmentInfo>& 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);
 
 
    // Operations on ObjectPoseSeq
 
    /**
     * @brief Find an object pose by the object ID. Return nullptr if not found.
     * @param objectPoses The object poses.
     * @param id The object ID.
     * @return A pointer to the (first) object pose with objectID == id, or nullptr if none is found.
     */
    ObjectPose* findObjectPoseByID(ObjectPoseSeq& objectPoses, const ObjectID& id);
    const ObjectPose* findObjectPoseByID(const ObjectPoseSeq& objectPoses, const ObjectID& id);
 
    data::ObjectPose* findObjectPoseByID(data::ObjectPoseSeq& objectPoses,
                                         const armarx::data::ObjectID& id);
    const data::ObjectPose* findObjectPoseByID(const data::ObjectPoseSeq& objectPoses,
                                               const armarx::data::ObjectID& id);
 
} // namespace armarx::objpose