From a89e44b0a473d3040a7170902047df88acb02658 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Wed, 21 Sep 2022 13:46:05 +0200
Subject: [PATCH 01/14] Unifying publication to transform server.

Avoid publishing the ports for the head, left and right arm.

Avoid publishing the eye frames

Adding the hands poses manually.
---
 src/devices/openxrheadset/CMakeLists.txt      |   6 +-
 src/devices/openxrheadset/EyesManager.cpp     |  14 +-
 src/devices/openxrheadset/EyesManager.h       |   6 +-
 src/devices/openxrheadset/FramePorts.cpp      | 172 ------------------
 src/devices/openxrheadset/FramePorts.h        |  50 -----
 src/devices/openxrheadset/OpenXrHeadset.cpp   |  44 +----
 src/devices/openxrheadset/OpenXrHeadset.h     |  14 +-
 src/devices/openxrheadset/OpenXrInterface.cpp |  21 ++-
 src/devices/openxrheadset/OpenXrInterface.h   |   2 +-
 ...nalPosesPublisher.cpp => PosesManager.cpp} |  18 +-
 ...itionalPosesPublisher.h => PosesManager.h} |  12 +-
 src/devices/openxrheadset/SingleEyePort.cpp   |  31 +---
 src/devices/openxrheadset/SingleEyePort.h     |  12 +-
 13 files changed, 52 insertions(+), 350 deletions(-)
 delete mode 100644 src/devices/openxrheadset/FramePorts.cpp
 delete mode 100644 src/devices/openxrheadset/FramePorts.h
 rename src/devices/openxrheadset/{AdditionalPosesPublisher.cpp => PosesManager.cpp} (54%)
 rename src/devices/openxrheadset/{AdditionalPosesPublisher.h => PosesManager.h} (69%)

diff --git a/src/devices/openxrheadset/CMakeLists.txt b/src/devices/openxrheadset/CMakeLists.txt
index e1fd5f6..1074791 100644
--- a/src/devices/openxrheadset/CMakeLists.txt
+++ b/src/devices/openxrheadset/CMakeLists.txt
@@ -50,8 +50,7 @@ set(yarp_openxrheadset_driver_SRCS
     OpenXrYarpUtilities.cpp
     SingleEyePort.cpp
     EyesManager.cpp
-    AdditionalPosesPublisher.cpp
-    FramePorts.cpp
+    PosesManager.cpp
     PosePublisher.cpp
 )
 
@@ -60,8 +59,7 @@ set(yarp_openxrheadset_driver_HDRS
     OpenXrYarpUtilities.h
     SingleEyePort.h
     EyesManager.h
-    AdditionalPosesPublisher.h
-    FramePorts.h
+    PosesManager.h
     PosePublisher.h
 )
 
diff --git a/src/devices/openxrheadset/EyesManager.cpp b/src/devices/openxrheadset/EyesManager.cpp
index d6e4221..99585d5 100644
--- a/src/devices/openxrheadset/EyesManager.cpp
+++ b/src/devices/openxrheadset/EyesManager.cpp
@@ -20,11 +20,10 @@ EyesManager::Options &EyesManager::options()
     return m_options;
 }
 
-bool EyesManager::initialize(yarp::dev::IFrameTransform *tfPublisher, const std::string &headFrame)
+bool EyesManager::initialize()
 {
     if (!m_leftEye.open(m_options.leftEyeQuadLayer,
-                        m_options.portPrefix + "/eyeAngles/left:i",
-                        tfPublisher, m_options.leftEyeFrame, headFrame)) {
+                        m_options.portPrefix + "/eyeAngles/left:i")) {
         yCError(OPENXRHEADSET) << "Cannot initialize left display texture.";
         return false;
     }
@@ -34,8 +33,7 @@ bool EyesManager::initialize(yarp::dev::IFrameTransform *tfPublisher, const std:
     m_leftEye.setEyeRelativeImagePosition(Eigen::Vector3f(0.0, 0.0, m_options.eyeZPosition));
 
     if (!m_rightEye.open(m_options.rightEyeQuadLayer,
-                         m_options.portPrefix + "/eyeAngles/right:i",
-                         tfPublisher, m_options.rightEyeFrame, headFrame)) {
+                         m_options.portPrefix + "/eyeAngles/right:i")) {
         yCError(OPENXRHEADSET) << "Cannot initialize right display texture.";
         return false;
     }
@@ -118,12 +116,6 @@ bool EyesManager::update()
     return true;
 }
 
-void EyesManager::publishEyesTransforms()
-{
-    m_leftEye.publishEyeTransform();
-    m_rightEye.publishEyeTransform();
-}
-
 std::vector<double> EyesManager::getLeftImageDimensions()
 {
     std::vector<double> output(2);
diff --git a/src/devices/openxrheadset/EyesManager.h b/src/devices/openxrheadset/EyesManager.h
index 8585e0a..b791582 100644
--- a/src/devices/openxrheadset/EyesManager.h
+++ b/src/devices/openxrheadset/EyesManager.h
@@ -24,8 +24,6 @@ class EyesManager
         std::shared_ptr<IOpenXrQuadLayer> leftEyeQuadLayer;
         std::shared_ptr<IOpenXrQuadLayer> rightEyeQuadLayer;
         std::string portPrefix;
-        std::string leftEyeFrame;
-        std::string rightEyeFrame;
         double leftAzimuthOffset{0.0};
         double leftElevationOffset{0.0};
         double rightAzimuthOffset{0.0};
@@ -39,14 +37,12 @@ class EyesManager
 
     Options& options();
 
-    bool initialize(yarp::dev::IFrameTransform *tfPublisher, const std::string& headFrame);
+    bool initialize();
 
     void close();
 
     bool update();
 
-    void publishEyesTransforms();
-
     std::vector<double> getLeftImageDimensions();
 
     std::vector<double> getRightImageDimensions();
diff --git a/src/devices/openxrheadset/FramePorts.cpp b/src/devices/openxrheadset/FramePorts.cpp
deleted file mode 100644
index 60c3b69..0000000
--- a/src/devices/openxrheadset/FramePorts.cpp
+++ /dev/null
@@ -1,172 +0,0 @@
-/*
- * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT)
- * All rights reserved.
- *
- * This software may be modified and distributed under the terms of the
- * BSD-2-Clause license. See the accompanying LICENSE file for details.
- */
-
-#include <FramePorts.h>
-#include <OpenXrHeadsetLogComponent.h>
-#include <OpenXrYarpUtilities.h>
-#include <yarp/os/LogStream.h>
-
-bool FramePorts::open(const std::string &name, const std::string &portPrefix, yarp::dev::IFrameTransform *tfPublisher, const std::string &tfFrame, const std::string &rootFrame)
-{
-    //opening ports
-    std::initializer_list<std::pair<yarp::os::BufferedPort<yarp::os::Bottle>**,
-                                    std::string>> ports =
-    {
-        { &m_orientationPort,                  "quaternion"                   },
-        { &m_positionPort,                     "position"                     },
-        { &m_angularVelocityPort,              "angularVelocity"              },
-        { &m_linearVelocityPort,               "linearVelocity"               }
-    };
-
-    for (auto port : ports)
-    {
-        if (*port.first)
-        {
-            yCError(OPENXRHEADSET) << port.second <<  "is already open.";
-            continue;
-        }
-
-        std::string portName;
-
-        *port.first = new yarp::os::BufferedPort<yarp::os::Bottle>;
-        portName        = portPrefix + "/" + port.second + ":o";
-
-        if (!(*port.first)->open(portName))
-        {
-            yCError(OPENXRHEADSET) << "Cannot open" << portName << "port";
-            close();
-            return false;
-        }
-
-        (*port.first)->setWriteOnly();
-    }
-
-    double timeNow = yarp::os::Time::now();
-    m_lastWarning["quaternion"] = timeNow - 10.0;
-    m_lastWarning["position"] = timeNow - 10.0;
-    m_lastWarning["angularVelocity"] = timeNow - 10.0;
-    m_lastWarning["linearVelocity"] = timeNow - 10.0;
-    m_lastWarning["republish"] = timeNow - 10.0;
-
-    m_name = name;
-    m_tfPublisher = tfPublisher;
-    m_tfFrame = tfFrame;
-    m_rootFrame = rootFrame;
-
-    m_localPose.resize(4,4);
-    m_localPose.eye();
-
-    return true;
-}
-
-void FramePorts::close()
-{
-    m_localPoseValid = false;
-
-    //Closing and deleting ports
-    std::initializer_list<yarp::os::BufferedPort<yarp::os::Bottle>**> ports =
-    {
-        &m_orientationPort,
-        &m_positionPort,
-        &m_angularVelocityPort,
-        &m_linearVelocityPort,
-    };
-
-    for (auto port : ports)
-    {
-        if (*port)
-        {
-            (*port)->close();
-
-            delete *port;
-
-            *port = nullptr;
-        }
-    }
-}
-
-void FramePorts::publishFrame(const OpenXrInterface::Pose &pose, const OpenXrInterface::Velocity &velocity, yarp::os::Stamp &stamp)
-{
-    if (pose.positionValid && pose.rotationValid)
-    {
-        poseToYarpMatrix(pose, m_localPose);
-        m_localPoseValid = true;
-        if (!m_tfPublisher->setTransform(m_tfFrame, m_rootFrame, m_localPose))
-        {
-            yCWarning(OPENXRHEADSET) << "Failed to publish" << m_tfFrame << "frame.";
-        }
-    }
-    else
-    {
-        if (m_localPoseValid)
-        {
-            if (!m_tfPublisher->setTransform(m_tfFrame, m_rootFrame, m_localPose))
-            {
-                yCWarning(OPENXRHEADSET) << "Failed to publish" << m_tfFrame << "frame.";
-            }
-
-            if (yarp::os::Time::now() - m_lastWarning["republish"] > 1.0)
-            {
-                yCWarning(OPENXRHEADSET) << "Publishing last" << m_name << "known pose.";
-                m_lastWarning["republish"] = yarp::os::Time::now();
-            }
-        }
-    }
-
-    if (pose.positionValid)
-    {
-        writeVec3OnPort(m_positionPort, pose.position, stamp);
-    }
-    else
-    {
-        if (yarp::os::Time::now() - m_lastWarning["position"] > 5.0)
-        {
-            yCWarning(OPENXRHEADSET) << m_name << "position not valid.";
-            m_lastWarning["position"] = yarp::os::Time::now();
-        }
-    }
-
-    if (pose.rotationValid)
-    {
-        writeQuaternionOnPort(m_orientationPort, pose.rotation, stamp);
-    }
-    else
-    {
-        if (yarp::os::Time::now() - m_lastWarning["quaternion"] > 5.0)
-        {
-            yCWarning(OPENXRHEADSET) << m_name << "rotation not valid.";
-            m_lastWarning["quaternion"] = yarp::os::Time::now();
-        }
-    }
-
-    if (velocity.linearValid)
-    {
-        writeVec3OnPort(m_linearVelocityPort, velocity.linear, stamp);
-    }
-    else
-    {
-        if (yarp::os::Time::now() - m_lastWarning["linearVelocity"] > 5.0)
-        {
-            yCWarning(OPENXRHEADSET) << m_name << "linear velocity not valid.";
-            m_lastWarning["linearVelocity"] = yarp::os::Time::now();
-        }
-    }
-
-    if (velocity.angularValid)
-    {
-        writeVec3OnPort(m_angularVelocityPort, velocity.angular, stamp);
-    }
-    else
-    {
-        if (yarp::os::Time::now() - m_lastWarning["angularVelocity"] > 5.0)
-        {
-            yCWarning(OPENXRHEADSET) << m_name << "angular velocity not valid.";
-            m_lastWarning["angularVelocity"] = yarp::os::Time::now();
-        }
-    }
-}
diff --git a/src/devices/openxrheadset/FramePorts.h b/src/devices/openxrheadset/FramePorts.h
deleted file mode 100644
index 506513a..0000000
--- a/src/devices/openxrheadset/FramePorts.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
- * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT)
- * All rights reserved.
- *
- * This software may be modified and distributed under the terms of the
- * BSD-2-Clause license. See the accompanying LICENSE file for details.
- */
-#ifndef YARP_DEV_FRAMEPORTS_H
-#define YARP_DEV_FRAMEPORTS_H
-
-#include <OpenXrInterface.h>
-#include <yarp/os/BufferedPort.h>
-#include <yarp/os/Bottle.h>
-#include <yarp/os/Stamp.h>
-#include <yarp/dev/IFrameTransform.h>
-#include <string>
-#include <unordered_map>
-
-class FramePorts
-{
-    yarp::os::BufferedPort<yarp::os::Bottle>* m_orientationPort{nullptr};
-    yarp::os::BufferedPort<yarp::os::Bottle>* m_positionPort{nullptr};
-    yarp::os::BufferedPort<yarp::os::Bottle>* m_angularVelocityPort{nullptr};
-    yarp::os::BufferedPort<yarp::os::Bottle>* m_linearVelocityPort{nullptr};
-
-    yarp::dev::IFrameTransform* m_tfPublisher;
-
-    std::string m_tfFrame;
-    std::string m_rootFrame;
-    std::string m_name;
-
-    std::unordered_map<std::string, double> m_lastWarning;
-
-    yarp::sig::Matrix m_localPose;
-    bool m_localPoseValid{false};
-
-
-public:
-
-    bool open(const std::string& name, const std::string& portPrefix,
-              yarp::dev::IFrameTransform* tfPublisher, const std::string& tfFrame, const std::string& rootFrame);
-
-    void close();
-
-    void publishFrame(const OpenXrInterface::Pose& pose,
-                      const OpenXrInterface::Velocity& velocity,
-                      yarp::os::Stamp &stamp);
-};
-
-#endif // YARP_DEV_FRAMEPORTS_H
diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp
index 1053a96..26d34dc 100644
--- a/src/devices/openxrheadset/OpenXrHeadset.cpp
+++ b/src/devices/openxrheadset/OpenXrHeadset.cpp
@@ -19,8 +19,7 @@ typedef bool(yarp::os::Value::*valueIsType)(void) const;
 
 yarp::dev::OpenXrHeadset::OpenXrHeadset()
     : yarp::dev::DeviceDriver(),
-      yarp::os::PeriodicThread(0.011, yarp::os::ShouldUseSystemClock::Yes), // ~90 fps
-      m_stamp(0,0.0)
+      yarp::os::PeriodicThread(0.011, yarp::os::ShouldUseSystemClock::Yes) // ~90 fps
 {}
 
 yarp::dev::OpenXrHeadset::~OpenXrHeadset()
@@ -218,11 +217,6 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
     m_openXrInterfaceSettings.posesPredictionInMs = cfg.check("vr_poses_prediction_in_ms", yarp::os::Value(0.0)).asFloat64();
 
     m_getStickAsAxis = cfg.check("stick_as_axis", yarp::os::Value(false)).asBool();
-    m_leftFrame = cfg.check("tf_left_hand_frame", yarp::os::Value("openxr_left_hand")).asString();
-    m_rightFrame = cfg.check("tf_right_hand_frame", yarp::os::Value("openxr_right_hand")).asString();
-    m_headFrame = cfg.check("tf_head_frame", yarp::os::Value("openxr_head")).asString();
-    m_eyesManager.options().leftEyeFrame = cfg.check("tf_left_eye_frame", yarp::os::Value("openxr_left_eye")).asString();
-    m_eyesManager.options().rightEyeFrame = cfg.check("tf_right_eye_frame", yarp::os::Value("openxr_right_eye")).asString();
     m_rootFrame = cfg.check("tf_root_frame", yarp::os::Value("openxr_origin")).asString();
     m_rootFrameRaw = m_rootFrame + "_raw";
     m_eyesManager.options().leftAzimuthOffset = cfg.check("left_azimuth_offset", yarp::os::Value(0.0)).asFloat64();
@@ -237,7 +231,7 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
     m_rawRootFrameTransform.resize(4,4);
     m_rawRootFrameTransform.eye();
 
-    std::vector<AdditionalPosesPublisher::Label> labels;
+    std::vector<PosesManager::Label> labels;
     yarp::os::Bottle& labelsGroup = cfg.findGroup("POSES_LABELS");
     for (size_t i = 1; i < labelsGroup.size(); ++i) //The first element is the name of the group itself
     {
@@ -271,21 +265,6 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
     }
     yCInfo(OPENXRHEADSET) << "TransformCLient successfully opened at port: " << cfg.find("tfLocal").asString();
 
-    if (!m_headFramePorts.open("Head", m_prefix + "/headpose", m_tfPublisher, m_headFrame, m_rootFrameRaw))
-    {
-        return false;
-    }
-
-    if (!m_leftHandFramePorts.open("Left Hand", m_prefix + "/left_hand", m_tfPublisher, m_leftFrame, m_rootFrameRaw))
-    {
-        return false;
-    }
-
-    if (!m_rightHandFramePorts.open("Right Hand", m_prefix + "/right_hand", m_tfPublisher, m_rightFrame, m_rootFrameRaw))
-    {
-        return false;
-    }
-
     PosePublisherSettings posePublisherSettings;
     posePublisherSettings.tfPublisher = m_tfPublisher;
     posePublisherSettings.rootFrame = m_rootFrameRaw;
@@ -302,7 +281,7 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
         yCWarning(OPENXRHEADSET) << "pose_check_convergence_ratio is supposed to be in the range [0, 1]. Clamping to" << posePublisherSettings.checks.convergenceRatio << ".";
     }
 
-    m_additionalPosesPublisher.initialize(labels, posePublisherSettings);
+    m_posesManager.initialize(labels, posePublisherSettings);
 
     // Start the thread
     if (!this->start()) {
@@ -334,7 +313,7 @@ bool yarp::dev::OpenXrHeadset::threadInit()
         m_eyesManager.options().leftEyeQuadLayer = m_openXrInterface.addHeadFixedQuadLayer();
         m_eyesManager.options().rightEyeQuadLayer = m_openXrInterface.addHeadFixedQuadLayer();
 
-        if (!m_eyesManager.initialize(m_tfPublisher, m_headFrame))
+        if (!m_eyesManager.initialize())
         {
             yCError(OPENXRHEADSET) << "Failed to initialize eyes.";
         }
@@ -427,9 +406,6 @@ void yarp::dev::OpenXrHeadset::threadRelease()
         m_tfPublisher = nullptr;
     }
 
-    m_headFramePorts.close();
-    m_leftHandFramePorts.close();
-    m_rightHandFramePorts.close();
     m_eyesManager.close();
 
     m_rpcPort.close();
@@ -474,9 +450,7 @@ void yarp::dev::OpenXrHeadset::run()
         m_openXrInterface.getButtons(m_buttons);
         m_openXrInterface.getAxes(m_axes);
         m_openXrInterface.getThumbsticks(m_thumbsticks);
-        m_openXrInterface.getAdditionalPoses(m_additionalPosesPublisher.inputs());
-
-        m_stamp.update(m_openXrInterface.currentNanosecondsSinceEpoch() * 1e-9);
+        m_openXrInterface.getAllPoses(m_posesManager.inputs());
 
         if (m_openXrInterface.shouldResetLocalReferenceSpace())
         {
@@ -490,13 +464,7 @@ void yarp::dev::OpenXrHeadset::run()
             yCWarning(OPENXRHEADSET) << "Failed to update the transformation of the raw root frame.";
         }
 
-        m_headFramePorts.publishFrame(m_openXrInterface.headPose(), m_openXrInterface.headVelocity(), m_stamp);
-        m_leftHandFramePorts.publishFrame(m_openXrInterface.leftHandPose(), m_openXrInterface.leftHandVelocity(), m_stamp);
-        m_rightHandFramePorts.publishFrame(m_openXrInterface.rightHandPose(), m_openXrInterface.rightHandVelocity(), m_stamp);
-
-        m_eyesManager.publishEyesTransforms();
-
-        m_additionalPosesPublisher.publishFrames();
+        m_posesManager.publishFrames();
     }
     else
     {
diff --git a/src/devices/openxrheadset/OpenXrHeadset.h b/src/devices/openxrheadset/OpenXrHeadset.h
index b7b95be..a7a1801 100644
--- a/src/devices/openxrheadset/OpenXrHeadset.h
+++ b/src/devices/openxrheadset/OpenXrHeadset.h
@@ -29,8 +29,7 @@
 #include <LabelPortToQuadLayer.h>
 #include <EyesManager.h>
 #include <SlideManager.h>
-#include <FramePorts.h>
-#include <AdditionalPosesPublisher.h>
+#include <PosesManager.h>
 #include <thrifts/OpenXrHeadsetCommands.h>
 
 #include <Eigen/Core>
@@ -236,13 +235,7 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
         SlideManager layer;
     };
 
-    FramePorts m_headFramePorts;
-    FramePorts m_leftHandFramePorts;
-    FramePorts m_rightHandFramePorts;
-
-    AdditionalPosesPublisher m_additionalPosesPublisher;
-
-    yarp::os::Stamp m_stamp;
+    PosesManager m_posesManager;
 
     std::string m_prefix;
 
@@ -255,9 +248,6 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
     bool m_getStickAsAxis;
 
     IFrameTransform* m_tfPublisher;
-    std::string      m_leftFrame;
-    std::string      m_rightFrame;
-    std::string      m_headFrame;
     std::string      m_rootFrameRaw;
     std::string      m_rootFrame;
     PolyDriver       m_driver;
diff --git a/src/devices/openxrheadset/OpenXrInterface.cpp b/src/devices/openxrheadset/OpenXrInterface.cpp
index 7c9e338..96fbb56 100644
--- a/src/devices/openxrheadset/OpenXrInterface.cpp
+++ b/src/devices/openxrheadset/OpenXrInterface.cpp
@@ -1558,9 +1558,10 @@ void OpenXrInterface::getThumbsticks(std::vector<Eigen::Vector2f> &thumbsticks)
     }
 }
 
-void OpenXrInterface::getAdditionalPoses(std::vector<NamedPoseVelocity> &additionalPoses) const
+void OpenXrInterface::getAllPoses(std::vector<NamedPoseVelocity> &additionalPoses) const
 {
     size_t numberOfPoses = 0;
+    numberOfPoses += 3; //This is because the head and the hands are added manually
     for (size_t topLevelIndex = 0; topLevelIndex <  m_pimpl->top_level_paths.size(); ++topLevelIndex)
     {
         if (topLevelIndex < 2) //Left and right hand are already considered with leftHandPose() and rightHandPose()
@@ -1580,6 +1581,24 @@ void OpenXrInterface::getAdditionalPoses(std::vector<NamedPoseVelocity> &additio
 
     size_t poseIndex = 0;
 
+    auto& head = additionalPoses[poseIndex];
+    head.name = "openxr_head";
+    head.pose = headPose();
+    head.velocity = headVelocity();
+    poseIndex++;
+
+    auto& left_arm = additionalPoses[poseIndex];
+    left_arm.name = "openxr_left_hand";
+    left_arm.pose = leftHandPose();
+    left_arm.velocity = leftHandVelocity();
+    poseIndex++;
+
+    auto& right_arm = additionalPoses[poseIndex];
+    right_arm.name = "openxr_right_hand";
+    right_arm.pose = rightHandPose();
+    right_arm.velocity = rightHandVelocity();
+    poseIndex++;
+
     for (size_t topLevelIndex = 0; topLevelIndex <  m_pimpl->top_level_paths.size(); ++topLevelIndex)
     {
         std::vector<PoseAction>& posesList = m_pimpl->top_level_paths[topLevelIndex].currentActions().poses;
diff --git a/src/devices/openxrheadset/OpenXrInterface.h b/src/devices/openxrheadset/OpenXrInterface.h
index 92880b7..36ec920 100644
--- a/src/devices/openxrheadset/OpenXrInterface.h
+++ b/src/devices/openxrheadset/OpenXrInterface.h
@@ -185,7 +185,7 @@ class OpenXrInterface
 
     void getThumbsticks(std::vector<Eigen::Vector2f>& thumbsticks) const;
 
-    void getAdditionalPoses(std::vector<NamedPoseVelocity> &additionalPoses) const;
+    void getAllPoses(std::vector<NamedPoseVelocity> &additionalPoses) const;
 
     int64_t currentNanosecondsSinceEpoch() const;
 
diff --git a/src/devices/openxrheadset/AdditionalPosesPublisher.cpp b/src/devices/openxrheadset/PosesManager.cpp
similarity index 54%
rename from src/devices/openxrheadset/AdditionalPosesPublisher.cpp
rename to src/devices/openxrheadset/PosesManager.cpp
index 17fe229..bf02463 100644
--- a/src/devices/openxrheadset/AdditionalPosesPublisher.cpp
+++ b/src/devices/openxrheadset/PosesManager.cpp
@@ -6,31 +6,31 @@
  * BSD-2-Clause license. See the accompanying LICENSE file for details.
  */
 
-#include <AdditionalPosesPublisher.h>
+#include <PosesManager.h>
 #include <OpenXrHeadsetLogComponent.h>
 #include <OpenXrYarpUtilities.h>
 #include <yarp/os/LogStream.h>
 
-void AdditionalPosesPublisher::initialize(const std::vector<Label>& labels, const PosePublisherSettings &settings)
+void PosesManager::initialize(const std::vector<Label>& labels, const PosePublisherSettings &settings)
 {
     m_settings = std::make_shared<PosePublisherSettings>(settings);
 
     for (auto& label : labels)
     {
-        m_additionalPoses[label.original].setLabel(label.modified);
+        m_poses[label.original].setLabel(label.modified);
     }
 }
 
-std::vector<OpenXrInterface::NamedPoseVelocity> &AdditionalPosesPublisher::inputs()
+std::vector<OpenXrInterface::NamedPoseVelocity> &PosesManager::inputs()
 {
-    return m_additionalPosesInputList;
+    return m_posesInputList;
 }
 
-void AdditionalPosesPublisher::publishFrames()
+void PosesManager::publishFrames()
 {
-    for (auto& inputPose : m_additionalPosesInputList)
+    for (auto& inputPose : m_posesInputList)
     {
-        PosePublisher& publisher = m_additionalPoses[inputPose.name];
+        PosePublisher& publisher = m_poses[inputPose.name];
 
         if (!publisher.configured())
         {
@@ -40,7 +40,7 @@ void AdditionalPosesPublisher::publishFrames()
         publisher.update(inputPose);
     }
 
-    for (auto& poseIt : m_additionalPoses)
+    for (auto& poseIt : m_poses)
     {
         poseIt.second.publish();
     }
diff --git a/src/devices/openxrheadset/AdditionalPosesPublisher.h b/src/devices/openxrheadset/PosesManager.h
similarity index 69%
rename from src/devices/openxrheadset/AdditionalPosesPublisher.h
rename to src/devices/openxrheadset/PosesManager.h
index c8b3e1e..7d67c22 100644
--- a/src/devices/openxrheadset/AdditionalPosesPublisher.h
+++ b/src/devices/openxrheadset/PosesManager.h
@@ -6,21 +6,21 @@
  * BSD-2-Clause license. See the accompanying LICENSE file for details.
  */
 
-#ifndef YARP_DEV_ADDITIONALPOSESPUBLISHER_H
-#define YARP_DEV_ADDITIONALPOSESPUBLISHER_H
+#ifndef YARP_DEV_POSESMANAGER_H
+#define YARP_DEV_POSESMANAGER_H
 
 #include <OpenXrInterface.h>
 #include <PosePublisher.h>
 #include <yarp/dev/IFrameTransform.h>
 #include <string>
 
-class AdditionalPosesPublisher
+class PosesManager
 {
     std::shared_ptr<PosePublisherSettings> m_settings{nullptr};
 
-    std::vector<OpenXrInterface::NamedPoseVelocity> m_additionalPosesInputList;
+    std::vector<OpenXrInterface::NamedPoseVelocity> m_posesInputList;
 
-    std::unordered_map<std::string, PosePublisher> m_additionalPoses;
+    std::unordered_map<std::string, PosePublisher> m_poses;
 
 public:
 
@@ -40,4 +40,4 @@ class AdditionalPosesPublisher
 };
 
 
-#endif // YARP_DEV_ADDITIONALPOSESPUBLISHER_H
+#endif // YARP_DEV_POSESMANAGER_H
diff --git a/src/devices/openxrheadset/SingleEyePort.cpp b/src/devices/openxrheadset/SingleEyePort.cpp
index 9789ba5..b9fef50 100644
--- a/src/devices/openxrheadset/SingleEyePort.cpp
+++ b/src/devices/openxrheadset/SingleEyePort.cpp
@@ -27,8 +27,7 @@ bool SingleEyePort::updateControlAngles()
     return true;
 }
 
-bool SingleEyePort::open(std::shared_ptr<IOpenXrQuadLayer> quadLayer, const std::string &anglesPortName,
-                   yarp::dev::IFrameTransform *tfPublisher, const std::string &tfFrame, const std::string &rootFrame)
+bool SingleEyePort::open(std::shared_ptr<IOpenXrQuadLayer> quadLayer, const std::string &anglesPortName)
 {
     m_initialized = false;
     if (!m_layer.initialize(quadLayer)) {
@@ -44,11 +43,6 @@ bool SingleEyePort::open(std::shared_ptr<IOpenXrQuadLayer> quadLayer, const std:
 
     m_eyeAnglesPort.setReadOnly();
 
-    if (!tfPublisher)
-    {
-        yCError(OPENXRHEADSET) << "The transform server interface is not valid.";
-        return false;
-    }
 
     m_desiredRotation.setIdentity();
     m_rotationOffset.setIdentity();
@@ -56,13 +50,6 @@ bool SingleEyePort::open(std::shared_ptr<IOpenXrQuadLayer> quadLayer, const std:
     m_eyeRelativeImagePosition[2] = -1.0; //Initialize the z position of the camera image visualization at 1m distance
     m_eyePosition.setZero();
 
-    m_localPose.resize(4,4);
-    m_localPose.eye();
-
-    m_tfPublisher = tfPublisher;
-    m_tfFrame = tfFrame;
-    m_rootFrame = rootFrame;
-
     m_initialized = true;
 
     return true;
@@ -77,7 +64,6 @@ void SingleEyePort::close()
 {
     m_initialized = false;
     m_eyeAnglesPort.close();
-    m_tfPublisher = nullptr;
     m_layer.close();
 }
 
@@ -94,21 +80,6 @@ void SingleEyePort::setEyePosition(const Eigen::Vector3f &position)
     m_layer.setPosition(m_rotationOffset * m_desiredRotation * m_eyeRelativeImagePosition + m_eyePosition);
 }
 
-void SingleEyePort::publishEyeTransform()
-{
-    if (!m_initialized)
-    {
-        yCError(OPENXRHEADSET) << "Eye port not initialized.";
-        return;
-    }
-
-    poseToYarpMatrix(m_layer.layerPosition(), m_layer.layerQuaternion(), m_localPose);
-    if (!m_tfPublisher->setTransform(m_tfFrame, m_rootFrame, m_localPose))
-    {
-        yCWarning(OPENXRHEADSET) << "Failed to publish" << m_tfFrame << "frame.";
-    }
-}
-
 bool SingleEyePort::active() const
 {
     return m_initialized && m_layer.active();
diff --git a/src/devices/openxrheadset/SingleEyePort.h b/src/devices/openxrheadset/SingleEyePort.h
index acf1249..26e8528 100644
--- a/src/devices/openxrheadset/SingleEyePort.h
+++ b/src/devices/openxrheadset/SingleEyePort.h
@@ -9,11 +9,8 @@
 #ifndef YARP_DEV_EYEPORT_H
 #define YARP_DEV_EYEPORT_H
 
-#include <yarp/dev/IFrameTransform.h>
 #include <yarp/sig/Image.h>
-#include <yarp/sig/Matrix.h>
 #include <yarp/sig/Vector.h>
-#include <yarp/os/Stamp.h>
 #include <yarp/os/BufferedPort.h>
 #include <OpenXrInterface.h>
 #include <ImagePortToQuadLayer.h>
@@ -23,10 +20,6 @@
 
 class SingleEyePort
 {
-    yarp::sig::Matrix m_localPose;
-    yarp::dev::IFrameTransform* m_tfPublisher{nullptr};
-    std::string m_tfFrame;
-    std::string m_rootFrame;
     float m_azimuthOffset {0.0};
     float m_elevationOffset {0.0};
     Eigen::Quaternionf m_desiredRotation;
@@ -41,8 +34,7 @@ class SingleEyePort
 
 public:
 
-    bool open(std::shared_ptr<IOpenXrQuadLayer> quadLayer, const std::string& anglesPortName,
-              yarp::dev::IFrameTransform* tfPublisher, const std::string& tfFrame, const std::string& rootFrame);
+    bool open(std::shared_ptr<IOpenXrQuadLayer> quadLayer, const std::string& anglesPortName);
 
     bool openImagePort(const std::string& imagePortName);
 
@@ -70,8 +62,6 @@ class SingleEyePort
 
     bool update(const yarp::sig::ImageOf<yarp::sig::PixelRgb>& img, GLint startX, GLint startY, GLint endX, GLint endY);
 
-    void publishEyeTransform();
-
     bool active() const;
 
     std::string controlPortName() const;

From 5e912a8bc7d59cd6813ee7b1ce693896b2cbed6e Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 22 Sep 2022 09:48:21 +0200
Subject: [PATCH 02/14] Renamed PosePublisher to FilteredPosePublisher.

---
 src/devices/openxrheadset/CMakeLists.txt      |  4 +--
 ...ublisher.cpp => FilteredPosePublisher.cpp} | 30 +++++++++----------
 ...osePublisher.h => FilteredPosePublisher.h} | 16 +++++-----
 src/devices/openxrheadset/OpenXrHeadset.cpp   |  2 +-
 src/devices/openxrheadset/PosesManager.cpp    |  6 ++--
 src/devices/openxrheadset/PosesManager.h      |  8 ++---
 6 files changed, 33 insertions(+), 33 deletions(-)
 rename src/devices/openxrheadset/{PosePublisher.cpp => FilteredPosePublisher.cpp} (88%)
 rename src/devices/openxrheadset/{PosePublisher.h => FilteredPosePublisher.h} (80%)

diff --git a/src/devices/openxrheadset/CMakeLists.txt b/src/devices/openxrheadset/CMakeLists.txt
index 1074791..1364be4 100644
--- a/src/devices/openxrheadset/CMakeLists.txt
+++ b/src/devices/openxrheadset/CMakeLists.txt
@@ -51,7 +51,7 @@ set(yarp_openxrheadset_driver_SRCS
     SingleEyePort.cpp
     EyesManager.cpp
     PosesManager.cpp
-    PosePublisher.cpp
+    FilteredPosePublisher.cpp
 )
 
 set(yarp_openxrheadset_driver_HDRS
@@ -60,7 +60,7 @@ set(yarp_openxrheadset_driver_HDRS
     SingleEyePort.h
     EyesManager.h
     PosesManager.h
-    PosePublisher.h
+    FilteredPosePublisher.h
 )
 
 set (THRIFTS thrifts/OpenXrHeadsetCommands.thrift)
diff --git a/src/devices/openxrheadset/PosePublisher.cpp b/src/devices/openxrheadset/FilteredPosePublisher.cpp
similarity index 88%
rename from src/devices/openxrheadset/PosePublisher.cpp
rename to src/devices/openxrheadset/FilteredPosePublisher.cpp
index ea8d4b5..ca90d77 100644
--- a/src/devices/openxrheadset/PosePublisher.cpp
+++ b/src/devices/openxrheadset/FilteredPosePublisher.cpp
@@ -6,12 +6,12 @@
  * BSD-2-Clause license. See the accompanying LICENSE file for details.
  */
 
-#include <PosePublisher.h>
+#include <FilteredPosePublisher.h>
 #include <OpenXrHeadsetLogComponent.h>
 #include <OpenXrYarpUtilities.h>
 #include <yarp/os/LogStream.h>
 
-bool PosePublisher::positionJumped()
+bool FilteredPosePublisher::positionJumped()
 {
     if (!m_lastValidData.pose.positionValid || !m_data.pose.positionValid)
     {
@@ -29,7 +29,7 @@ bool PosePublisher::positionJumped()
     return (expectedPosition - m_data.pose.position).norm() > m_settings->checks.maxDistance;
 }
 
-bool PosePublisher::rotationJumped()
+bool FilteredPosePublisher::rotationJumped()
 {
     if (!m_lastValidData.pose.rotationValid || !m_data.pose.rotationValid)
     {
@@ -58,7 +58,7 @@ bool PosePublisher::rotationJumped()
     return std::abs(actualRotationDifference.angularDistance(expectedRotationDifference)) > m_settings->checks.maxAngularDistanceInRad;
 }
 
-void PosePublisher::filterJumps()
+void FilteredPosePublisher::filterJumps()
 {
     bool positionHasJumped = positionJumped();
     bool rotationHasJumped = rotationJumped();
@@ -104,13 +104,13 @@ void PosePublisher::filterJumps()
     }
 }
 
-void PosePublisher::resetWarnings()
+void FilteredPosePublisher::resetWarnings()
 {
     m_lastWarningTime = 0.0;
     m_warningCount = 0;
 }
 
-void PosePublisher::resetLastValidData()
+void FilteredPosePublisher::resetLastValidData()
 {
     //Invalidate previous pose
     m_lastValidData.pose.positionValid = false;
@@ -119,7 +119,7 @@ void PosePublisher::resetLastValidData()
     m_convergingToJump = false;
 }
 
-void PosePublisher::publishOldTransform()
+void FilteredPosePublisher::publishOldTransform()
 {
     if (!m_publishedOnce)
     {
@@ -146,7 +146,7 @@ void PosePublisher::publishOldTransform()
     }
 }
 
-void PosePublisher::publishNewTransform()
+void FilteredPosePublisher::publishNewTransform()
 {
     if (!m_publishedOnce)
     {
@@ -168,7 +168,7 @@ void PosePublisher::publishNewTransform()
     m_data.pose.rotationValid = false;
 }
 
-void PosePublisher::deactivate()
+void FilteredPosePublisher::deactivate()
 {
     resetWarnings();
     resetLastValidData();
@@ -176,28 +176,28 @@ void PosePublisher::deactivate()
     m_active = false;
 }
 
-PosePublisher::PosePublisher()
+FilteredPosePublisher::FilteredPosePublisher()
 {
     m_localPose.resize(4,4);
     m_localPose.eye();
 }
 
-void PosePublisher::setLabel(const std::string &label)
+void FilteredPosePublisher::setLabel(const std::string &label)
 {
     m_label = label;
 }
 
-void PosePublisher::configure(std::shared_ptr<PosePublisherSettings> settings)
+void FilteredPosePublisher::configure(std::shared_ptr<FilteredPosePublisherSettings> settings)
 {
     m_settings = settings;
 }
 
-bool PosePublisher::configured() const
+bool FilteredPosePublisher::configured() const
 {
     return m_settings != nullptr;
 }
 
-void PosePublisher::update(const OpenXrInterface::NamedPoseVelocity &input)
+void FilteredPosePublisher::update(const OpenXrInterface::NamedPoseVelocity &input)
 {
     if (!configured())
     {
@@ -210,7 +210,7 @@ void PosePublisher::update(const OpenXrInterface::NamedPoseVelocity &input)
     m_active = !wasActive || inputValid; //We are active if: - it is the first time we update, - we were active the step before, - we received a valid input
 }
 
-void PosePublisher::publish()
+void FilteredPosePublisher::publish()
 {
     if (!configured() || !m_active)
     {
diff --git a/src/devices/openxrheadset/PosePublisher.h b/src/devices/openxrheadset/FilteredPosePublisher.h
similarity index 80%
rename from src/devices/openxrheadset/PosePublisher.h
rename to src/devices/openxrheadset/FilteredPosePublisher.h
index 6921ee5..3f66a46 100644
--- a/src/devices/openxrheadset/PosePublisher.h
+++ b/src/devices/openxrheadset/FilteredPosePublisher.h
@@ -6,15 +6,15 @@
  * BSD-2-Clause license. See the accompanying LICENSE file for details.
  */
 
-#ifndef YARP_DEV_POSEPUBLISHER_H
-#define YARP_DEV_POSEPUBLISHER_H
+#ifndef YARP_DEV_FILTEREDPOSEPUBLISHER_H
+#define YARP_DEV_FILTEREDPOSEPUBLISHER_H
 
 #include <OpenXrInterface.h>
 #include <yarp/dev/IFrameTransform.h>
 #include <string>
 #include <memory>
 
-struct PosePublisherSettings
+struct FilteredPosePublisherSettings
 {
     yarp::dev::IFrameTransform* tfPublisher;
     std::string rootFrame;
@@ -32,7 +32,7 @@ struct PosePublisherSettings
     ValidityChecks checks;
 };
 
-class PosePublisher
+class FilteredPosePublisher
 {
 
     std::string m_label;
@@ -43,7 +43,7 @@ class PosePublisher
     bool m_active{false};
     OpenXrInterface::NamedPoseVelocity m_data;
     OpenXrInterface::NamedPoseVelocity m_lastValidData;
-    std::shared_ptr<PosePublisherSettings> m_settings{nullptr};
+    std::shared_ptr<FilteredPosePublisherSettings> m_settings{nullptr};
     double m_lastValidDataTime;
     bool m_convergingToJump{false};
 
@@ -64,11 +64,11 @@ class PosePublisher
     void deactivate();
 
 public:
-    PosePublisher();
+    FilteredPosePublisher();
 
     void setLabel(const std::string& label);
 
-    void configure(std::shared_ptr<PosePublisherSettings> settings);
+    void configure(std::shared_ptr<FilteredPosePublisherSettings> settings);
 
     bool configured() const;
 
@@ -77,4 +77,4 @@ class PosePublisher
     void publish();
 };
 
-#endif // YARP_DEV_POSEPUBLISHER_H
+#endif // YARP_DEV_FILTEREDPOSEPUBLISHER_H
diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp
index 26d34dc..34e9798 100644
--- a/src/devices/openxrheadset/OpenXrHeadset.cpp
+++ b/src/devices/openxrheadset/OpenXrHeadset.cpp
@@ -265,7 +265,7 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
     }
     yCInfo(OPENXRHEADSET) << "TransformCLient successfully opened at port: " << cfg.find("tfLocal").asString();
 
-    PosePublisherSettings posePublisherSettings;
+    FilteredPosePublisherSettings posePublisherSettings;
     posePublisherSettings.tfPublisher = m_tfPublisher;
     posePublisherSettings.rootFrame = m_rootFrameRaw;
     posePublisherSettings.period = getPeriod();
diff --git a/src/devices/openxrheadset/PosesManager.cpp b/src/devices/openxrheadset/PosesManager.cpp
index bf02463..682b766 100644
--- a/src/devices/openxrheadset/PosesManager.cpp
+++ b/src/devices/openxrheadset/PosesManager.cpp
@@ -11,9 +11,9 @@
 #include <OpenXrYarpUtilities.h>
 #include <yarp/os/LogStream.h>
 
-void PosesManager::initialize(const std::vector<Label>& labels, const PosePublisherSettings &settings)
+void PosesManager::initialize(const std::vector<Label>& labels, const FilteredPosePublisherSettings &settings)
 {
-    m_settings = std::make_shared<PosePublisherSettings>(settings);
+    m_settings = std::make_shared<FilteredPosePublisherSettings>(settings);
 
     for (auto& label : labels)
     {
@@ -30,7 +30,7 @@ void PosesManager::publishFrames()
 {
     for (auto& inputPose : m_posesInputList)
     {
-        PosePublisher& publisher = m_poses[inputPose.name];
+        FilteredPosePublisher& publisher = m_poses[inputPose.name];
 
         if (!publisher.configured())
         {
diff --git a/src/devices/openxrheadset/PosesManager.h b/src/devices/openxrheadset/PosesManager.h
index 7d67c22..ce8e94c 100644
--- a/src/devices/openxrheadset/PosesManager.h
+++ b/src/devices/openxrheadset/PosesManager.h
@@ -10,17 +10,17 @@
 #define YARP_DEV_POSESMANAGER_H
 
 #include <OpenXrInterface.h>
-#include <PosePublisher.h>
+#include <FilteredPosePublisher.h>
 #include <yarp/dev/IFrameTransform.h>
 #include <string>
 
 class PosesManager
 {
-    std::shared_ptr<PosePublisherSettings> m_settings{nullptr};
+    std::shared_ptr<FilteredPosePublisherSettings> m_settings{nullptr};
 
     std::vector<OpenXrInterface::NamedPoseVelocity> m_posesInputList;
 
-    std::unordered_map<std::string, PosePublisher> m_poses;
+    std::unordered_map<std::string, FilteredPosePublisher> m_poses;
 
 public:
 
@@ -31,7 +31,7 @@ class PosesManager
     };
 
     void initialize(const std::vector<Label> &labels,
-                    const PosePublisherSettings &settings);
+                    const FilteredPosePublisherSettings &settings);
 
     std::vector<OpenXrInterface::NamedPoseVelocity>& inputs();
 

From 9f2c2c0cff3c855e77fb003bb674642c7b495ded Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 22 Sep 2022 14:24:52 +0200
Subject: [PATCH 03/14] Refactored PosePublisher class

---
 src/devices/openxrheadset/CMakeLists.txt      |   2 +
 .../openxrheadset/FilteredPosePublisher.cpp   | 134 ++++--------------
 .../openxrheadset/FilteredPosePublisher.h     |  38 ++---
 src/devices/openxrheadset/PosePublisher.cpp   | 114 +++++++++++++++
 src/devices/openxrheadset/PosePublisher.h     |  59 ++++++++
 src/devices/openxrheadset/PosesManager.cpp    |   2 +-
 6 files changed, 212 insertions(+), 137 deletions(-)
 create mode 100644 src/devices/openxrheadset/PosePublisher.cpp
 create mode 100644 src/devices/openxrheadset/PosePublisher.h

diff --git a/src/devices/openxrheadset/CMakeLists.txt b/src/devices/openxrheadset/CMakeLists.txt
index 1364be4..0027593 100644
--- a/src/devices/openxrheadset/CMakeLists.txt
+++ b/src/devices/openxrheadset/CMakeLists.txt
@@ -51,6 +51,7 @@ set(yarp_openxrheadset_driver_SRCS
     SingleEyePort.cpp
     EyesManager.cpp
     PosesManager.cpp
+    PosePublisher.cpp
     FilteredPosePublisher.cpp
 )
 
@@ -60,6 +61,7 @@ set(yarp_openxrheadset_driver_HDRS
     SingleEyePort.h
     EyesManager.h
     PosesManager.h
+    PosePublisher.h
     FilteredPosePublisher.h
 )
 
diff --git a/src/devices/openxrheadset/FilteredPosePublisher.cpp b/src/devices/openxrheadset/FilteredPosePublisher.cpp
index ca90d77..7cb9d95 100644
--- a/src/devices/openxrheadset/FilteredPosePublisher.cpp
+++ b/src/devices/openxrheadset/FilteredPosePublisher.cpp
@@ -11,34 +11,34 @@
 #include <OpenXrYarpUtilities.h>
 #include <yarp/os/LogStream.h>
 
-bool FilteredPosePublisher::positionJumped()
+bool FilteredPosePublisher::positionJumped(const OpenXrInterface::NamedPoseVelocity& input)
 {
-    if (!m_lastValidData.pose.positionValid || !m_data.pose.positionValid)
+    if (!m_lastValidData.pose.positionValid || !input.pose.positionValid)
     {
         return false; //Also the last "valid" pose is not valid, or the input is already not valid
     }
 
     if (!m_lastValidData.velocity.linearValid)
     {
-        yCWarning(OPENXRHEADSET) << "The last valid pose of" << m_label << "has non-valid linear velocity. Assuming there are jumps.";
+        yCWarning(OPENXRHEADSET) << "The last valid pose of" << label() << "has non-valid linear velocity. Assuming there are jumps.";
         return true;
     }
 
     Eigen::Vector3f expectedPosition = m_lastValidData.pose.position + m_settings->period * m_lastValidData.velocity.linear;
 
-    return (expectedPosition - m_data.pose.position).norm() > m_settings->checks.maxDistance;
+    return (expectedPosition - input.pose.position).norm() > m_settings->checks.maxDistance;
 }
 
-bool FilteredPosePublisher::rotationJumped()
+bool FilteredPosePublisher::rotationJumped(const OpenXrInterface::NamedPoseVelocity &input)
 {
-    if (!m_lastValidData.pose.rotationValid || !m_data.pose.rotationValid)
+    if (!m_lastValidData.pose.rotationValid || !input.pose.rotationValid)
     {
         return false; //Also the last "valid" pose is not valid, or the input is already not valid
     }
 
     if (!m_lastValidData.velocity.angularValid)
     {
-        yCWarning(OPENXRHEADSET) << "The last valid pose of" << m_label << "has non-valid angular velocity. Assuming there are jumps.";
+        yCWarning(OPENXRHEADSET) << "The last valid pose of" << label() << "has non-valid angular velocity. Assuming there are jumps.";
         return true;
     }
 
@@ -53,15 +53,16 @@ bool FilteredPosePublisher::rotationJumped()
     Eigen::Quaternionf expectedRotationDifference;
     expectedRotationDifference = Eigen::AngleAxisf(m_settings->period * angularVelocity, angularAxis);
 
-    Eigen::Quaternionf actualRotationDifference = m_data.pose.rotation * m_lastValidData.pose.rotation.inverse();
+    Eigen::Quaternionf actualRotationDifference = input.pose.rotation * m_lastValidData.pose.rotation.inverse();
 
     return std::abs(actualRotationDifference.angularDistance(expectedRotationDifference)) > m_settings->checks.maxAngularDistanceInRad;
 }
 
-void FilteredPosePublisher::filterJumps()
+OpenXrInterface::NamedPoseVelocity FilteredPosePublisher::filterJumps(const OpenXrInterface::NamedPoseVelocity &input)
 {
-    bool positionHasJumped = positionJumped();
-    bool rotationHasJumped = rotationJumped();
+    bool positionHasJumped = positionJumped(input);
+    bool rotationHasJumped = rotationJumped(input);
+    OpenXrInterface::NamedPoseVelocity output{input};
 
     double interpolationFactor = 0.0;
 
@@ -76,7 +77,7 @@ void FilteredPosePublisher::filterJumps()
         {
             if (!m_convergingToJump) //Print the warning only once
             {
-                yCWarning(OPENXRHEADSET) << m_label << "last valid data has expired. The pose will converge to the measured one.";
+                yCWarning(OPENXRHEADSET) << label() << "last valid data has expired. The pose will converge to the measured one.";
             }
             m_convergingToJump = true;
             interpolationFactor = m_settings->checks.convergenceRatio;
@@ -85,29 +86,25 @@ void FilteredPosePublisher::filterJumps()
 
     if (positionHasJumped)
     {
-        yCWarning(OPENXRHEADSET) << m_label << "position had a jump.";
-        m_data.pose.position = (1 - interpolationFactor) * m_lastValidData.pose.position + interpolationFactor * m_data.pose.position;
+        yCWarning(OPENXRHEADSET) << label() << "position had a jump.";
+        output.pose.position = (1 - interpolationFactor) * m_lastValidData.pose.position + interpolationFactor * output.pose.position;
     }
 
     if (rotationHasJumped)
     {
-        yCWarning(OPENXRHEADSET) << m_label << "rotation had a jump.";
-        m_data.pose.rotation = m_lastValidData.pose.rotation.slerp(interpolationFactor, m_data.pose.rotation);
+        yCWarning(OPENXRHEADSET) << label() << "rotation had a jump.";
+        output.pose.rotation = m_lastValidData.pose.rotation.slerp(interpolationFactor, output.pose.rotation);
     }
 
-    m_lastValidData = m_data;
+    m_lastValidData = output;
 
     if ((yarp::os::Time::now() - m_lastValidDataTime) > (m_settings->checks.lastDataExpirationTime + m_settings->checks.maxConvergenceTime))
     {
-        yCWarning(OPENXRHEADSET) << m_label << "last valid convergence has expired. The pose will be aligned to the measured one.";
+        yCWarning(OPENXRHEADSET) << label() << "last valid convergence has expired. The pose will be aligned to the measured one.";
         resetLastValidData();
     }
-}
 
-void FilteredPosePublisher::resetWarnings()
-{
-    m_lastWarningTime = 0.0;
-    m_warningCount = 0;
+    return output;
 }
 
 void FilteredPosePublisher::resetLastValidData()
@@ -119,115 +116,36 @@ void FilteredPosePublisher::resetLastValidData()
     m_convergingToJump = false;
 }
 
-void FilteredPosePublisher::publishOldTransform()
-{
-    if (!m_publishedOnce)
-    {
-        return;
-    }
-
-    //Publish old pose
-    if (!m_settings->tfPublisher->setTransform(m_label, m_settings->rootFrame, m_localPose))
-    {
-        yCWarning(OPENXRHEADSET) << "Failed to publish" << m_label << "frame.";
-    }
-
-    if (m_warningCount == 0 || yarp::os::Time::now() - m_lastWarningTime > 5.0)
-    {
-        yCWarning(OPENXRHEADSET) << m_label << " is not valid. Publishing its last known pose.";
-        m_lastWarningTime = yarp::os::Time::now();
-        m_warningCount++;
-    }
-
-    if (m_warningCount > 6)
-    {
-        yCWarning(OPENXRHEADSET) << m_label << " was not valid for 30s. Deactivated.";
-        deactivate();
-    }
-}
-
-void FilteredPosePublisher::publishNewTransform()
-{
-    if (!m_publishedOnce)
-    {
-        if (m_label.empty())
-        {
-            m_label = m_data.name;
-        }
-        m_publishedOnce = true;
-    }
-
-    poseToYarpMatrix(m_data.pose, m_localPose);
-
-    if (!m_settings->tfPublisher->setTransform(m_label, m_settings->rootFrame, m_localPose))
-    {
-        yCWarning(OPENXRHEADSET) << "Failed to publish" << m_label << "frame.";
-    }
-
-    m_data.pose.positionValid = false; //We invalidate the data after use. This is to detect if some pose do not get updated.
-    m_data.pose.rotationValid = false;
-}
-
 void FilteredPosePublisher::deactivate()
 {
-    resetWarnings();
+    PosePublisher::deactivate();
     resetLastValidData();
-
-    m_active = false;
-}
-
-FilteredPosePublisher::FilteredPosePublisher()
-{
-    m_localPose.resize(4,4);
-    m_localPose.eye();
-}
-
-void FilteredPosePublisher::setLabel(const std::string &label)
-{
-    m_label = label;
 }
 
 void FilteredPosePublisher::configure(std::shared_ptr<FilteredPosePublisherSettings> settings)
 {
     m_settings = settings;
+    configurePublisher(m_settings);
 }
 
 bool FilteredPosePublisher::configured() const
 {
-    return m_settings != nullptr;
+    return m_settings != nullptr && PosePublisher::configured();
 }
 
-void FilteredPosePublisher::update(const OpenXrInterface::NamedPoseVelocity &input)
+void FilteredPosePublisher::updateInputPose(const OpenXrInterface::NamedPoseVelocity &input)
 {
     if (!configured())
     {
         return;
     }
 
-    m_data = input;
-    bool inputValid = m_data.pose.positionValid && m_data.pose.rotationValid; //we consider the input valid if both the position and the rotation are valid
-    bool wasActive = m_publishedOnce && !m_active; //if the pose was published once, but now it is not active, it means that there has been problems. So we should be active only if the input is valid
-    m_active = !wasActive || inputValid; //We are active if: - it is the first time we update, - we were active the step before, - we received a valid input
-}
-
-void FilteredPosePublisher::publish()
-{
-    if (!configured() || !m_active)
-    {
-        return;
-    }
-
-    bool inputValid = m_data.pose.positionValid && m_data.pose.rotationValid;
+    bool inputValid = input.pose.positionValid && input.pose.rotationValid;
 
     if (!inputValid)
     {
-        publishOldTransform();
         return;
     }
 
-    filterJumps();
-
-    resetWarnings();
-
-    publishNewTransform();
+    PosePublisher::updateInputPose(filterJumps(input));
 }
diff --git a/src/devices/openxrheadset/FilteredPosePublisher.h b/src/devices/openxrheadset/FilteredPosePublisher.h
index 3f66a46..c12a30e 100644
--- a/src/devices/openxrheadset/FilteredPosePublisher.h
+++ b/src/devices/openxrheadset/FilteredPosePublisher.h
@@ -13,11 +13,10 @@
 #include <yarp/dev/IFrameTransform.h>
 #include <string>
 #include <memory>
+#include <PosePublisher.h>
 
-struct FilteredPosePublisherSettings
+struct FilteredPosePublisherSettings : public PosePublisherSettings
 {
-    yarp::dev::IFrameTransform* tfPublisher;
-    std::string rootFrame;
     double period{0.01};
 
     struct ValidityChecks
@@ -32,49 +31,32 @@ struct FilteredPosePublisherSettings
     ValidityChecks checks;
 };
 
-class FilteredPosePublisher
+class FilteredPosePublisher : public PosePublisher
 {
 
-    std::string m_label;
-    double m_lastWarningTime{0.0};
-    size_t m_warningCount{0};
-    bool m_publishedOnce{false};
-    yarp::sig::Matrix m_localPose;
-    bool m_active{false};
-    OpenXrInterface::NamedPoseVelocity m_data;
+    OpenXrInterface::NamedPoseVelocity m_filteredData;
     OpenXrInterface::NamedPoseVelocity m_lastValidData;
     std::shared_ptr<FilteredPosePublisherSettings> m_settings{nullptr};
     double m_lastValidDataTime;
     bool m_convergingToJump{false};
 
-    bool positionJumped();
+    bool positionJumped(const OpenXrInterface::NamedPoseVelocity& input);
 
-    bool rotationJumped();
+    bool rotationJumped(const OpenXrInterface::NamedPoseVelocity& input);
 
-    void filterJumps();
-
-    void resetWarnings();
+    OpenXrInterface::NamedPoseVelocity filterJumps(const OpenXrInterface::NamedPoseVelocity& input);
 
     void resetLastValidData();
 
-    void publishOldTransform();
-
-    void publishNewTransform();
-
-    void deactivate();
+    virtual void deactivate() override;
 
 public:
-    FilteredPosePublisher();
-
-    void setLabel(const std::string& label);
 
     void configure(std::shared_ptr<FilteredPosePublisherSettings> settings);
 
-    bool configured() const;
-
-    void update(const OpenXrInterface::NamedPoseVelocity& input);
+    virtual bool configured() const override;
 
-    void publish();
+    virtual void updateInputPose(const OpenXrInterface::NamedPoseVelocity& input) override;
 };
 
 #endif // YARP_DEV_FILTEREDPOSEPUBLISHER_H
diff --git a/src/devices/openxrheadset/PosePublisher.cpp b/src/devices/openxrheadset/PosePublisher.cpp
new file mode 100644
index 0000000..c50c24e
--- /dev/null
+++ b/src/devices/openxrheadset/PosePublisher.cpp
@@ -0,0 +1,114 @@
+/*
+ * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This software may be modified and distributed under the terms of the
+ * BSD-2-Clause license. See the accompanying LICENSE file for details.
+ */
+
+#include <PosePublisher.h>
+#include <OpenXrHeadsetLogComponent.h>
+#include <OpenXrYarpUtilities.h>
+#include <yarp/os/LogStream.h>
+
+void PosePublisher::resetWarnings()
+{
+    m_lastWarningTime = 0.0;
+    m_warningCount = 0;
+}
+
+void PosePublisher::deactivate()
+{
+    resetWarnings();
+    m_active = false;
+}
+
+PosePublisher::PosePublisher()
+{
+    m_localPose.resize(4,4);
+    m_localPose.eye();
+}
+
+void PosePublisher::configurePublisher(std::shared_ptr<PosePublisherSettings> settings)
+{
+    m_baseSettings = settings;
+}
+
+void PosePublisher::setLabel(const std::string &label)
+{
+    m_label = label;
+}
+
+const std::string &PosePublisher::label() const
+{
+    return m_label;
+}
+
+bool PosePublisher::configured() const
+{
+    return m_baseSettings != nullptr;
+}
+
+void PosePublisher::updateInputPose(const OpenXrInterface::NamedPoseVelocity &input)
+{
+    if (!configured())
+    {
+        return;
+    }
+
+    m_data = input;
+    bool inputValid = m_data.pose.positionValid && m_data.pose.rotationValid; //we consider the input valid if both the position and the rotation are valid
+    bool wasActive = m_publishedOnce && !m_active; //if the pose was published once, but now it is not active, it means that there has been problems. So we should be active only if the input is valid
+    m_active = !wasActive || inputValid; //We are active if: - it is the first time we update, - we were active the step before, - we received a valid input
+}
+
+void PosePublisher::publish()
+{
+    bool usePreviousTransform = !m_data.pose.positionValid || !m_data.pose.rotationValid;
+    if (!configured() || !m_active || (!m_publishedOnce && usePreviousTransform))
+    {
+        return;
+    }
+
+    if (!m_publishedOnce)
+    {
+        if (m_label.empty())
+        {
+            m_label = m_data.name;
+        }
+        m_publishedOnce = true;
+    }
+
+    if (!usePreviousTransform)
+    {
+        poseToYarpMatrix(m_data.pose, m_localPose);
+        m_data.pose.positionValid = false; //We invalidate the data after use. This is to detect if some pose do not get updated.
+        m_data.pose.rotationValid = false;
+    }
+    else
+    {
+        if (m_warningCount == 0 || yarp::os::Time::now() - m_lastWarningTime > 5.0)
+        {
+            yCWarning(OPENXRHEADSET) << m_label << " is not valid. Publishing its last known pose.";
+            m_lastWarningTime = yarp::os::Time::now();
+            m_warningCount++;
+        }
+
+        if (m_warningCount > 6)
+        {
+            yCWarning(OPENXRHEADSET) << m_label << " was not valid for 30s. Deactivated.";
+            deactivate();
+            return;
+        }
+    }
+
+    if (!m_baseSettings->tfPublisher->setTransform(m_label, m_baseSettings->rootFrame, m_localPose))
+    {
+        yCWarning(OPENXRHEADSET) << "Failed to publish" << m_label << "frame.";
+    }
+}
+
+bool PosePublisher::isActive() const
+{
+    return m_active;
+}
diff --git a/src/devices/openxrheadset/PosePublisher.h b/src/devices/openxrheadset/PosePublisher.h
new file mode 100644
index 0000000..e15029a
--- /dev/null
+++ b/src/devices/openxrheadset/PosePublisher.h
@@ -0,0 +1,59 @@
+/*
+ * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This software may be modified and distributed under the terms of the
+ * BSD-2-Clause license. See the accompanying LICENSE file for details.
+ */
+
+#ifndef YARP_DEV_POSEPUBLISHER_H
+#define YARP_DEV_POSEPUBLISHER_H
+
+#include <OpenXrInterface.h>
+#include <yarp/dev/IFrameTransform.h>
+#include <string>
+#include <memory>
+
+struct PosePublisherSettings
+{
+    yarp::dev::IFrameTransform* tfPublisher;
+    std::string rootFrame;
+};
+
+class PosePublisher
+{
+    std::string m_label;
+    OpenXrInterface::NamedPoseVelocity m_data;
+    yarp::sig::Matrix m_localPose;
+    std::shared_ptr<PosePublisherSettings> m_baseSettings{nullptr};
+    bool m_active{false};
+    bool m_publishedOnce{false};
+    double m_lastWarningTime{0.0};
+    size_t m_warningCount{0};
+
+    void resetWarnings();
+
+protected:
+
+    virtual void deactivate();
+
+public:
+
+    PosePublisher();
+
+    void configurePublisher(std::shared_ptr<PosePublisherSettings> settings);
+
+    void setLabel(const std::string& label);
+
+    const std::string& label() const;
+
+    virtual bool configured() const;
+
+    virtual void updateInputPose(const OpenXrInterface::NamedPoseVelocity& input);
+
+    void publish();
+
+    bool isActive() const;
+};
+
+#endif // YARP_DEV_POSEPUBLISHER_H
diff --git a/src/devices/openxrheadset/PosesManager.cpp b/src/devices/openxrheadset/PosesManager.cpp
index 682b766..20f2264 100644
--- a/src/devices/openxrheadset/PosesManager.cpp
+++ b/src/devices/openxrheadset/PosesManager.cpp
@@ -37,7 +37,7 @@ void PosesManager::publishFrames()
             publisher.configure(m_settings);
         }
 
-        publisher.update(inputPose);
+        publisher.updateInputPose(inputPose);
     }
 
     for (auto& poseIt : m_poses)

From bef0aa1c192f7f5e0fd3d1486c21a157da901f31 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 22 Sep 2022 16:04:38 +0200
Subject: [PATCH 04/14] Added possibility for the PosePublisher to expose the
 pose that will be published.

---
 .../openxrheadset/OpenXrYarpUtilities.cpp     | 30 ++++++++++++++++
 .../openxrheadset/OpenXrYarpUtilities.h       |  2 ++
 src/devices/openxrheadset/PosePublisher.cpp   | 35 ++++++++++++++-----
 src/devices/openxrheadset/PosePublisher.h     |  7 +++-
 4 files changed, 65 insertions(+), 9 deletions(-)

diff --git a/src/devices/openxrheadset/OpenXrYarpUtilities.cpp b/src/devices/openxrheadset/OpenXrYarpUtilities.cpp
index 344ec52..00483fc 100644
--- a/src/devices/openxrheadset/OpenXrYarpUtilities.cpp
+++ b/src/devices/openxrheadset/OpenXrYarpUtilities.cpp
@@ -7,6 +7,8 @@
  */
 
 #include <OpenXrYarpUtilities.h>
+#include <OpenXrHeadsetLogComponent.h>
+#include <yarp/os/LogStream.h>
 
 void poseToYarpMatrix(const Eigen::Vector3f &inputPosition, const Eigen::Quaternionf &inputQuaternion, yarp::sig::Matrix &output)
 {
@@ -54,3 +56,31 @@ void writeQuaternionOnPort(yarp::os::BufferedPort<yarp::os::Bottle> * const &por
         port->write();
     }
 }
+
+OpenXrInterface::Pose yarpMatrixToPose(const yarp::sig::Matrix &input)
+{
+    OpenXrInterface::Pose output;
+
+    if (input.rows() != 4 || input.cols() != 4)
+    {
+        yCWarning(OPENXRHEADSET) << "[yarpMatrixToPose] The input matrix is not 4x4.";
+        return output;
+    }
+
+    Eigen::Matrix3f rotationMatrix;
+    for (size_t i = 0; i < 3; ++i)
+    {
+        for (size_t j = 0; j < 3; ++j)
+        {
+            rotationMatrix(i,j) = input(i, j);
+        }
+        output.position(i) = input(i, 3);
+    }
+
+    output.rotation = Eigen::Quaternionf(rotationMatrix);
+
+    output.positionValid = true;
+    output.rotationValid = true;
+
+    return output;
+}
diff --git a/src/devices/openxrheadset/OpenXrYarpUtilities.h b/src/devices/openxrheadset/OpenXrYarpUtilities.h
index eec94d2..902be29 100644
--- a/src/devices/openxrheadset/OpenXrYarpUtilities.h
+++ b/src/devices/openxrheadset/OpenXrYarpUtilities.h
@@ -21,6 +21,8 @@ void poseToYarpMatrix(const Eigen::Vector3f& inputPosition, const Eigen::Quatern
 
 void poseToYarpMatrix(const OpenXrInterface::Pose& input, yarp::sig::Matrix& output);
 
+OpenXrInterface::Pose yarpMatrixToPose(const yarp::sig::Matrix& input);
+
 void writeVec3OnPort(yarp::os::BufferedPort<yarp::os::Bottle>*const & port, const Eigen::Vector3f& vec3, yarp::os::Stamp& stamp);
 
 void writeQuaternionOnPort(yarp::os::BufferedPort<yarp::os::Bottle>*const & port,
diff --git a/src/devices/openxrheadset/PosePublisher.cpp b/src/devices/openxrheadset/PosePublisher.cpp
index c50c24e..78c533e 100644
--- a/src/devices/openxrheadset/PosePublisher.cpp
+++ b/src/devices/openxrheadset/PosePublisher.cpp
@@ -17,6 +17,16 @@ void PosePublisher::resetWarnings()
     m_warningCount = 0;
 }
 
+bool PosePublisher::usePreviousPose() const
+{
+    return !m_data.pose.positionValid || !m_data.pose.rotationValid;
+}
+
+bool PosePublisher::canPublish() const
+{
+    return configured() && m_active && (m_publishedOnce || !usePreviousPose());
+}
+
 void PosePublisher::deactivate()
 {
     resetWarnings();
@@ -62,10 +72,24 @@ void PosePublisher::updateInputPose(const OpenXrInterface::NamedPoseVelocity &in
     m_active = !wasActive || inputValid; //We are active if: - it is the first time we update, - we were active the step before, - we received a valid input
 }
 
+OpenXrInterface::Pose PosePublisher::pose() const
+{
+    if (!canPublish())
+    {
+        return OpenXrInterface::Pose();
+    }
+
+    if (usePreviousPose())
+    {
+        return yarpMatrixToPose(m_localPose);
+    }
+
+    return m_data.pose;
+}
+
 void PosePublisher::publish()
 {
-    bool usePreviousTransform = !m_data.pose.positionValid || !m_data.pose.rotationValid;
-    if (!configured() || !m_active || (!m_publishedOnce && usePreviousTransform))
+    if (!canPublish())
     {
         return;
     }
@@ -79,7 +103,7 @@ void PosePublisher::publish()
         m_publishedOnce = true;
     }
 
-    if (!usePreviousTransform)
+    if (!usePreviousPose())
     {
         poseToYarpMatrix(m_data.pose, m_localPose);
         m_data.pose.positionValid = false; //We invalidate the data after use. This is to detect if some pose do not get updated.
@@ -107,8 +131,3 @@ void PosePublisher::publish()
         yCWarning(OPENXRHEADSET) << "Failed to publish" << m_label << "frame.";
     }
 }
-
-bool PosePublisher::isActive() const
-{
-    return m_active;
-}
diff --git a/src/devices/openxrheadset/PosePublisher.h b/src/devices/openxrheadset/PosePublisher.h
index e15029a..e08269d 100644
--- a/src/devices/openxrheadset/PosePublisher.h
+++ b/src/devices/openxrheadset/PosePublisher.h
@@ -33,6 +33,10 @@ class PosePublisher
 
     void resetWarnings();
 
+    bool usePreviousPose() const;
+
+    bool canPublish() const;
+
 protected:
 
     virtual void deactivate();
@@ -51,9 +55,10 @@ class PosePublisher
 
     virtual void updateInputPose(const OpenXrInterface::NamedPoseVelocity& input);
 
+    OpenXrInterface::Pose pose() const;
+
     void publish();
 
-    bool isActive() const;
 };
 
 #endif // YARP_DEV_POSEPUBLISHER_H

From 4ad86effe9030571c191667451d46ab9f9e77ede Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Thu, 22 Sep 2022 19:59:15 +0200
Subject: [PATCH 05/14] Added custom pose publisher.

---
 src/devices/openxrheadset/CMakeLists.txt      |   2 +
 .../openxrheadset/CustomPosePublisher.cpp     | 214 ++++++++++++++++++
 .../openxrheadset/CustomPosePublisher.h       |  67 ++++++
 src/devices/openxrheadset/OpenXrHeadset.cpp   |  16 +-
 src/devices/openxrheadset/OpenXrInterface.cpp |  16 ++
 src/devices/openxrheadset/OpenXrInterface.h   |   2 +
 src/devices/openxrheadset/PosePublisher.cpp   |  11 +-
 src/devices/openxrheadset/PosePublisher.h     |   4 +-
 src/devices/openxrheadset/PosesManager.cpp    |  35 ++-
 src/devices/openxrheadset/PosesManager.h      |   9 +
 10 files changed, 366 insertions(+), 10 deletions(-)
 create mode 100644 src/devices/openxrheadset/CustomPosePublisher.cpp
 create mode 100644 src/devices/openxrheadset/CustomPosePublisher.h

diff --git a/src/devices/openxrheadset/CMakeLists.txt b/src/devices/openxrheadset/CMakeLists.txt
index 0027593..048d4b7 100644
--- a/src/devices/openxrheadset/CMakeLists.txt
+++ b/src/devices/openxrheadset/CMakeLists.txt
@@ -52,6 +52,7 @@ set(yarp_openxrheadset_driver_SRCS
     EyesManager.cpp
     PosesManager.cpp
     PosePublisher.cpp
+    CustomPosePublisher.cpp
     FilteredPosePublisher.cpp
 )
 
@@ -62,6 +63,7 @@ set(yarp_openxrheadset_driver_HDRS
     EyesManager.h
     PosesManager.h
     PosePublisher.h
+    CustomPosePublisher.h
     FilteredPosePublisher.h
 )
 
diff --git a/src/devices/openxrheadset/CustomPosePublisher.cpp b/src/devices/openxrheadset/CustomPosePublisher.cpp
new file mode 100644
index 0000000..8714f99
--- /dev/null
+++ b/src/devices/openxrheadset/CustomPosePublisher.cpp
@@ -0,0 +1,214 @@
+/*
+ * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This software may be modified and distributed under the terms of the
+ * BSD-2-Clause license. See the accompanying LICENSE file for details.
+ */
+
+#include <CustomPosePublisher.h>
+#include <OpenXrHeadsetLogComponent.h>
+#include <yarp/os/LogStream.h>
+#include <cctype>
+
+void CustomPosePublisher::configure(std::shared_ptr<CustomPosePublisherSettings> settings)
+{
+    if (settings)
+    {
+        m_settings = settings;
+        m_parentFrame = settings->parentFrame;
+        //We publish the transform relative to the parent frame
+        m_settings->rootFrame = m_parentFrame;
+        PosePublisher::configurePublisher(settings);
+    }
+}
+
+const std::string &CustomPosePublisher::relativeFrame() const
+{
+    return m_parentFrame;
+}
+
+bool CustomPosePublisher::configured() const
+{
+    return m_settings != nullptr && !m_settings->name.empty() && PosePublisher::configured();
+}
+
+void CustomPosePublisher::updateInputPose(const OpenXrInterface::NamedPoseVelocity &input)
+{
+    if (!configured())
+    {
+        return;
+    }
+
+    bool inputValid = input.pose.positionValid && input.pose.rotationValid;
+
+    if (!inputValid)
+    {
+        return;
+    }
+
+    OpenXrInterface::NamedPoseVelocity output;
+    output.name = m_settings->name;
+    output.pose.positionValid = true;
+    output.pose.rotationValid = true;
+    output.velocity.linearValid = false;
+    output.velocity.angularValid = false;
+
+    Eigen::Quaternionf inverseRotation = input.pose.rotation.inverse();
+    Eigen::Vector3f inversePosition = -inverseRotation.toRotationMatrix() * input.pose.position;
+
+    for (size_t i = 0; i < 3; ++i)
+    {
+        if (m_settings->positionMask[i])
+        {
+            output.pose.position[i] = m_settings->relativePosition[i];
+        }
+        else
+        {
+            output.pose.position[i] = inversePosition[i];
+        }
+    }
+
+    Eigen::Vector3f parentFrameEulerAngles =
+            input.pose.rotation.toRotationMatrix().eulerAngles(
+                static_cast<Eigen::Index>(m_settings->anglesOrder[0]),
+                static_cast<Eigen::Index>(m_settings->anglesOrder[1]),
+                static_cast<Eigen::Index>(m_settings->anglesOrder[2]));
+
+    Eigen::Matrix3f absoluteOrientation;
+    absoluteOrientation.setIdentity();
+
+    for (size_t i = 0; i < 3; ++i)
+    {
+        float angle = 0;
+        if (m_settings->rotationMask[i])
+        {
+            angle = parentFrameEulerAngles[i] + m_settings->relativeRotation[i];
+
+        }
+        absoluteOrientation = absoluteOrientation * Eigen::AngleAxisf(angle,
+                                                                      Eigen::Vector3f::Unit(static_cast<Eigen::Index>(m_settings->anglesOrder[i])));
+    }
+
+    output.pose.rotation = inverseRotation * Eigen::Quaternionf(absoluteOrientation);
+
+    PosePublisher::updateInputPose(output);
+}
+
+
+bool CustomPosePublisherSettings::parseFromConfigurationFile(yarp::dev::IFrameTransform *publisher, const std::string &rootFrameName, const yarp::os::Bottle &inputGroup)
+{
+    if (!publisher)
+    {
+        yCError(OPENXRHEADSET) << "The tfPublisher is not valid.";
+        return false;
+    }
+
+    if (inputGroup.isNull())
+    {
+        yCError(OPENXRHEADSET) << "The input group is not found in the configuration file.";
+        return false;
+    }
+
+    tfPublisher = publisher;
+    rootFrame = rootFrameName;
+
+    name = inputGroup.check("name", yarp::os::Value("")).toString();
+    if (name == "")
+    {
+        yCError(OPENXRHEADSET) << "Failed to find name parameter";
+        return false;
+    }
+
+    parentFrame = inputGroup.check("parent_frame", yarp::os::Value("")).toString();
+    if (parentFrame == "")
+    {
+        yCError(OPENXRHEADSET) << "Failed to find parent_frame parameter";
+        return false;
+    }
+
+    std::string parametrization = inputGroup.check("euler_angles", yarp::os::Value("")).toString();
+
+    if (parametrization.size() != 3)
+    {
+        yCError(OPENXRHEADSET) << "Failed to parse euler_angles parameter. It is either not present, or it is not a string of three values.";
+        return false;
+    }
+
+    for (size_t i = 0; i < 3; ++i)
+    {
+        switch (std::tolower(parametrization[i]))
+        {
+        case 'x':
+            anglesOrder[i] = RotationAxis::x;
+            break;
+
+        case 'y':
+            anglesOrder[i] = RotationAxis::y;
+            break;
+
+        case 'z':
+            anglesOrder[i] = RotationAxis::z;
+            break;
+
+        default:
+            yCError(OPENXRHEADSET) << "Failed to parse euler_angles parameter."
+                                   << "The element at position" << i << "is not in the set (x, y, z)";
+            return false;
+        }
+    }
+
+    auto parseMask = [](const yarp::os::Bottle& inputGroup, const std::string& maskName,
+            std::array<bool, 3> &outputMask, Eigen::Vector3f &outputOffset) -> bool
+    {
+        yarp::os::Value maskValue = inputGroup.find(maskName);
+        if (!maskValue.isList())
+        {
+            yCError(OPENXRHEADSET) << "Failed to parse" << maskName <<", it is not present, or not a list.";
+            return false;
+        }
+
+        yarp::os::Bottle* positionMaskList = maskValue.asList();
+        if (positionMaskList->size() != 3)
+        {
+            yCError(OPENXRHEADSET) << "Failed to parse" << maskName <<", the list has dimension different from 3.";
+            return false;
+        }
+
+        for (size_t i = 0; i < 3; ++i)
+        {
+            yarp::os::Value& value = positionMaskList->get(i);
+            if (value.isFloat32())
+            {
+                outputOffset[i] = value.asFloat32();
+                outputMask[i] = true;
+            }
+            else if (value.isInt8() && value.asInt8() == '*')
+            {
+                outputOffset[i] = 0;
+                outputMask[i] = false;
+            }
+            else
+            {
+                yCError(OPENXRHEADSET) << "Failed to parse" << maskName
+                                       << "at element" << i
+                                       << ". Only float numbers and the special carachter '*' are allowed";
+                return false;
+            }
+        }
+
+        return true;
+    };
+
+    if (!parseMask(inputGroup, "relative_position", positionMask, relativePosition))
+    {
+        return false;
+    }
+
+    if (!parseMask(inputGroup, "relative_rotation", rotationMask, relativeRotation))
+    {
+        return false;
+    }
+
+    return true;
+}
diff --git a/src/devices/openxrheadset/CustomPosePublisher.h b/src/devices/openxrheadset/CustomPosePublisher.h
new file mode 100644
index 0000000..956116b
--- /dev/null
+++ b/src/devices/openxrheadset/CustomPosePublisher.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright (C) 2022 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This software may be modified and distributed under the terms of the
+ * BSD-2-Clause license. See the accompanying LICENSE file for details.
+ */
+
+#ifndef YARP_DEV_CUSTOMPOSEPUBLISHER_H
+#define YARP_DEV_CUSTOMPOSEPUBLISHER_H
+
+#include <PosePublisher.h>
+#include <OpenXrInterface.h>
+#include <array>
+#include <string>
+#include <memory>
+#include <yarp/os/Bottle.h>
+
+struct CustomPosePublisherSettings : PosePublisherSettings
+{
+    enum class RotationAxis : Eigen::Index
+    {
+        x = 0,
+        y = 1,
+        z = 2
+    };
+
+    std::string parentFrame;
+    std::string name;
+
+    std::array<bool, 3> positionMask = {false, false, false};
+    Eigen::Vector3f relativePosition;
+
+    std::array<bool, 3> rotationMask = {false, false, false};
+    std::array<RotationAxis, 3> anglesOrder = {RotationAxis::y, RotationAxis::x, RotationAxis::z};
+    Eigen::Vector3f relativeRotation;
+
+    bool parseFromConfigurationFile(yarp::dev::IFrameTransform* publisher,
+                                    const std::string& rootFrameName,
+                                    const yarp::os::Bottle& inputGroup);
+
+};
+
+class CustomPosePublisher : public PosePublisher
+{
+    std::shared_ptr<CustomPosePublisherSettings> m_settings;
+    std::string m_parentFrame;
+
+public:
+
+    void configure(std::shared_ptr<CustomPosePublisherSettings> settings);
+
+    const std::string& relativeFrame() const;
+
+    virtual bool configured() const override;
+
+    virtual void updateInputPose(const OpenXrInterface::NamedPoseVelocity& input) override;
+
+};
+
+//Notes
+// if it is the openxr_origin, or its alias, then the input pose is the identity
+//the pose of the parent frame should be set from outside
+//the name of the parent frame should be chosen amongs the set of labels first
+//and then among the set of pose publisher
+
+#endif // YARP_DEV_CUSTOMPOSEPUBLISHER_H
diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp
index 34e9798..ee3db49 100644
--- a/src/devices/openxrheadset/OpenXrHeadset.cpp
+++ b/src/devices/openxrheadset/OpenXrHeadset.cpp
@@ -245,7 +245,6 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
         labels.push_back({labelList->get(0).asString(), labelList->get(1).asString()});
     }
 
-
     //opening tf client
     yarp::os::Property tfClientCfg;
     tfClientCfg.put("device", cfg.check("tfDevice", yarp::os::Value("transformClient")).asString());
@@ -281,7 +280,20 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
         yCWarning(OPENXRHEADSET) << "pose_check_convergence_ratio is supposed to be in the range [0, 1]. Clamping to" << posePublisherSettings.checks.convergenceRatio << ".";
     }
 
-    m_posesManager.initialize(labels, posePublisherSettings);
+    std::vector<CustomPosePublisherSettings> customPoses;
+    int custom_poses_count = cfg.find("custom_poses").asInt32();
+    for (int i = 0; i < custom_poses_count; ++i)
+    {
+        customPoses.emplace_back();
+        std::string groupName = "CUSTOM_POSE_" + std::to_string(i);
+        if (!customPoses.back().parseFromConfigurationFile(m_tfPublisher, m_rootFrameRaw, cfg.findGroup(groupName)))
+        {
+            yCError(OPENXRHEADSET) << "Failed to parse" << groupName;
+            return false;
+        }
+    }
+
+    m_posesManager.initialize(labels, customPoses, posePublisherSettings);
 
     // Start the thread
     if (!this->start()) {
diff --git a/src/devices/openxrheadset/OpenXrInterface.cpp b/src/devices/openxrheadset/OpenXrInterface.cpp
index 96fbb56..c6925c2 100644
--- a/src/devices/openxrheadset/OpenXrInterface.cpp
+++ b/src/devices/openxrheadset/OpenXrInterface.cpp
@@ -1691,3 +1691,19 @@ void OpenXrInterface::close()
 
     m_pimpl->closed = true;
 }
+
+OpenXrInterface::NamedPoseVelocity OpenXrInterface::NamedPoseVelocity::Identity(const std::string &name)
+{
+    OpenXrInterface::NamedPoseVelocity output;
+    output.name = name;
+    output.pose.positionValid = true;
+    output.pose.position.setZero();
+    output.pose.rotationValid = true;
+    output.pose.rotation.setIdentity();
+    output.velocity.linearValid = true;
+    output.velocity.linear.setZero();
+    output.velocity.angularValid = true;
+    output.velocity.angular.setZero();
+
+    return output;
+}
diff --git a/src/devices/openxrheadset/OpenXrInterface.h b/src/devices/openxrheadset/OpenXrInterface.h
index 36ec920..6359a68 100644
--- a/src/devices/openxrheadset/OpenXrInterface.h
+++ b/src/devices/openxrheadset/OpenXrInterface.h
@@ -139,6 +139,8 @@ class OpenXrInterface
         std::string name;
         Pose pose;
         Velocity velocity;
+
+        static NamedPoseVelocity Identity(const std::string& name);
     };
 
     OpenXrInterface();
diff --git a/src/devices/openxrheadset/PosePublisher.cpp b/src/devices/openxrheadset/PosePublisher.cpp
index 78c533e..edd340a 100644
--- a/src/devices/openxrheadset/PosePublisher.cpp
+++ b/src/devices/openxrheadset/PosePublisher.cpp
@@ -72,19 +72,19 @@ void PosePublisher::updateInputPose(const OpenXrInterface::NamedPoseVelocity &in
     m_active = !wasActive || inputValid; //We are active if: - it is the first time we update, - we were active the step before, - we received a valid input
 }
 
-OpenXrInterface::Pose PosePublisher::pose() const
+OpenXrInterface::NamedPoseVelocity PosePublisher::data() const
 {
     if (!canPublish())
     {
-        return OpenXrInterface::Pose();
+        return OpenXrInterface::NamedPoseVelocity();
     }
 
     if (usePreviousPose())
     {
-        return yarpMatrixToPose(m_localPose);
+        return m_previouslyPublishedData;
     }
 
-    return m_data.pose;
+    return m_data;
 }
 
 void PosePublisher::publish()
@@ -100,12 +100,13 @@ void PosePublisher::publish()
         {
             m_label = m_data.name;
         }
-        m_publishedOnce = true;
     }
 
     if (!usePreviousPose())
     {
         poseToYarpMatrix(m_data.pose, m_localPose);
+        m_publishedOnce = true;
+        m_previouslyPublishedData = m_data;
         m_data.pose.positionValid = false; //We invalidate the data after use. This is to detect if some pose do not get updated.
         m_data.pose.rotationValid = false;
     }
diff --git a/src/devices/openxrheadset/PosePublisher.h b/src/devices/openxrheadset/PosePublisher.h
index e08269d..3cf1c6b 100644
--- a/src/devices/openxrheadset/PosePublisher.h
+++ b/src/devices/openxrheadset/PosePublisher.h
@@ -23,7 +23,7 @@ struct PosePublisherSettings
 class PosePublisher
 {
     std::string m_label;
-    OpenXrInterface::NamedPoseVelocity m_data;
+    OpenXrInterface::NamedPoseVelocity m_data, m_previouslyPublishedData;
     yarp::sig::Matrix m_localPose;
     std::shared_ptr<PosePublisherSettings> m_baseSettings{nullptr};
     bool m_active{false};
@@ -55,7 +55,7 @@ class PosePublisher
 
     virtual void updateInputPose(const OpenXrInterface::NamedPoseVelocity& input);
 
-    OpenXrInterface::Pose pose() const;
+    OpenXrInterface::NamedPoseVelocity data() const;
 
     void publish();
 
diff --git a/src/devices/openxrheadset/PosesManager.cpp b/src/devices/openxrheadset/PosesManager.cpp
index 20f2264..ecbbaf8 100644
--- a/src/devices/openxrheadset/PosesManager.cpp
+++ b/src/devices/openxrheadset/PosesManager.cpp
@@ -11,14 +11,23 @@
 #include <OpenXrYarpUtilities.h>
 #include <yarp/os/LogStream.h>
 
-void PosesManager::initialize(const std::vector<Label>& labels, const FilteredPosePublisherSettings &settings)
+void PosesManager::initialize(const std::vector<Label>& labels, const std::vector<CustomPosePublisherSettings> &customPoses, const FilteredPosePublisherSettings &settings)
 {
     m_settings = std::make_shared<FilteredPosePublisherSettings>(settings);
 
     for (auto& label : labels)
     {
         m_poses[label.original].setLabel(label.modified);
+        m_labelsReverseMap[label.modified] = label.original;
     }
+
+    for (auto& customPose : customPoses)
+    {
+        m_customPoses.emplace_back();
+        m_customPoses.back().configure(std::make_shared<CustomPosePublisherSettings>(customPose));
+    }
+
+    m_rootPose = OpenXrInterface::NamedPoseVelocity::Identity(m_settings->rootFrame);;
 }
 
 std::vector<OpenXrInterface::NamedPoseVelocity> &PosesManager::inputs()
@@ -40,8 +49,32 @@ void PosesManager::publishFrames()
         publisher.updateInputPose(inputPose);
     }
 
+    for (auto& customPose : m_customPoses)
+    {
+        const std::string& relativeFrame = customPose.relativeFrame();
+        if (relativeFrame == m_settings->rootFrame)
+        {
+            customPose.updateInputPose(m_rootPose);
+        }
+
+        const auto& labelIt = m_labelsReverseMap.find(relativeFrame);
+        if (labelIt != m_labelsReverseMap.end()) //The parent frame name is a label
+        {
+            customPose.updateInputPose(m_poses[labelIt->second].data());
+        }
+        else
+        {
+            customPose.updateInputPose(m_poses[relativeFrame].data());
+        }
+    }
+
     for (auto& poseIt : m_poses)
     {
         poseIt.second.publish();
     }
+
+    for (auto& customPose : m_customPoses)
+    {
+        customPose.publish();
+    }
 }
diff --git a/src/devices/openxrheadset/PosesManager.h b/src/devices/openxrheadset/PosesManager.h
index ce8e94c..44ec6bd 100644
--- a/src/devices/openxrheadset/PosesManager.h
+++ b/src/devices/openxrheadset/PosesManager.h
@@ -11,8 +11,10 @@
 
 #include <OpenXrInterface.h>
 #include <FilteredPosePublisher.h>
+#include <CustomPosePublisher.h>
 #include <yarp/dev/IFrameTransform.h>
 #include <string>
+#include <unordered_map>
 
 class PosesManager
 {
@@ -22,6 +24,12 @@ class PosesManager
 
     std::unordered_map<std::string, FilteredPosePublisher> m_poses;
 
+    std::vector<CustomPosePublisher> m_customPoses;
+
+    std::unordered_map<std::string, std::string> m_labelsReverseMap;
+
+    OpenXrInterface::NamedPoseVelocity m_rootPose;
+
 public:
 
     struct Label
@@ -31,6 +39,7 @@ class PosesManager
     };
 
     void initialize(const std::vector<Label> &labels,
+                    const std::vector<CustomPosePublisherSettings> &customPoses,
                     const FilteredPosePublisherSettings &settings);
 
     std::vector<OpenXrInterface::NamedPoseVelocity>& inputs();

From 6bead0d02085e85c2920cf072c0965e22859b867 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 23 Sep 2022 12:31:02 +0200
Subject: [PATCH 06/14] The raw root frame is published as a custom frame.

---
 .../openxrheadset/CustomPosePublisher.cpp     | 42 ++++++++---
 .../openxrheadset/CustomPosePublisher.h       | 21 +++---
 src/devices/openxrheadset/OpenXrHeadset.cpp   | 28 ++++----
 src/devices/openxrheadset/OpenXrHeadset.h     |  2 +-
 src/devices/openxrheadset/PosePublisher.cpp   |  4 +-
 src/devices/openxrheadset/PosePublisher.h     |  4 +-
 src/devices/openxrheadset/PosesManager.cpp    | 72 ++++++++++++++++---
 src/devices/openxrheadset/PosesManager.h      |  9 ++-
 8 files changed, 128 insertions(+), 54 deletions(-)

diff --git a/src/devices/openxrheadset/CustomPosePublisher.cpp b/src/devices/openxrheadset/CustomPosePublisher.cpp
index 8714f99..279bfd9 100644
--- a/src/devices/openxrheadset/CustomPosePublisher.cpp
+++ b/src/devices/openxrheadset/CustomPosePublisher.cpp
@@ -17,17 +17,46 @@ void CustomPosePublisher::configure(std::shared_ptr<CustomPosePublisherSettings>
     {
         m_settings = settings;
         m_parentFrame = settings->parentFrame;
+        m_name = settings->name;
         //We publish the transform relative to the parent frame
-        m_settings->rootFrame = m_parentFrame;
+        m_settings->rawRootFrame = m_parentFrame;
         PosePublisher::configurePublisher(settings);
     }
 }
 
+const std::string &CustomPosePublisher::name() const
+{
+    return m_name;
+}
+
 const std::string &CustomPosePublisher::relativeFrame() const
 {
     return m_parentFrame;
 }
 
+void CustomPosePublisher::setRelativePosition(const Eigen::Vector3f relativePosition)
+{
+    if (!configured())
+    {
+        return;
+    }
+
+    m_settings->relativePosition = relativePosition;
+}
+
+void CustomPosePublisher::setRelativeOrientation(const Eigen::Quaternionf &relativeOrientation)
+{
+    if (!configured())
+    {
+        return;
+    }
+
+    m_settings->relativeRotation = relativeOrientation.toRotationMatrix().eulerAngles(
+                static_cast<Eigen::Index>(m_settings->anglesOrder[0]),
+                static_cast<Eigen::Index>(m_settings->anglesOrder[1]),
+                static_cast<Eigen::Index>(m_settings->anglesOrder[2]));
+}
+
 bool CustomPosePublisher::configured() const
 {
     return m_settings != nullptr && !m_settings->name.empty() && PosePublisher::configured();
@@ -96,23 +125,14 @@ void CustomPosePublisher::updateInputPose(const OpenXrInterface::NamedPoseVeloci
 }
 
 
-bool CustomPosePublisherSettings::parseFromConfigurationFile(yarp::dev::IFrameTransform *publisher, const std::string &rootFrameName, const yarp::os::Bottle &inputGroup)
+bool CustomPosePublisherSettings::parseFromConfigurationFile(const yarp::os::Bottle &inputGroup)
 {
-    if (!publisher)
-    {
-        yCError(OPENXRHEADSET) << "The tfPublisher is not valid.";
-        return false;
-    }
-
     if (inputGroup.isNull())
     {
         yCError(OPENXRHEADSET) << "The input group is not found in the configuration file.";
         return false;
     }
 
-    tfPublisher = publisher;
-    rootFrame = rootFrameName;
-
     name = inputGroup.check("name", yarp::os::Value("")).toString();
     if (name == "")
     {
diff --git a/src/devices/openxrheadset/CustomPosePublisher.h b/src/devices/openxrheadset/CustomPosePublisher.h
index 956116b..1cd9c7a 100644
--- a/src/devices/openxrheadset/CustomPosePublisher.h
+++ b/src/devices/openxrheadset/CustomPosePublisher.h
@@ -28,16 +28,14 @@ struct CustomPosePublisherSettings : PosePublisherSettings
     std::string parentFrame;
     std::string name;
 
-    std::array<bool, 3> positionMask = {false, false, false};
+    std::array<bool, 3> positionMask = {true, true, true};
     Eigen::Vector3f relativePosition;
 
-    std::array<bool, 3> rotationMask = {false, false, false};
+    std::array<bool, 3> rotationMask = {true, true, true};
     std::array<RotationAxis, 3> anglesOrder = {RotationAxis::y, RotationAxis::x, RotationAxis::z};
     Eigen::Vector3f relativeRotation;
 
-    bool parseFromConfigurationFile(yarp::dev::IFrameTransform* publisher,
-                                    const std::string& rootFrameName,
-                                    const yarp::os::Bottle& inputGroup);
+    bool parseFromConfigurationFile(const yarp::os::Bottle& inputGroup);
 
 };
 
@@ -45,23 +43,24 @@ class CustomPosePublisher : public PosePublisher
 {
     std::shared_ptr<CustomPosePublisherSettings> m_settings;
     std::string m_parentFrame;
+    std::string m_name;
 
 public:
 
     void configure(std::shared_ptr<CustomPosePublisherSettings> settings);
 
+    const std::string& name() const;
+
     const std::string& relativeFrame() const;
 
+    void setRelativePosition(const Eigen::Vector3f relativePosition);
+
+    void setRelativeOrientation(const Eigen::Quaternionf& relativeOrientation);
+
     virtual bool configured() const override;
 
     virtual void updateInputPose(const OpenXrInterface::NamedPoseVelocity& input) override;
 
 };
 
-//Notes
-// if it is the openxr_origin, or its alias, then the input pose is the identity
-//the pose of the parent frame should be set from outside
-//the name of the parent frame should be chosen amongs the set of labels first
-//and then among the set of pose publisher
-
 #endif // YARP_DEV_CUSTOMPOSEPUBLISHER_H
diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp
index ee3db49..95ac994 100644
--- a/src/devices/openxrheadset/OpenXrHeadset.cpp
+++ b/src/devices/openxrheadset/OpenXrHeadset.cpp
@@ -228,8 +228,11 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
     m_eyesManager.options().splitEyes = cfg.check("split_eye_ports", yarp::os::Value(true)).asBool();
     m_eyesManager.options().portPrefix = m_prefix;
 
-    m_rawRootFrameTransform.resize(4,4);
-    m_rawRootFrameTransform.eye();
+    m_rootFrameRawHRootFrame.position.setZero();
+    m_rootFrameRawHRootFrame.positionValid = true;
+    m_rootFrameRawHRootFrame.rotation.setIdentity();
+    m_rootFrameRawHRootFrame.rotationValid = true;
+
 
     std::vector<PosesManager::Label> labels;
     yarp::os::Bottle& labelsGroup = cfg.findGroup("POSES_LABELS");
@@ -266,7 +269,7 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
 
     FilteredPosePublisherSettings posePublisherSettings;
     posePublisherSettings.tfPublisher = m_tfPublisher;
-    posePublisherSettings.rootFrame = m_rootFrameRaw;
+    posePublisherSettings.rawRootFrame = m_rootFrameRaw;
     posePublisherSettings.period = getPeriod();
     posePublisherSettings.checks.maxDistance = cfg.check("pose_check_max_distance", yarp::os::Value(0.1)).asFloat64();
     posePublisherSettings.checks.maxAngularDistanceInRad = cfg.check("pose_check_max_angle_rad", yarp::os::Value(0.5)).asFloat64();
@@ -286,14 +289,14 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
     {
         customPoses.emplace_back();
         std::string groupName = "CUSTOM_POSE_" + std::to_string(i);
-        if (!customPoses.back().parseFromConfigurationFile(m_tfPublisher, m_rootFrameRaw, cfg.findGroup(groupName)))
+        if (!customPoses.back().parseFromConfigurationFile(cfg.findGroup(groupName)))
         {
             yCError(OPENXRHEADSET) << "Failed to parse" << groupName;
             return false;
         }
     }
 
-    m_posesManager.initialize(labels, customPoses, posePublisherSettings);
+    m_posesManager.initialize(m_rootFrame, labels, customPoses, posePublisherSettings);
 
     // Start the thread
     if (!this->start()) {
@@ -467,14 +470,12 @@ void yarp::dev::OpenXrHeadset::run()
         if (m_openXrInterface.shouldResetLocalReferenceSpace())
         {
             //The local reference space has been changed by the user.
-            m_rawRootFrameTransform.eye();
+            m_rootFrameRawHRootFrame.position.setZero();
+            m_rootFrameRawHRootFrame.rotation.setIdentity();
         }
 
         //Publish the transformation from the root frame to the OpenXR root frame
-        if (!m_tfPublisher->setTransform(m_rootFrameRaw, m_rootFrame, m_rawRootFrameTransform))
-        {
-            yCWarning(OPENXRHEADSET) << "Failed to update the transformation of the raw root frame.";
-        }
+        m_posesManager.setTransformFromRawToRootFrame(m_rootFrameRawHRootFrame);
 
         m_posesManager.publishFrames();
     }
@@ -838,12 +839,9 @@ bool yarp::dev::OpenXrHeadset::alignRootFrameToHeadset()
         gravityAngle = std::atan2(root_R_headset(0, 2), root_R_headset(0, 0));
     }
 
-    OpenXrInterface::Pose rootFramePose;
-
-    rootFramePose.rotation = Eigen::AngleAxisf(-gravityAngle, Eigen::Vector3f::UnitY());
-    rootFramePose.position = -(rootFramePose.rotation * headPose.position);
 
-    poseToYarpMatrix(rootFramePose, m_rawRootFrameTransform);
+    m_rootFrameRawHRootFrame.rotation = Eigen::AngleAxisf(gravityAngle, Eigen::Vector3f::UnitY());
+    m_rootFrameRawHRootFrame.position = headPose.position;
 
     return true;
 }
diff --git a/src/devices/openxrheadset/OpenXrHeadset.h b/src/devices/openxrheadset/OpenXrHeadset.h
index a7a1801..56a8e2c 100644
--- a/src/devices/openxrheadset/OpenXrHeadset.h
+++ b/src/devices/openxrheadset/OpenXrHeadset.h
@@ -252,7 +252,7 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
     std::string      m_rootFrame;
     PolyDriver       m_driver;
 
-    yarp::sig::Matrix m_rawRootFrameTransform;
+    OpenXrInterface::Pose m_rootFrameRawHRootFrame;
 
     yarp::os::Port m_rpcPort;
 
diff --git a/src/devices/openxrheadset/PosePublisher.cpp b/src/devices/openxrheadset/PosePublisher.cpp
index edd340a..49d510b 100644
--- a/src/devices/openxrheadset/PosePublisher.cpp
+++ b/src/devices/openxrheadset/PosePublisher.cpp
@@ -56,7 +56,7 @@ const std::string &PosePublisher::label() const
 
 bool PosePublisher::configured() const
 {
-    return m_baseSettings != nullptr;
+    return m_baseSettings != nullptr && m_baseSettings->tfPublisher != nullptr;
 }
 
 void PosePublisher::updateInputPose(const OpenXrInterface::NamedPoseVelocity &input)
@@ -127,7 +127,7 @@ void PosePublisher::publish()
         }
     }
 
-    if (!m_baseSettings->tfPublisher->setTransform(m_label, m_baseSettings->rootFrame, m_localPose))
+    if (!m_baseSettings->tfPublisher->setTransform(m_label, m_baseSettings->rawRootFrame, m_localPose))
     {
         yCWarning(OPENXRHEADSET) << "Failed to publish" << m_label << "frame.";
     }
diff --git a/src/devices/openxrheadset/PosePublisher.h b/src/devices/openxrheadset/PosePublisher.h
index 3cf1c6b..e1d5f75 100644
--- a/src/devices/openxrheadset/PosePublisher.h
+++ b/src/devices/openxrheadset/PosePublisher.h
@@ -16,8 +16,8 @@
 
 struct PosePublisherSettings
 {
-    yarp::dev::IFrameTransform* tfPublisher;
-    std::string rootFrame;
+    yarp::dev::IFrameTransform* tfPublisher{nullptr};
+    std::string rawRootFrame;
 };
 
 class PosePublisher
diff --git a/src/devices/openxrheadset/PosesManager.cpp b/src/devices/openxrheadset/PosesManager.cpp
index ecbbaf8..5b20cd0 100644
--- a/src/devices/openxrheadset/PosesManager.cpp
+++ b/src/devices/openxrheadset/PosesManager.cpp
@@ -11,10 +11,19 @@
 #include <OpenXrYarpUtilities.h>
 #include <yarp/os/LogStream.h>
 
-void PosesManager::initialize(const std::vector<Label>& labels, const std::vector<CustomPosePublisherSettings> &customPoses, const FilteredPosePublisherSettings &settings)
+void PosesManager::initialize(const std::string &editedRootFrame, const std::vector<Label>& labels, const std::vector<CustomPosePublisherSettings> &customPoses, const FilteredPosePublisherSettings &settings)
 {
     m_settings = std::make_shared<FilteredPosePublisherSettings>(settings);
 
+    auto rootFramePublisherOptions = std::make_shared<CustomPosePublisherSettings>();
+    rootFramePublisherOptions->tfPublisher = m_settings->tfPublisher;
+    rootFramePublisherOptions->rawRootFrame = m_settings->rawRootFrame;
+    rootFramePublisherOptions->name = editedRootFrame;
+    rootFramePublisherOptions->parentFrame = m_settings->rawRootFrame;
+    rootFramePublisherOptions->relativePosition.setZero();
+    rootFramePublisherOptions->relativeRotation.setZero();
+    m_rootFramePublisher.configure(rootFramePublisherOptions);
+
     for (auto& label : labels)
     {
         m_poses[label.original].setLabel(label.modified);
@@ -24,10 +33,13 @@ void PosesManager::initialize(const std::vector<Label>& labels, const std::vecto
     for (auto& customPose : customPoses)
     {
         m_customPoses.emplace_back();
-        m_customPoses.back().configure(std::make_shared<CustomPosePublisherSettings>(customPose));
+        auto customOptions = std::make_shared<CustomPosePublisherSettings>(customPose);
+        customOptions->tfPublisher = m_settings->tfPublisher;
+        customOptions->rawRootFrame = m_settings->rawRootFrame;
+        m_customPoses.back().configure(customOptions);
     }
 
-    m_rootPose = OpenXrInterface::NamedPoseVelocity::Identity(m_settings->rootFrame);;
+    m_rootPose = OpenXrInterface::NamedPoseVelocity::Identity(editedRootFrame);
 }
 
 std::vector<OpenXrInterface::NamedPoseVelocity> &PosesManager::inputs()
@@ -37,6 +49,9 @@ std::vector<OpenXrInterface::NamedPoseVelocity> &PosesManager::inputs()
 
 void PosesManager::publishFrames()
 {
+
+    m_rootFramePublisher.updateInputPose(m_rootPose);
+
     for (auto& inputPose : m_posesInputList)
     {
         FilteredPosePublisher& publisher = m_poses[inputPose.name];
@@ -52,22 +67,44 @@ void PosesManager::publishFrames()
     for (auto& customPose : m_customPoses)
     {
         const std::string& relativeFrame = customPose.relativeFrame();
-        if (relativeFrame == m_settings->rootFrame)
-        {
-            customPose.updateInputPose(m_rootPose);
-        }
+        OpenXrInterface::NamedPoseVelocity parentFramePose;
 
-        const auto& labelIt = m_labelsReverseMap.find(relativeFrame);
-        if (labelIt != m_labelsReverseMap.end()) //The parent frame name is a label
+        if (relativeFrame == m_rootFramePublisher.name())
         {
-            customPose.updateInputPose(m_poses[labelIt->second].data());
+            parentFramePose = m_rootPose;
         }
         else
         {
-            customPose.updateInputPose(m_poses[relativeFrame].data());
+            const auto& labelIt = m_labelsReverseMap.find(relativeFrame);
+
+            OpenXrInterface::NamedPoseVelocity poseRawRootToParent;
+
+            if (labelIt != m_labelsReverseMap.end()) //The parent frame name is a label
+            {
+                poseRawRootToParent = m_poses[labelIt->second].data();
+            }
+            else
+            {
+                poseRawRootToParent = m_poses[relativeFrame].data();
+            }
+
+            //The pose of the parent frame is expressed with respect to the raw root, i.e. the frame wrt we recieve the poses from OpenXR.
+            //The user is interested on the root_frame that can be aligned to headset via RPC call.
+
+            parentFramePose.name = poseRawRootToParent.name;
+            parentFramePose.pose.positionValid = poseRawRootToParent.pose.positionValid;
+            parentFramePose.pose.rotationValid = poseRawRootToParent.pose.rotationValid;
+
+            parentFramePose.pose.position = m_rootFrameRawRelativePoseInverse.position + m_rootFrameRawRelativePoseInverse.rotation * poseRawRootToParent.pose.position;
+            parentFramePose.pose.rotation = m_rootFrameRawRelativePoseInverse.rotation * poseRawRootToParent.pose.rotation;
+
         }
+
+        customPose.updateInputPose(parentFramePose);
     }
 
+    m_rootFramePublisher.publish();
+
     for (auto& poseIt : m_poses)
     {
         poseIt.second.publish();
@@ -78,3 +115,16 @@ void PosesManager::publishFrames()
         customPose.publish();
     }
 }
+
+void PosesManager::setTransformFromRawToRootFrame(const OpenXrInterface::Pose &relativePose)
+{
+    m_rootFrameRawRelativePose = relativePose;
+    m_rootFramePublisher.setRelativePosition(relativePose.position);
+    m_rootFramePublisher.setRelativeOrientation(relativePose.rotation);
+
+    m_rootFrameRawRelativePoseInverse.positionValid = relativePose.positionValid;
+    m_rootFrameRawRelativePoseInverse.rotationValid = relativePose.rotationValid;
+
+    m_rootFrameRawRelativePoseInverse.rotation = relativePose.rotation.inverse();
+    m_rootFrameRawRelativePoseInverse.position = -(m_rootFrameRawRelativePoseInverse.rotation * relativePose.position);
+}
diff --git a/src/devices/openxrheadset/PosesManager.h b/src/devices/openxrheadset/PosesManager.h
index 44ec6bd..65bfd0a 100644
--- a/src/devices/openxrheadset/PosesManager.h
+++ b/src/devices/openxrheadset/PosesManager.h
@@ -30,6 +30,10 @@ class PosesManager
 
     OpenXrInterface::NamedPoseVelocity m_rootPose;
 
+    CustomPosePublisher m_rootFramePublisher;
+
+    OpenXrInterface::Pose m_rootFrameRawRelativePose, m_rootFrameRawRelativePoseInverse;
+
 public:
 
     struct Label
@@ -38,7 +42,8 @@ class PosesManager
         std::string modified;
     };
 
-    void initialize(const std::vector<Label> &labels,
+    void initialize(const std::string& editedRootFrame,
+                    const std::vector<Label> &labels,
                     const std::vector<CustomPosePublisherSettings> &customPoses,
                     const FilteredPosePublisherSettings &settings);
 
@@ -46,6 +51,8 @@ class PosesManager
 
     void publishFrames();
 
+    void setTransformFromRawToRootFrame(const OpenXrInterface::Pose& relativePose);
+
 };
 
 

From e78991c46a29ef8018d6b3544624a609d28b886c Mon Sep 17 00:00:00 2001
From: iCub Virtualizer <icub@foo.com>
Date: Fri, 23 Sep 2022 13:52:12 +0200
Subject: [PATCH 07/14] Fixed parsing of custom frames.

---
 src/devices/openxrheadset/CustomPosePublisher.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/devices/openxrheadset/CustomPosePublisher.cpp b/src/devices/openxrheadset/CustomPosePublisher.cpp
index 279bfd9..889c9fe 100644
--- a/src/devices/openxrheadset/CustomPosePublisher.cpp
+++ b/src/devices/openxrheadset/CustomPosePublisher.cpp
@@ -198,12 +198,12 @@ bool CustomPosePublisherSettings::parseFromConfigurationFile(const yarp::os::Bot
         for (size_t i = 0; i < 3; ++i)
         {
             yarp::os::Value& value = positionMaskList->get(i);
-            if (value.isFloat32())
+            if (value.isFloat32() || value.isFloat64())
             {
                 outputOffset[i] = value.asFloat32();
                 outputMask[i] = true;
             }
-            else if (value.isInt8() && value.asInt8() == '*')
+            else if (value.isString() && value.asString() == "*")
             {
                 outputOffset[i] = 0;
                 outputMask[i] = false;

From 5f35eb8282c88a2af53d402909582b9191f273c7 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 23 Sep 2022 18:39:43 +0200
Subject: [PATCH 08/14] Implemented custom euler angles methods.

---
 src/devices/openxrheadset/CMakeLists.txt      |   2 +
 .../openxrheadset/CustomPosePublisher.cpp     |  25 +-
 .../openxrheadset/CustomPosePublisher.h       |  10 +-
 src/devices/openxrheadset/EulerAngles.cpp     | 268 ++++++++++++++++++
 src/devices/openxrheadset/EulerAngles.h       |  48 ++++
 5 files changed, 332 insertions(+), 21 deletions(-)
 create mode 100644 src/devices/openxrheadset/EulerAngles.cpp
 create mode 100644 src/devices/openxrheadset/EulerAngles.h

diff --git a/src/devices/openxrheadset/CMakeLists.txt b/src/devices/openxrheadset/CMakeLists.txt
index 048d4b7..9ba2b36 100644
--- a/src/devices/openxrheadset/CMakeLists.txt
+++ b/src/devices/openxrheadset/CMakeLists.txt
@@ -30,6 +30,7 @@ set(yarp_openxrheadset_SRCS
   impl/OpenXrInterfaceImpl.cpp
   LabelPortToQuadLayer.cpp
   SlideManager.cpp
+  EulerAngles.cpp
 )
 
 set(yarp_openxrheadset_HDRS
@@ -43,6 +44,7 @@ set(yarp_openxrheadset_HDRS
   impl/OpenXrQuadLayer.h
   impl/OpenXrInterfaceImpl.h
   SlideManager.h
+  EulerAngles.h
   )
 
 set(yarp_openxrheadset_driver_SRCS
diff --git a/src/devices/openxrheadset/CustomPosePublisher.cpp b/src/devices/openxrheadset/CustomPosePublisher.cpp
index 889c9fe..4d9cfbb 100644
--- a/src/devices/openxrheadset/CustomPosePublisher.cpp
+++ b/src/devices/openxrheadset/CustomPosePublisher.cpp
@@ -51,10 +51,7 @@ void CustomPosePublisher::setRelativeOrientation(const Eigen::Quaternionf &relat
         return;
     }
 
-    m_settings->relativeRotation = relativeOrientation.toRotationMatrix().eulerAngles(
-                static_cast<Eigen::Index>(m_settings->anglesOrder[0]),
-                static_cast<Eigen::Index>(m_settings->anglesOrder[1]),
-                static_cast<Eigen::Index>(m_settings->anglesOrder[2]));
+    m_settings->relativeRotation = eulerAngles(m_settings->anglesOrder, relativeOrientation);
 }
 
 bool CustomPosePublisher::configured() const
@@ -98,11 +95,7 @@ void CustomPosePublisher::updateInputPose(const OpenXrInterface::NamedPoseVeloci
         }
     }
 
-    Eigen::Vector3f parentFrameEulerAngles =
-            input.pose.rotation.toRotationMatrix().eulerAngles(
-                static_cast<Eigen::Index>(m_settings->anglesOrder[0]),
-                static_cast<Eigen::Index>(m_settings->anglesOrder[1]),
-                static_cast<Eigen::Index>(m_settings->anglesOrder[2]));
+    Eigen::Vector3f parentFrameEulerAngles = eulerAngles(m_settings->anglesOrder, input.pose.rotation);
 
     Eigen::Matrix3f absoluteOrientation;
     absoluteOrientation.setIdentity();
@@ -155,20 +148,26 @@ bool CustomPosePublisherSettings::parseFromConfigurationFile(const yarp::os::Bot
         return false;
     }
 
+    if (!EulerAngles::isParametrizationAvailable(parametrization))
+    {
+        yCError(OPENXRHEADSET) << "Failed to parse euler_angles parameter. The parametrization" << parametrization << "is not available.";
+        return false;
+    }
+
     for (size_t i = 0; i < 3; ++i)
     {
         switch (std::tolower(parametrization[i]))
         {
         case 'x':
-            anglesOrder[i] = RotationAxis::x;
+            anglesOrder[i] = RotationAxis::X;
             break;
 
         case 'y':
-            anglesOrder[i] = RotationAxis::y;
+            anglesOrder[i] = RotationAxis::Y;
             break;
 
         case 'z':
-            anglesOrder[i] = RotationAxis::z;
+            anglesOrder[i] = RotationAxis::Z;
             break;
 
         default:
@@ -212,7 +211,7 @@ bool CustomPosePublisherSettings::parseFromConfigurationFile(const yarp::os::Bot
             {
                 yCError(OPENXRHEADSET) << "Failed to parse" << maskName
                                        << "at element" << i
-                                       << ". Only float numbers and the special carachter '*' are allowed";
+                                       << ". Only float numbers and the special carachter \"*\" are allowed";
                 return false;
             }
         }
diff --git a/src/devices/openxrheadset/CustomPosePublisher.h b/src/devices/openxrheadset/CustomPosePublisher.h
index 1cd9c7a..16a9a60 100644
--- a/src/devices/openxrheadset/CustomPosePublisher.h
+++ b/src/devices/openxrheadset/CustomPosePublisher.h
@@ -11,6 +11,7 @@
 
 #include <PosePublisher.h>
 #include <OpenXrInterface.h>
+#include <EulerAngles.h>
 #include <array>
 #include <string>
 #include <memory>
@@ -18,13 +19,6 @@
 
 struct CustomPosePublisherSettings : PosePublisherSettings
 {
-    enum class RotationAxis : Eigen::Index
-    {
-        x = 0,
-        y = 1,
-        z = 2
-    };
-
     std::string parentFrame;
     std::string name;
 
@@ -32,7 +26,7 @@ struct CustomPosePublisherSettings : PosePublisherSettings
     Eigen::Vector3f relativePosition;
 
     std::array<bool, 3> rotationMask = {true, true, true};
-    std::array<RotationAxis, 3> anglesOrder = {RotationAxis::y, RotationAxis::x, RotationAxis::z};
+    std::array<RotationAxis, 3> anglesOrder = {RotationAxis::Y, RotationAxis::X, RotationAxis::Z};
     Eigen::Vector3f relativeRotation;
 
     bool parseFromConfigurationFile(const yarp::os::Bottle& inputGroup);
diff --git a/src/devices/openxrheadset/EulerAngles.cpp b/src/devices/openxrheadset/EulerAngles.cpp
new file mode 100644
index 0000000..17be4fe
--- /dev/null
+++ b/src/devices/openxrheadset/EulerAngles.cpp
@@ -0,0 +1,268 @@
+/*
+ * Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This software may be modified and distributed under the terms of the
+ * BSD-2-Clause license. See the accompanying LICENSE file for details.
+ */
+
+//This code has been taken from https://www.geometrictools.com/Documentation/EulerAngles.pdf
+
+#include <EulerAngles.h>
+#include <cmath>
+#include <unordered_set>
+#include <algorithm>
+#include <yarp/os/LogStream.h>
+#include <OpenXrHeadsetLogComponent.h>
+
+Eigen::Vector3f EulerAngles::XYZ(const Eigen::Matrix3f &r)
+{
+    Eigen::Vector3f output;
+    float& thetaX = output[0];
+    float& thetaY = output[1];
+    float& thetaZ = output[2];
+
+    if ( r(0, 2) < +1)
+    {
+        if ( r(0, 2) > -1)
+        {
+            thetaY = asin(r(0, 2));
+            thetaX = atan2(-r(1, 2) , r(2, 2));
+            thetaZ = atan2(-r(0, 1) , r(0, 0));
+        }
+        else // r 0 2 = -1
+        {
+            // Not a unique solution: thetaZ - thetaX = atan2(r10 , r11)
+            thetaY = -M_PI / 2;
+            thetaX = -atan2(r(1,0) , r(1, 1));
+            thetaZ = 0;
+        }
+    }
+    else // r02 = +1
+    {
+        // Not a unique solution: thetaZ + thetaX = atan2( r10, r11)
+        thetaY = +M_PI / 2;
+        thetaX = atan2(r(1,0) , r(1,1));
+        thetaZ = 0;
+    }
+
+    return output;
+}
+
+Eigen::Vector3f EulerAngles::XZY(const Eigen::Matrix3f &r)
+{
+    Eigen::Vector3f output;
+    float& thetaX = output[0];
+    float& thetaZ = output[1];
+    float& thetaY = output[2];
+
+    if (r(0, 1) < +1)
+    {
+        if ( r(0, 1) > -1)
+        {
+            thetaZ = asin(-r(0, 1));
+            thetaX = atan2(r(2, 1) , r(1,1));
+            thetaY = atan2(r(0,2) , r(0,0));
+        }
+        else // r01 = -1
+        {
+            // Not a unique solution: thetaY - thetaX = atan2(-r20 , r22)
+            thetaZ = +M_PI / 2;
+            thetaX = -atan2(-r(2, 0) , r(2, 2));
+            thetaY = 0;
+        }
+    }
+    else // r01 = +1
+    {
+        // Not a unique solution: thetaY + thetaX = atan2(-r20, r22)
+        thetaZ = -M_PI / 2;
+        thetaX = atan2(-r(2,0) , r(2, 2));
+        thetaY = 0;
+    }
+
+    return output;
+}
+
+Eigen::Vector3f EulerAngles::YXZ(const Eigen::Matrix3f &r)
+{
+    Eigen::Vector3f output;
+    float& thetaY = output[0];
+    float& thetaX = output[1];
+    float& thetaZ = output[2];
+
+    if (r(1, 2) < +1)
+    {
+        if ( r(1, 2) > -1)
+        {
+            thetaX = asin(-r(1, 2));
+            thetaY = atan2(r(0, 2), r(2, 2));
+            thetaZ = atan2(r(1, 0), r(1,1));
+        }
+        else // r12 = -1
+        {
+            // Not a unique solution : thetaZ - thetaY = atan2(-r01, r00)
+            thetaX = +M_PI / 2;
+            thetaY = -atan2(-r(0,1) , r(0,0));
+            thetaZ = 0;
+        }
+    }
+    else // r12 = +1
+    {
+        // Not a unique solution: thetaZ + thetaY = atan2(-r01 , r00)
+        thetaX = -M_PI / 2 ;
+        thetaY = atan2(-r(0,1) , r(0,0));
+        thetaZ = 0;
+    }
+
+    return output;
+}
+
+Eigen::Vector3f EulerAngles::YZX(const Eigen::Matrix3f &r)
+{
+    Eigen::Vector3f output;
+    float& thetaY = output[0];
+    float& thetaZ = output[1];
+    float& thetaX = output[2];
+
+    if (r(1, 0) < +1)
+    {
+        if (r(1, 0) > -1)
+        {
+            thetaZ = asin(r(1, 0));
+            thetaY = atan2(-r(2, 0), r(0, 0));
+            thetaX = atan2(-r(1, 2) , r(1,1));
+        }
+        else // r10 = -1
+        {
+            // Not a unique solution: thetaX - thetaY = atan2(r21, r22)
+            thetaZ = -M_PI / 2 ;
+            thetaY = -atan2(r(2, 1) , r(2, 2));
+            thetaX = 0;
+        }
+    }
+    else
+    {
+        // Not a unique solution: thetaX + thetaY = atan2(r21, r22)
+        thetaZ = +M_PI / 2 ;
+        thetaY = atan2(r(2, 1), r(2, 2));
+        thetaX = 0;
+    }
+
+    return output;
+}
+
+Eigen::Vector3f EulerAngles::ZXY(const Eigen::Matrix3f &r)
+{
+    Eigen::Vector3f output;
+    float& thetaZ = output[0];
+    float& thetaX = output[1];
+    float& thetaY = output[2];
+
+    if (r(2, 1) < +1)
+    {
+        if (r(2, 1) > -1)
+        {
+            thetaX = asin(r(2,1));
+            thetaZ = atan2(-r(0, 1) , r(1, 1));
+            thetaY = atan2(-r(2, 0) , r(2, 2));
+        }
+        else // r21 = -1
+        {
+            // Not a unique solution: thetaY - thetaZ = atan2(r02, r00)
+            thetaX = -M_PI / 2;
+            thetaZ = -atan2(r(0, 2), r(0, 0));
+            thetaY = 0;
+        }
+    }
+    else // r21 = +1
+    {
+        // Not a unique solution: thetaY + thetaZ = atan2(r02, r00)
+        thetaX = +M_PI / 2;
+        thetaZ = atan2(r(0, 2), r(0, 0));
+        thetaY = 0;
+    }
+
+    return output;
+}
+
+Eigen::Vector3f EulerAngles::ZYX(const Eigen::Matrix3f &r)
+{
+    Eigen::Vector3f output;
+    float& thetaZ = output[0];
+    float& thetaY = output[1];
+    float& thetaX = output[2];
+
+    if (r(2, 0) < +1)
+    {
+        if (r(2, 0) > -1)
+        {
+            thetaY = asin(-r(2, 0));
+            thetaZ = atan2(r(1, 0), r(0, 0));
+            thetaX = atan2(r(2, 1), r(2, 2));
+        }
+        else // r20 = -1
+        {
+            // Not a unique solution: thetaX - thetaZ = atan2(-r12 , r11)
+            thetaY = +M_PI/ 2;
+            thetaZ = -atan2(-r(1, 2), r(1, 1));
+            thetaX = 0;
+        }
+    }
+    else // r20 = +1
+    {
+        // Not a unique solution: thetaX + thetaZ = atan2(-r12, r11)
+        thetaY = -M_PI / 2 ;
+        thetaZ = atan2(-r(1, 2), r(1, 1));
+        thetaX = 0;
+    }
+
+    return output;
+}
+
+bool EulerAngles::isParametrizationAvailable(const std::string &parametrization)
+{
+    std::unordered_set<std::string> availableParametrizations{"XYZ", "XZY", "YXZ", "YZX", "ZXY", "ZYX"};
+    std::string upperCased = parametrization;
+    std::transform(upperCased.begin(), upperCased.end(),upperCased.begin(), ::toupper);
+
+    return availableParametrizations.find(upperCased) != availableParametrizations.end();
+}
+
+Eigen::Vector3f eulerAngles(const std::array<RotationAxis, 3> &parametrization, const Eigen::Quaternionf &rotation)
+{
+
+    if (parametrization == std::array<RotationAxis, 3>{RotationAxis::X, RotationAxis::Y, RotationAxis::Z})
+    {
+        return EulerAngles::XYZ(rotation.toRotationMatrix());
+    }
+
+    if (parametrization == std::array<RotationAxis, 3>{RotationAxis::X, RotationAxis::Z, RotationAxis::Y})
+    {
+        return EulerAngles::XZY(rotation.toRotationMatrix());
+    }
+
+    if (parametrization == std::array<RotationAxis, 3>{RotationAxis::Y, RotationAxis::X, RotationAxis::Z})
+    {
+        return EulerAngles::YXZ(rotation.toRotationMatrix());
+    }
+
+    if (parametrization == std::array<RotationAxis, 3>{RotationAxis::Y, RotationAxis::Z, RotationAxis::X})
+    {
+        return EulerAngles::YZX(rotation.toRotationMatrix());
+    }
+
+    if (parametrization == std::array<RotationAxis, 3>{RotationAxis::Z, RotationAxis::X, RotationAxis::Y})
+    {
+        return EulerAngles::ZXY(rotation.toRotationMatrix());
+    }
+
+    if (parametrization == std::array<RotationAxis, 3>{RotationAxis::Z, RotationAxis::Y, RotationAxis::X})
+    {
+        return EulerAngles::ZYX(rotation.toRotationMatrix());
+    }
+
+    yCError(OPENXRHEADSET) << "Parametrization not supported.";
+
+    return Eigen::Vector3f::Zero();
+}
+
diff --git a/src/devices/openxrheadset/EulerAngles.h b/src/devices/openxrheadset/EulerAngles.h
new file mode 100644
index 0000000..d7d9e97
--- /dev/null
+++ b/src/devices/openxrheadset/EulerAngles.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT)
+ * All rights reserved.
+ *
+ * This software may be modified and distributed under the terms of the
+ * BSD-2-Clause license. See the accompanying LICENSE file for details.
+ */
+
+
+#ifndef YARP_DEV_EULERANGLES_H
+#define YARP_DEV_EULERANGLES_H
+
+#include <Eigen/Geometry>
+#include <Eigen/Dense>
+#include <string>
+
+
+enum class RotationAxis : Eigen::Index
+{
+    X = 0,
+    Y = 1,
+    Z = 2
+};
+
+namespace EulerAngles
+{
+
+Eigen::Vector3f XYZ(const Eigen::Matrix3f& r);
+
+Eigen::Vector3f XZY(const Eigen::Matrix3f& r);
+
+Eigen::Vector3f YXZ(const Eigen::Matrix3f& r);
+
+Eigen::Vector3f YZX(const Eigen::Matrix3f& r);
+
+Eigen::Vector3f ZXY(const Eigen::Matrix3f& r);
+
+Eigen::Vector3f ZYX(const Eigen::Matrix3f& r);
+
+bool isParametrizationAvailable(const std::string& parametrization);
+
+}
+
+Eigen::Vector3f eulerAngles(const std::array<RotationAxis, 3>& parametrization, const Eigen::Quaternionf& rotation);
+
+
+
+#endif // YARP_DEV_EULERANGLES_H

From dc7e526749dbb4717b9b10b1a5490e65d9ea9fdf Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Fri, 23 Sep 2022 19:09:32 +0200
Subject: [PATCH 09/14] Publishing custom poses wrt openxr_root frame.

---
 CMakeLists.txt                                 |  4 ++++
 .../openxrheadset/CustomPosePublisher.cpp      | 18 +++++-------------
 src/devices/openxrheadset/PosesManager.cpp     | 18 +++++++++---------
 3 files changed, 18 insertions(+), 22 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index d924b7c..938c904 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -36,6 +36,10 @@ set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
 # installation directories.
 if(MSVC)
     set(CMAKE_DEBUG_POSTFIX "d")
+    #_USE_MATH_DEFINES is to have constants like M_PI defined also on Windows
+    add_definitions(-D_USE_MATH_DEFINES)
+    #NOMINMAX is to avoid windows.h defining its own versions of min and max
+    add_definitions(-DNOMINMAX)
 endif()
 
 # Build position independent code.
diff --git a/src/devices/openxrheadset/CustomPosePublisher.cpp b/src/devices/openxrheadset/CustomPosePublisher.cpp
index 4d9cfbb..3605049 100644
--- a/src/devices/openxrheadset/CustomPosePublisher.cpp
+++ b/src/devices/openxrheadset/CustomPosePublisher.cpp
@@ -18,8 +18,6 @@ void CustomPosePublisher::configure(std::shared_ptr<CustomPosePublisherSettings>
         m_settings = settings;
         m_parentFrame = settings->parentFrame;
         m_name = settings->name;
-        //We publish the transform relative to the parent frame
-        m_settings->rawRootFrame = m_parentFrame;
         PosePublisher::configurePublisher(settings);
     }
 }
@@ -80,26 +78,22 @@ void CustomPosePublisher::updateInputPose(const OpenXrInterface::NamedPoseVeloci
     output.velocity.linearValid = false;
     output.velocity.angularValid = false;
 
-    Eigen::Quaternionf inverseRotation = input.pose.rotation.inverse();
-    Eigen::Vector3f inversePosition = -inverseRotation.toRotationMatrix() * input.pose.position;
+    Eigen::Vector3f relativePosition = input.pose.position + input.pose.rotation * m_settings->relativePosition;
 
     for (size_t i = 0; i < 3; ++i)
     {
         if (m_settings->positionMask[i])
         {
-            output.pose.position[i] = m_settings->relativePosition[i];
+            output.pose.position[i] = relativePosition[i];
         }
         else
         {
-            output.pose.position[i] = inversePosition[i];
+            output.pose.position[i] = 0.0;
         }
     }
 
     Eigen::Vector3f parentFrameEulerAngles = eulerAngles(m_settings->anglesOrder, input.pose.rotation);
 
-    Eigen::Matrix3f absoluteOrientation;
-    absoluteOrientation.setIdentity();
-
     for (size_t i = 0; i < 3; ++i)
     {
         float angle = 0;
@@ -108,12 +102,10 @@ void CustomPosePublisher::updateInputPose(const OpenXrInterface::NamedPoseVeloci
             angle = parentFrameEulerAngles[i] + m_settings->relativeRotation[i];
 
         }
-        absoluteOrientation = absoluteOrientation * Eigen::AngleAxisf(angle,
-                                                                      Eigen::Vector3f::Unit(static_cast<Eigen::Index>(m_settings->anglesOrder[i])));
+        output.pose.rotation = output.pose.rotation * Eigen::AngleAxisf(angle,
+                                                                        Eigen::Vector3f::Unit(static_cast<Eigen::Index>(m_settings->anglesOrder[i])));
     }
 
-    output.pose.rotation = inverseRotation * Eigen::Quaternionf(absoluteOrientation);
-
     PosePublisher::updateInputPose(output);
 }
 
diff --git a/src/devices/openxrheadset/PosesManager.cpp b/src/devices/openxrheadset/PosesManager.cpp
index 5b20cd0..72409ac 100644
--- a/src/devices/openxrheadset/PosesManager.cpp
+++ b/src/devices/openxrheadset/PosesManager.cpp
@@ -35,7 +35,7 @@ void PosesManager::initialize(const std::string &editedRootFrame, const std::vec
         m_customPoses.emplace_back();
         auto customOptions = std::make_shared<CustomPosePublisherSettings>(customPose);
         customOptions->tfPublisher = m_settings->tfPublisher;
-        customOptions->rawRootFrame = m_settings->rawRootFrame;
+        customOptions->rawRootFrame = editedRootFrame;
         m_customPoses.back().configure(customOptions);
     }
 
@@ -77,26 +77,26 @@ void PosesManager::publishFrames()
         {
             const auto& labelIt = m_labelsReverseMap.find(relativeFrame);
 
-            OpenXrInterface::NamedPoseVelocity poseRawRootToParent;
+            OpenXrInterface::NamedPoseVelocity rawRootToParentPose;
 
             if (labelIt != m_labelsReverseMap.end()) //The parent frame name is a label
             {
-                poseRawRootToParent = m_poses[labelIt->second].data();
+                rawRootToParentPose = m_poses[labelIt->second].data();
             }
             else
             {
-                poseRawRootToParent = m_poses[relativeFrame].data();
+                rawRootToParentPose = m_poses[relativeFrame].data();
             }
 
             //The pose of the parent frame is expressed with respect to the raw root, i.e. the frame wrt we recieve the poses from OpenXR.
             //The user is interested on the root_frame that can be aligned to headset via RPC call.
 
-            parentFramePose.name = poseRawRootToParent.name;
-            parentFramePose.pose.positionValid = poseRawRootToParent.pose.positionValid;
-            parentFramePose.pose.rotationValid = poseRawRootToParent.pose.rotationValid;
+            parentFramePose.name = rawRootToParentPose.name;
+            parentFramePose.pose.positionValid = rawRootToParentPose.pose.positionValid;
+            parentFramePose.pose.rotationValid = rawRootToParentPose.pose.rotationValid;
 
-            parentFramePose.pose.position = m_rootFrameRawRelativePoseInverse.position + m_rootFrameRawRelativePoseInverse.rotation * poseRawRootToParent.pose.position;
-            parentFramePose.pose.rotation = m_rootFrameRawRelativePoseInverse.rotation * poseRawRootToParent.pose.rotation;
+            parentFramePose.pose.position = m_rootFrameRawRelativePoseInverse.position + m_rootFrameRawRelativePoseInverse.rotation * rawRootToParentPose.pose.position;
+            parentFramePose.pose.rotation = m_rootFrameRawRelativePoseInverse.rotation * rawRootToParentPose.pose.rotation;
 
         }
 

From 04e57d4122f6e6b8d988cf4908af265a7d71be6b Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 26 Sep 2022 11:10:39 +0200
Subject: [PATCH 10/14] Fixed some unitialized variables.

Used a better naming for the root frame in tf settings.
---
 src/devices/openxrheadset/CustomPosePublisher.cpp | 1 +
 src/devices/openxrheadset/OpenXrHeadset.cpp       | 2 +-
 src/devices/openxrheadset/PosePublisher.cpp       | 2 +-
 src/devices/openxrheadset/PosePublisher.h         | 2 +-
 src/devices/openxrheadset/PosesManager.cpp        | 7 ++++---
 5 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/src/devices/openxrheadset/CustomPosePublisher.cpp b/src/devices/openxrheadset/CustomPosePublisher.cpp
index 3605049..cf13d39 100644
--- a/src/devices/openxrheadset/CustomPosePublisher.cpp
+++ b/src/devices/openxrheadset/CustomPosePublisher.cpp
@@ -94,6 +94,7 @@ void CustomPosePublisher::updateInputPose(const OpenXrInterface::NamedPoseVeloci
 
     Eigen::Vector3f parentFrameEulerAngles = eulerAngles(m_settings->anglesOrder, input.pose.rotation);
 
+    output.pose.rotation.setIdentity();
     for (size_t i = 0; i < 3; ++i)
     {
         float angle = 0;
diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp
index 95ac994..3e07678 100644
--- a/src/devices/openxrheadset/OpenXrHeadset.cpp
+++ b/src/devices/openxrheadset/OpenXrHeadset.cpp
@@ -269,7 +269,7 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
 
     FilteredPosePublisherSettings posePublisherSettings;
     posePublisherSettings.tfPublisher = m_tfPublisher;
-    posePublisherSettings.rawRootFrame = m_rootFrameRaw;
+    posePublisherSettings.tfBaseFrame = m_rootFrameRaw;
     posePublisherSettings.period = getPeriod();
     posePublisherSettings.checks.maxDistance = cfg.check("pose_check_max_distance", yarp::os::Value(0.1)).asFloat64();
     posePublisherSettings.checks.maxAngularDistanceInRad = cfg.check("pose_check_max_angle_rad", yarp::os::Value(0.5)).asFloat64();
diff --git a/src/devices/openxrheadset/PosePublisher.cpp b/src/devices/openxrheadset/PosePublisher.cpp
index 49d510b..db35cc4 100644
--- a/src/devices/openxrheadset/PosePublisher.cpp
+++ b/src/devices/openxrheadset/PosePublisher.cpp
@@ -127,7 +127,7 @@ void PosePublisher::publish()
         }
     }
 
-    if (!m_baseSettings->tfPublisher->setTransform(m_label, m_baseSettings->rawRootFrame, m_localPose))
+    if (!m_baseSettings->tfPublisher->setTransform(m_label, m_baseSettings->tfBaseFrame, m_localPose))
     {
         yCWarning(OPENXRHEADSET) << "Failed to publish" << m_label << "frame.";
     }
diff --git a/src/devices/openxrheadset/PosePublisher.h b/src/devices/openxrheadset/PosePublisher.h
index e1d5f75..d2bb895 100644
--- a/src/devices/openxrheadset/PosePublisher.h
+++ b/src/devices/openxrheadset/PosePublisher.h
@@ -17,7 +17,7 @@
 struct PosePublisherSettings
 {
     yarp::dev::IFrameTransform* tfPublisher{nullptr};
-    std::string rawRootFrame;
+    std::string tfBaseFrame;
 };
 
 class PosePublisher
diff --git a/src/devices/openxrheadset/PosesManager.cpp b/src/devices/openxrheadset/PosesManager.cpp
index 72409ac..3338c2c 100644
--- a/src/devices/openxrheadset/PosesManager.cpp
+++ b/src/devices/openxrheadset/PosesManager.cpp
@@ -17,9 +17,9 @@ void PosesManager::initialize(const std::string &editedRootFrame, const std::vec
 
     auto rootFramePublisherOptions = std::make_shared<CustomPosePublisherSettings>();
     rootFramePublisherOptions->tfPublisher = m_settings->tfPublisher;
-    rootFramePublisherOptions->rawRootFrame = m_settings->rawRootFrame;
+    rootFramePublisherOptions->tfBaseFrame = m_settings->tfBaseFrame;
     rootFramePublisherOptions->name = editedRootFrame;
-    rootFramePublisherOptions->parentFrame = m_settings->rawRootFrame;
+    rootFramePublisherOptions->parentFrame = m_settings->tfBaseFrame;
     rootFramePublisherOptions->relativePosition.setZero();
     rootFramePublisherOptions->relativeRotation.setZero();
     m_rootFramePublisher.configure(rootFramePublisherOptions);
@@ -35,11 +35,12 @@ void PosesManager::initialize(const std::string &editedRootFrame, const std::vec
         m_customPoses.emplace_back();
         auto customOptions = std::make_shared<CustomPosePublisherSettings>(customPose);
         customOptions->tfPublisher = m_settings->tfPublisher;
-        customOptions->rawRootFrame = editedRootFrame;
+        customOptions->tfBaseFrame = editedRootFrame;
         m_customPoses.back().configure(customOptions);
     }
 
     m_rootPose = OpenXrInterface::NamedPoseVelocity::Identity(editedRootFrame);
+    m_rootFrameRawRelativePoseInverse = OpenXrInterface::NamedPoseVelocity::Identity(editedRootFrame).pose;
 }
 
 std::vector<OpenXrInterface::NamedPoseVelocity> &PosesManager::inputs()

From 9b510b80c7b34edd4837131c80b5b93769d848a2 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 26 Sep 2022 13:35:41 +0200
Subject: [PATCH 11/14] Exposing the tfBaseFrame.

---
 src/devices/openxrheadset/PosePublisher.cpp | 15 ++++++++++++---
 src/devices/openxrheadset/PosePublisher.h   |  6 ++++--
 2 files changed, 16 insertions(+), 5 deletions(-)

diff --git a/src/devices/openxrheadset/PosePublisher.cpp b/src/devices/openxrheadset/PosePublisher.cpp
index db35cc4..bc51b2c 100644
--- a/src/devices/openxrheadset/PosePublisher.cpp
+++ b/src/devices/openxrheadset/PosePublisher.cpp
@@ -41,7 +41,11 @@ PosePublisher::PosePublisher()
 
 void PosePublisher::configurePublisher(std::shared_ptr<PosePublisherSettings> settings)
 {
-    m_baseSettings = settings;
+    if (!settings)
+        return;
+
+    m_tfBaseFrame = settings->tfBaseFrame;
+    m_tfPublisher = settings->tfPublisher;
 }
 
 void PosePublisher::setLabel(const std::string &label)
@@ -54,9 +58,14 @@ const std::string &PosePublisher::label() const
     return m_label;
 }
 
+const std::string &PosePublisher::tfBaseFrame() const
+{
+    return m_tfBaseFrame;
+}
+
 bool PosePublisher::configured() const
 {
-    return m_baseSettings != nullptr && m_baseSettings->tfPublisher != nullptr;
+    return m_tfPublisher != nullptr;
 }
 
 void PosePublisher::updateInputPose(const OpenXrInterface::NamedPoseVelocity &input)
@@ -127,7 +136,7 @@ void PosePublisher::publish()
         }
     }
 
-    if (!m_baseSettings->tfPublisher->setTransform(m_label, m_baseSettings->tfBaseFrame, m_localPose))
+    if (!m_tfPublisher->setTransform(m_label, m_tfBaseFrame, m_localPose))
     {
         yCWarning(OPENXRHEADSET) << "Failed to publish" << m_label << "frame.";
     }
diff --git a/src/devices/openxrheadset/PosePublisher.h b/src/devices/openxrheadset/PosePublisher.h
index d2bb895..f4c6e75 100644
--- a/src/devices/openxrheadset/PosePublisher.h
+++ b/src/devices/openxrheadset/PosePublisher.h
@@ -22,10 +22,10 @@ struct PosePublisherSettings
 
 class PosePublisher
 {
-    std::string m_label;
+    std::string m_label, m_tfBaseFrame;
     OpenXrInterface::NamedPoseVelocity m_data, m_previouslyPublishedData;
     yarp::sig::Matrix m_localPose;
-    std::shared_ptr<PosePublisherSettings> m_baseSettings{nullptr};
+    yarp::dev::IFrameTransform* m_tfPublisher{nullptr};
     bool m_active{false};
     bool m_publishedOnce{false};
     double m_lastWarningTime{0.0};
@@ -51,6 +51,8 @@ class PosePublisher
 
     const std::string& label() const;
 
+    const std::string& tfBaseFrame() const;
+
     virtual bool configured() const;
 
     virtual void updateInputPose(const OpenXrInterface::NamedPoseVelocity& input);

From 7e057fab1f2b75ce9e5abaf83ed5d6dd643b1ed4 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 26 Sep 2022 13:36:00 +0200
Subject: [PATCH 12/14] Added possibility to have custom poses defined wrt
 other poses.

---
 src/devices/openxrheadset/PosesManager.cpp | 58 +++++++++++++++-------
 src/devices/openxrheadset/PosesManager.h   |  2 +
 2 files changed, 43 insertions(+), 17 deletions(-)

diff --git a/src/devices/openxrheadset/PosesManager.cpp b/src/devices/openxrheadset/PosesManager.cpp
index 3338c2c..abfad35 100644
--- a/src/devices/openxrheadset/PosesManager.cpp
+++ b/src/devices/openxrheadset/PosesManager.cpp
@@ -37,6 +37,7 @@ void PosesManager::initialize(const std::string &editedRootFrame, const std::vec
         customOptions->tfPublisher = m_settings->tfPublisher;
         customOptions->tfBaseFrame = editedRootFrame;
         m_customPoses.back().configure(customOptions);
+        m_customPosesMap[customPose.name] = m_customPoses.size() - 1;
     }
 
     m_rootPose = OpenXrInterface::NamedPoseVelocity::Identity(editedRootFrame);
@@ -65,8 +66,9 @@ void PosesManager::publishFrames()
         publisher.updateInputPose(inputPose);
     }
 
-    for (auto& customPose : m_customPoses)
+    for (size_t i = 0; i < m_customPoses.size(); ++i)
     {
+        auto& customPose = m_customPoses[i];
         const std::string& relativeFrame = customPose.relativeFrame();
         OpenXrInterface::NamedPoseVelocity parentFramePose;
 
@@ -76,29 +78,51 @@ void PosesManager::publishFrames()
         }
         else
         {
-            const auto& labelIt = m_labelsReverseMap.find(relativeFrame);
+            auto labelIt = m_labelsReverseMap.find(relativeFrame);
+            auto posesIt = m_poses.find(relativeFrame);
+            auto customPoseIt = m_customPosesMap.find(relativeFrame);
+
+            if (labelIt != m_labelsReverseMap.end() || posesIt != m_poses.end())
+            {
+
+                OpenXrInterface::NamedPoseVelocity rawRootToParentPose;
+
+                if (labelIt != m_labelsReverseMap.end()) //The parent frame name is a label
+                {
+                    rawRootToParentPose = m_poses[labelIt->second].data();
+                }
+                else
+                {
+                    rawRootToParentPose = posesIt->second.data();
+                }
+
+                //The pose of the parent frame is expressed with respect to the raw root, i.e. the frame wrt we receive the poses from OpenXR.
+                //We publish the custom poses wrt the modified root, i.e. openxr_origin, thus we need to provide the parent frame wrt this other frame
 
-            OpenXrInterface::NamedPoseVelocity rawRootToParentPose;
+                parentFramePose.name = rawRootToParentPose.name;
+                parentFramePose.pose.positionValid = rawRootToParentPose.pose.positionValid;
+                parentFramePose.pose.rotationValid = rawRootToParentPose.pose.rotationValid;
 
-            if (labelIt != m_labelsReverseMap.end()) //The parent frame name is a label
+                parentFramePose.pose.position = m_rootFrameRawRelativePoseInverse.position + m_rootFrameRawRelativePoseInverse.rotation * rawRootToParentPose.pose.position;
+                parentFramePose.pose.rotation = m_rootFrameRawRelativePoseInverse.rotation * rawRootToParentPose.pose.rotation;
+            }
+            else if (customPoseIt != m_customPosesMap.end())
             {
-                rawRootToParentPose = m_poses[labelIt->second].data();
+                if (customPoseIt->second < i)
+                {
+                    parentFramePose = m_customPoses[customPoseIt->second].data();
+                }
+                else
+                {
+                    yCWarningThrottle(OPENXRHEADSET, 10) << "The custom pose" << customPose.name()
+                                                         << "requires" << relativeFrame << "that is not updated yet (custom poses are updated in order depending on the configuration file).";
+                }
             }
             else
             {
-                rawRootToParentPose = m_poses[relativeFrame].data();
+                yCWarningThrottle(OPENXRHEADSET, 10) << "The custom pose" << customPose.name()
+                                                     << "requires" << relativeFrame << "but it does not seem to be available.";
             }
-
-            //The pose of the parent frame is expressed with respect to the raw root, i.e. the frame wrt we recieve the poses from OpenXR.
-            //The user is interested on the root_frame that can be aligned to headset via RPC call.
-
-            parentFramePose.name = rawRootToParentPose.name;
-            parentFramePose.pose.positionValid = rawRootToParentPose.pose.positionValid;
-            parentFramePose.pose.rotationValid = rawRootToParentPose.pose.rotationValid;
-
-            parentFramePose.pose.position = m_rootFrameRawRelativePoseInverse.position + m_rootFrameRawRelativePoseInverse.rotation * rawRootToParentPose.pose.position;
-            parentFramePose.pose.rotation = m_rootFrameRawRelativePoseInverse.rotation * rawRootToParentPose.pose.rotation;
-
         }
 
         customPose.updateInputPose(parentFramePose);
diff --git a/src/devices/openxrheadset/PosesManager.h b/src/devices/openxrheadset/PosesManager.h
index 65bfd0a..f70724e 100644
--- a/src/devices/openxrheadset/PosesManager.h
+++ b/src/devices/openxrheadset/PosesManager.h
@@ -26,6 +26,8 @@ class PosesManager
 
     std::vector<CustomPosePublisher> m_customPoses;
 
+    std::unordered_map<std::string, size_t> m_customPosesMap;
+
     std::unordered_map<std::string, std::string> m_labelsReverseMap;
 
     OpenXrInterface::NamedPoseVelocity m_rootPose;

From be8f14fcabda36f0fc028a4f42e6900c9ebefda0 Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Mon, 26 Sep 2022 15:22:32 +0200
Subject: [PATCH 13/14] The computation of the custom pose rotation is done
 relative to the parent frame.

---
 src/devices/openxrheadset/CustomPosePublisher.cpp | 15 +++++++++------
 1 file changed, 9 insertions(+), 6 deletions(-)

diff --git a/src/devices/openxrheadset/CustomPosePublisher.cpp b/src/devices/openxrheadset/CustomPosePublisher.cpp
index cf13d39..9b6d432 100644
--- a/src/devices/openxrheadset/CustomPosePublisher.cpp
+++ b/src/devices/openxrheadset/CustomPosePublisher.cpp
@@ -92,20 +92,23 @@ void CustomPosePublisher::updateInputPose(const OpenXrInterface::NamedPoseVeloci
         }
     }
 
-    Eigen::Vector3f parentFrameEulerAngles = eulerAngles(m_settings->anglesOrder, input.pose.rotation);
 
-    output.pose.rotation.setIdentity();
+    Eigen::Vector3f inverseParentFrameEulerAngles = eulerAngles(m_settings->anglesOrder, input.pose.rotation.inverse());
+
+    Eigen::Quaternionf relativeRotation;
+    relativeRotation.setIdentity();
     for (size_t i = 0; i < 3; ++i)
     {
-        float angle = 0;
+        float angle = inverseParentFrameEulerAngles[i];
         if (m_settings->rotationMask[i])
         {
-            angle = parentFrameEulerAngles[i] + m_settings->relativeRotation[i];
+            angle = m_settings->relativeRotation[i];
 
         }
-        output.pose.rotation = output.pose.rotation * Eigen::AngleAxisf(angle,
-                                                                        Eigen::Vector3f::Unit(static_cast<Eigen::Index>(m_settings->anglesOrder[i])));
+        relativeRotation = relativeRotation * Eigen::AngleAxisf(angle,
+                                                                Eigen::Vector3f::Unit(static_cast<Eigen::Index>(m_settings->anglesOrder[i])));
     }
+    output.pose.rotation = input.pose.rotation * relativeRotation;
 
     PosePublisher::updateInputPose(output);
 }

From 4f3d15e93b65e1369f6dab27ca29081ec8468eec Mon Sep 17 00:00:00 2001
From: Stefano <stefano.dafarra@gmail.com>
Date: Tue, 27 Sep 2022 13:51:30 +0200
Subject: [PATCH 14/14] Added RPC call to set custom pose offsets.

---
 .../openxrheadset/CustomPosePublisher.cpp     | 10 +++++
 .../openxrheadset/CustomPosePublisher.h       |  2 +
 src/devices/openxrheadset/OpenXrHeadset.cpp   | 18 ++++++++
 src/devices/openxrheadset/OpenXrHeadset.h     | 21 ++++++++++
 src/devices/openxrheadset/PosesManager.cpp    | 42 +++++++++++++++++++
 src/devices/openxrheadset/PosesManager.h      |  6 +++
 .../thrifts/OpenXrHeadsetCommands.thrift      | 21 ++++++++++
 7 files changed, 120 insertions(+)

diff --git a/src/devices/openxrheadset/CustomPosePublisher.cpp b/src/devices/openxrheadset/CustomPosePublisher.cpp
index 9b6d432..a7152be 100644
--- a/src/devices/openxrheadset/CustomPosePublisher.cpp
+++ b/src/devices/openxrheadset/CustomPosePublisher.cpp
@@ -52,6 +52,16 @@ void CustomPosePublisher::setRelativeOrientation(const Eigen::Quaternionf &relat
     m_settings->relativeRotation = eulerAngles(m_settings->anglesOrder, relativeOrientation);
 }
 
+void CustomPosePublisher::setRelativeOrientation(const Eigen::Vector3f &relativeOrientationEulerAngles)
+{
+    if (!configured())
+    {
+        return;
+    }
+
+    m_settings->relativeRotation = relativeOrientationEulerAngles;
+}
+
 bool CustomPosePublisher::configured() const
 {
     return m_settings != nullptr && !m_settings->name.empty() && PosePublisher::configured();
diff --git a/src/devices/openxrheadset/CustomPosePublisher.h b/src/devices/openxrheadset/CustomPosePublisher.h
index 16a9a60..4f385d3 100644
--- a/src/devices/openxrheadset/CustomPosePublisher.h
+++ b/src/devices/openxrheadset/CustomPosePublisher.h
@@ -51,6 +51,8 @@ class CustomPosePublisher : public PosePublisher
 
     void setRelativeOrientation(const Eigen::Quaternionf& relativeOrientation);
 
+    void setRelativeOrientation(const Eigen::Vector3f& relativeOrientationEulerAngles);
+
     virtual bool configured() const override;
 
     virtual void updateInputPose(const OpenXrInterface::NamedPoseVelocity& input) override;
diff --git a/src/devices/openxrheadset/OpenXrHeadset.cpp b/src/devices/openxrheadset/OpenXrHeadset.cpp
index 3e07678..e775874 100644
--- a/src/devices/openxrheadset/OpenXrHeadset.cpp
+++ b/src/devices/openxrheadset/OpenXrHeadset.cpp
@@ -845,3 +845,21 @@ bool yarp::dev::OpenXrHeadset::alignRootFrameToHeadset()
 
     return true;
 }
+
+bool yarp::dev::OpenXrHeadset::setCustomPoseRelativePosition(const std::string &customFrameName, const double x, const double y, const double z)
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+
+    return m_posesManager.setCustomPoseRelativePosition(customFrameName, {static_cast<float>(x),
+                                                                           static_cast<float>(y),
+                                                                           static_cast<float>(z)});
+}
+
+bool yarp::dev::OpenXrHeadset::setCustomPoseRelativeOrientation(const std::string &customFrameName, const double angle1, const double angle2, const double angle3)
+{
+    std::lock_guard<std::mutex> lock(m_mutex);
+
+    return m_posesManager.setCustomPoseRelativeOrientation(customFrameName, {static_cast<float>(angle1),
+                                                                              static_cast<float>(angle2),
+                                                                              static_cast<float>(angle3)});
+}
diff --git a/src/devices/openxrheadset/OpenXrHeadset.h b/src/devices/openxrheadset/OpenXrHeadset.h
index 56a8e2c..0cdc0b5 100644
--- a/src/devices/openxrheadset/OpenXrHeadset.h
+++ b/src/devices/openxrheadset/OpenXrHeadset.h
@@ -200,6 +200,27 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
      */
     virtual bool alignRootFrameToHeadset() override;
 
+    /**
+     * Set the position offset of a custom frame
+     * @param customFrameName The name of the frame to edit
+     * @param x The x coordinate of the relative position (with respect the parent frame)
+     * @param y The y coordinate of the relative position (with respect the parent frame)
+     * @param z The z coordinate of the relative position (with respect the parent frame)
+     * @return True if successfull, false if the frame is not found
+     */
+    virtual bool setCustomPoseRelativePosition(const std::string& customFrameName, const double x, const double y, const double z) override;
+
+    /**
+     * Set the rotation offset of a custom frame
+     * The order depends on the chosen euler angles in the configuration file.
+     * @param customFrameName The name of the frame to edit
+     * @param angle1 The first angle offset (with respect the parent frame).
+     * @param angle2 The second angle offset (with respect the parent frame).
+     * @param angle3 The third angle offset (with respect the parent frame).
+     * @return True if successfull, false if the frame is not found
+     */
+    virtual bool setCustomPoseRelativeOrientation(const std::string& customFrameName, const double angle1, const double angle2, const double angle3) override;
+
 private:
 
     struct GuiParam
diff --git a/src/devices/openxrheadset/PosesManager.cpp b/src/devices/openxrheadset/PosesManager.cpp
index abfad35..05fb331 100644
--- a/src/devices/openxrheadset/PosesManager.cpp
+++ b/src/devices/openxrheadset/PosesManager.cpp
@@ -153,3 +153,45 @@ void PosesManager::setTransformFromRawToRootFrame(const OpenXrInterface::Pose &r
     m_rootFrameRawRelativePoseInverse.rotation = relativePose.rotation.inverse();
     m_rootFrameRawRelativePoseInverse.position = -(m_rootFrameRawRelativePoseInverse.rotation * relativePose.position);
 }
+
+bool PosesManager::setCustomPoseRelativePosition(const std::string &customFrameName, const Eigen::Vector3f &relativePosition)
+{
+    auto customFrameIt = m_customPosesMap.find(customFrameName);
+
+    if (customFrameIt == m_customPosesMap.end())
+    {
+        yCError(OPENXRHEADSET) << "The frame" << customFrameName << "does not exist.";
+        return false;
+    }
+
+    m_customPoses[customFrameIt->second].setRelativePosition(relativePosition);
+    return true;
+}
+
+bool PosesManager::setCustomPoseRelativeOrientation(const std::string &customFrameName, const Eigen::Quaternionf &relativeOrientation)
+{
+    auto customFrameIt = m_customPosesMap.find(customFrameName);
+
+    if (customFrameIt == m_customPosesMap.end())
+    {
+        yCError(OPENXRHEADSET) << "The frame" << customFrameName << "does not exist.";
+        return false;
+    }
+
+    m_customPoses[customFrameIt->second].setRelativeOrientation(relativeOrientation);
+    return true;
+}
+
+bool PosesManager::setCustomPoseRelativeOrientation(const std::string &customFrameName, const Eigen::Vector3f &relativeOrientationEulerAngles)
+{
+    auto customFrameIt = m_customPosesMap.find(customFrameName);
+
+    if (customFrameIt == m_customPosesMap.end())
+    {
+        yCError(OPENXRHEADSET) << "The frame" << customFrameName << "does not exist.";
+        return false;
+    }
+
+    m_customPoses[customFrameIt->second].setRelativeOrientation(relativeOrientationEulerAngles);
+    return true;
+}
diff --git a/src/devices/openxrheadset/PosesManager.h b/src/devices/openxrheadset/PosesManager.h
index f70724e..96dccd2 100644
--- a/src/devices/openxrheadset/PosesManager.h
+++ b/src/devices/openxrheadset/PosesManager.h
@@ -55,6 +55,12 @@ class PosesManager
 
     void setTransformFromRawToRootFrame(const OpenXrInterface::Pose& relativePose);
 
+    bool setCustomPoseRelativePosition(const std::string& customFrameName, const Eigen::Vector3f& relativePosition);
+
+    bool setCustomPoseRelativeOrientation(const std::string& customFrameName, const Eigen::Quaternionf& relativeOrientation);
+
+    bool setCustomPoseRelativeOrientation(const std::string& customFrameName, const Eigen::Vector3f& relativeOrientationEulerAngles);
+
 };
 
 
diff --git a/src/devices/openxrheadset/thrifts/OpenXrHeadsetCommands.thrift b/src/devices/openxrheadset/thrifts/OpenXrHeadsetCommands.thrift
index 8221f6d..521634a 100644
--- a/src/devices/openxrheadset/thrifts/OpenXrHeadsetCommands.thrift
+++ b/src/devices/openxrheadset/thrifts/OpenXrHeadsetCommands.thrift
@@ -124,4 +124,25 @@ service OpenXrHeadsetCommands
      * @return True if successfull. False otherwise, e.g. if the current headset pose is not valid
      */
      bool alignRootFrameToHeadset();
+
+    /**
+     * Set the position offset of a custom pose
+     * @param customFrameName The name of the pose to edit
+     * @param x The x coordinate of the relative position (with respect the parent frame)
+     * @param y The y coordinate of the relative position (with respect the parent frame)
+     * @param z The z coordinate of the relative position (with respect the parent frame)
+     * @return True if successfull, false if the pose is not found
+     */
+     bool setCustomPoseRelativePosition(1:string customFrameName, 2:double x, 3:double y, 4:double z);
+
+    /**
+     * Set the rotation offset of a custom pose
+     * The order depends on the chosen euler angles in the configuration file.
+     * @param customFrameName The name of the pose to edit
+     * @param angle1 The first angle offset (with respect the parent frame).
+     * @param angle2 The second angle offset (with respect the parent frame).
+     * @param angle3 The third angle offset (with respect the parent frame).
+     * @return True if successfull, false if the pose is not found
+     */
+     bool setCustomPoseRelativeOrientation(1:string customFrameName, 2:double angle1, 3:double angle2, 4:double angle3);
 }