Merge pull request #44 from ami-iit/test_projecion_quad

Fixed computation of projection matrix in case of asymmetric FOV

.gitignore

@@ -8,3 +8,7 @@ build/*
 # Qtcreator
 *.user*
 *.autosave
+
+# Visual Studio
+.vs/
+CmakeSettings.json

CMakeLists.txt

@@ -8,7 +8,7 @@ cmake_minimum_required(VERSION 3.12)
 
 project(yarp-device-openxrheadset
         LANGUAGES C CXX
-        VERSION 0.0.3)
+        VERSION 0.0.4)
 
 # Defines the CMAKE_INSTALL_LIBDIR, CMAKE_INSTALL_BINDIR and many other useful macros.
 # See https://cmake.org/cmake/help/latest/module/GNUInstallDirs.html

src/devices/openxrheadset/OpenXrEigenConversions.h

@@ -14,7 +14,7 @@
 #include <Eigen/Geometry>
 
 
-inline Eigen::Vector3f toEigen(const XrVector3f vector)
+inline Eigen::Vector3f toEigen(const XrVector3f &vector)
 {
     Eigen::Vector3f output;
     output[0] = vector.x;
@@ -23,7 +23,7 @@ inline Eigen::Vector3f toEigen(const XrVector3f vector)
     return output;
 }
 
-inline Eigen::Quaternionf toEigen(const XrQuaternionf quaternion)
+inline Eigen::Quaternionf toEigen(const XrQuaternionf &quaternion)
 {
     Eigen::Quaternionf output;
     output.w() = quaternion.w;

src/devices/openxrheadset/OpenXrHeadset.cpp

@@ -251,9 +251,11 @@ bool yarp::dev::OpenXrHeadset::open(yarp::os::Searchable &cfg)
     double period = cfg.check("vr_period", yarp::os::Value(0.011)).asFloat64();
     this->setPeriod(period);
 
+    m_useNativeQuadLayers = cfg.check("use_native_quad_layers") && (cfg.find("use_native_quad_layers").isNull() || cfg.find("use_native_quad_layers").asBool());
 
     m_openXrInterfaceSettings.posesPredictionInMs = cfg.check("vr_poses_prediction_in_ms", yarp::os::Value(0.0)).asFloat64();
-    m_openXrInterfaceSettings.hideWindow = cfg.check("hide_window") && (cfg.find("hide_window").isNull() || cfg.find("hide_window").asBool());
+    m_openXrInterfaceSettings.hideWindow = (m_useNativeQuadLayers && !cfg.check("hide_window")) || (cfg.check("hide_window") && (cfg.find("hide_window").isNull() || cfg.find("hide_window").asBool()));
+    m_openXrInterfaceSettings.renderInPlaySpace = cfg.check("render_in_play_space") && (cfg.find("render_in_play_space").isNull() || cfg.find("render_in_play_space").asBool());
 
     m_getStickAsAxis = cfg.check("stick_as_axis", yarp::os::Value(false)).asBool();
     m_rootFrame = cfg.check("tf_root_frame", yarp::os::Value("openxr_origin")).asString();
@@ -397,8 +399,20 @@ bool yarp::dev::OpenXrHeadset::threadInit()
             return false;
         }
 
-        m_eyesManager.options().leftEyeQuadLayer = m_openXrInterface.addHeadFixedOpenGLQuadLayer();
-        m_eyesManager.options().rightEyeQuadLayer = m_openXrInterface.addHeadFixedOpenGLQuadLayer();
+        auto getLayer = [this]() -> std::shared_ptr<IOpenXrQuadLayer>
+        {
+            if (m_useNativeQuadLayers)
+            {
+                return m_openXrInterface.addHeadFixedQuadLayer();
+            }
+            else
+            {
+                return m_openXrInterface.addHeadFixedOpenGLQuadLayer();
+            }
+        };
+
+        m_eyesManager.options().leftEyeQuadLayer = getLayer();
+        m_eyesManager.options().rightEyeQuadLayer = getLayer();
 
         if (!m_eyesManager.initialize())
         {
@@ -407,7 +421,7 @@ bool yarp::dev::OpenXrHeadset::threadInit()
 
         for (GuiParam& gui : m_huds)
         {
-            if (!gui.layer.initialize(m_openXrInterface.addHeadFixedOpenGLQuadLayer(), gui.portName)) {
+            if (!gui.layer.initialize(getLayer(), gui.portName)) {
                 yCError(OPENXRHEADSET) << "Cannot initialize" << gui.portName << "display texture.";
                 return false;
             }
@@ -418,7 +432,7 @@ bool yarp::dev::OpenXrHeadset::threadInit()
 
         for (LabelLayer& label : m_labels)
         {
-            label.options.quadLayer = m_openXrInterface.addHeadFixedOpenGLQuadLayer();
+            label.options.quadLayer = getLayer();
 
             if (!label.layer.initialize(label.options)) {
                 yCError(OPENXRHEADSET) << "Cannot initialize" << label.options.portName << "label.";
@@ -431,7 +445,7 @@ bool yarp::dev::OpenXrHeadset::threadInit()
 
         for (SlideLayer& slide : m_slides)
         {
-            slide.options.quadLayer = m_openXrInterface.addHeadFixedOpenGLQuadLayer();
+            slide.options.quadLayer = getLayer();
             if (!slide.layer.initialize(slide.options)) {
                 yCError(OPENXRHEADSET) << "Cannot initialize" << slide.options.portName << "slide.";
                 return false;

src/devices/openxrheadset/OpenXrHeadset.h

@@ -317,6 +317,7 @@ class yarp::dev::OpenXrHeadset : public yarp::dev::DeviceDriver,
     std::atomic_bool m_closed{ false };
 
     OpenXrInterfaceSettings m_openXrInterfaceSettings;
+    bool m_useNativeQuadLayers{ false };
     OpenXrInterface m_openXrInterface;
 
     std::vector<bool> m_buttons;

src/devices/openxrheadset/OpenXrInterface.cpp

@@ -9,6 +9,7 @@
 #include <impl/OpenXrInterfaceImpl.h>
 
 //#define DEBUG_RENDERING
+//#define DEBUG_RENDERING_LOCATION
 
 
 bool OpenXrInterface::checkExtensions()
@@ -504,8 +505,8 @@ bool OpenXrInterface::prepareXrCompositionLayers()
         m_pimpl->depth_projection_views[i].next = NULL;
         m_pimpl->depth_projection_views[i].minDepth = 0.f;
         m_pimpl->depth_projection_views[i].maxDepth = 1.f;
-        m_pimpl->depth_projection_views[i].nearZ = 0.01f;
-        m_pimpl->depth_projection_views[i].farZ = 100.0f;
+        m_pimpl->depth_projection_views[i].nearZ = m_pimpl->nearZ;
+        m_pimpl->depth_projection_views[i].farZ = m_pimpl->farZ;
 
         m_pimpl->depth_projection_views[i].subImage.swapchain = m_pimpl->projection_view_depth_swapchains[i].swapchain;
         m_pimpl->depth_projection_views[i].subImage.imageArrayIndex = 0;
@@ -524,7 +525,7 @@ bool OpenXrInterface::prepareXrCompositionLayers()
         .type = XR_TYPE_COMPOSITION_LAYER_PROJECTION,
         .next = NULL,
         .layerFlags = 0,
-        .space = m_pimpl->view_space,
+        .space = m_pimpl->renderInPlaySpace? m_pimpl->play_space : m_pimpl->view_space,
         .viewCount = static_cast<uint32_t>(m_pimpl->projection_views.size()),
         .views = m_pimpl->projection_views.data(),
     };
@@ -882,23 +883,37 @@ void OpenXrInterface::updateXrSpaces()
                                          .viewConfigurationType =
                                              XR_VIEW_CONFIGURATION_TYPE_PRIMARY_STEREO,
                                          .displayTime = m_pimpl->frame_state.predictedDisplayTime,
-                                         .space = m_pimpl->view_space};
+                                         .space = m_pimpl->renderInPlaySpace ? m_pimpl->play_space : m_pimpl->view_space };
 
 
-    uint32_t output_viewCount = m_pimpl->views.size();
+    uint32_t output_viewCount = static_cast<uint32_t>(m_pimpl->views.size());
     XrResult result = xrLocateViews(m_pimpl->session, &view_locate_info, &(m_pimpl->view_state),
-                                    m_pimpl->views.size(), &output_viewCount, m_pimpl->views.data());
+                                    static_cast<uint32_t>(m_pimpl->views.size()), &output_viewCount, m_pimpl->views.data());
     if (!m_pimpl->checkXrOutput(result, "Failed to locate the views!"))
     {
         m_pimpl->view_state.viewStateFlags = 0; //Set to zero all the valid/tracked flags
     }
 
-    XrPosef identity_pose = { .orientation = {.x = 0, .y = 0, .z = 0, .w = 1.0},
-                            .position = {.x = 0, .y = 0, .z = 0} };
+    m_pimpl->mid_views_pose = m_pimpl->views[0].pose;
+
+    for (size_t i = 1; i < m_pimpl->views.size(); ++i)
+    {
+        m_pimpl->mid_views_pose.position = toXr(Eigen::Vector3f(toEigen(m_pimpl->mid_views_pose.position) + toEigen(m_pimpl->views[i].pose.position)));
+    }
+    m_pimpl->mid_views_pose.position = toXr(Eigen::Vector3f(toEigen(m_pimpl->mid_views_pose.position)/static_cast<float>(m_pimpl->views.size())));
+
+    m_pimpl->mid_views_pose_inverted.orientation = toXr(toEigen(m_pimpl->mid_views_pose.orientation).inverse());
+    m_pimpl->mid_views_pose_inverted.position = toXr(toEigen(m_pimpl->mid_views_pose_inverted.orientation) * -toEigen(m_pimpl->mid_views_pose.position));
 
     for (size_t i = 0; i < m_pimpl->views.size(); ++i)
     {
+#ifdef DEBUG_RENDERING_LOCATION
+        XrPosef identity_pose = {.orientation = {.x = 0, .y = 0, .z = 0, .w = 1.0},
+                                       .position = {.x = 0, .y = 0, .z = 0}};
         m_pimpl->projection_views[i].pose = identity_pose;
+#else
+        m_pimpl->projection_views[i].pose = m_pimpl->mid_views_pose;
+#endif
         m_pimpl->projection_views[i].fov = m_pimpl->views[i].fov;
     }
 
@@ -1170,12 +1185,14 @@ void OpenXrInterface::render()
     {
         if (openGLLayer->shouldRender() && (openGLLayer->visibility() == IOpenXrQuadLayer::Visibility::LEFT_EYE || openGLLayer->visibility() == IOpenXrQuadLayer::Visibility::BOTH_EYES))
         {
-            openGLLayer->setFOVs(std::abs(m_pimpl->views[0].fov.angleLeft) + std::abs(m_pimpl->views[0].fov.angleRight), std::abs(m_pimpl->views[0].fov.angleUp) + std::abs(m_pimpl->views[0].fov.angleDown));
+            openGLLayer->setFOVs(m_pimpl->views[0].fov);
+            openGLLayer->setDepthLimits(m_pimpl->nearZ, m_pimpl->farZ);
             if (openGLLayer->visibility() == IOpenXrQuadLayer::Visibility::BOTH_EYES || !openGLLayer->offsetIsSet())
             {
                 if (viewIsValid)
                 {
-                    openGLLayer->setOffsetPosition(toEigen(m_pimpl->views[0].pose.position));
+                    Eigen::Vector3f offset = toEigen(m_pimpl->mid_views_pose_inverted.orientation) * toEigen(m_pimpl->views[0].pose.position) + toEigen(m_pimpl->mid_views_pose_inverted.position);
+                    openGLLayer->setOffsetPosition(offset);
                 }
                 else
                 {
@@ -1219,12 +1236,14 @@ void OpenXrInterface::render()
     {
         if (openGLLayer->shouldRender() && (openGLLayer->visibility() == IOpenXrQuadLayer::Visibility::RIGHT_EYE || openGLLayer->visibility() == IOpenXrQuadLayer::Visibility::BOTH_EYES))
         {
-            openGLLayer->setFOVs(std::abs(m_pimpl->views[1].fov.angleLeft) + std::abs(m_pimpl->views[1].fov.angleRight), std::abs(m_pimpl->views[1].fov.angleUp) + std::abs(m_pimpl->views[1].fov.angleDown));
+            openGLLayer->setFOVs(m_pimpl->views[1].fov);
+            openGLLayer->setDepthLimits(m_pimpl->nearZ, m_pimpl->farZ);
             if (openGLLayer->visibility() == IOpenXrQuadLayer::Visibility::BOTH_EYES || !openGLLayer->offsetIsSet())
             {
                 if (viewIsValid)
                 {
-                    openGLLayer->setOffsetPosition(toEigen(m_pimpl->views[1].pose.position));
+                    Eigen::Vector3f offset = toEigen(m_pimpl->mid_views_pose_inverted.orientation) * toEigen(m_pimpl->views[1].pose.position) + toEigen(m_pimpl->mid_views_pose_inverted.position);
+                    openGLLayer->setOffsetPosition(offset);
                 }
                 else
                 {
@@ -1293,6 +1312,12 @@ void OpenXrInterface::endXrFrame()
         {
             if (layer->shouldSubmit())
             {
+#ifdef DEBUG_RENDERING_LOCATION
+                layer->layer.pose = layer->desiredHeadFixedPose;
+#else
+                layer->layer.pose = toXr(Eigen::Matrix4f(toEigen(m_pimpl->mid_views_pose) * toEigen(layer->desiredHeadFixedPose)));
+#endif
+
                 m_pimpl->submitLayer((XrCompositionLayerBaseHeader*) &layer->layer);
             }
         }
@@ -1339,6 +1364,12 @@ bool OpenXrInterface::initialize(const OpenXrInterfaceSettings &settings)
     m_pimpl->locate_space_prediction_in_ns = static_cast<long>(std::round(settings.posesPredictionInMs * 1e6));
 
     m_pimpl->hideWindow = settings.hideWindow;
+    m_pimpl->renderInPlaySpace = settings.renderInPlaySpace;
+
+#ifdef DEBUG_RENDERING_LOCATION
+    m_pimpl->renderInPlaySpace = true;
+#endif // DEBUG_RENDERING_LOCATION
+
 
     m_pimpl->closing = false;
     m_pimpl->closed = false;
@@ -1442,7 +1473,7 @@ std::shared_ptr<IOpenXrQuadLayer> OpenXrInterface::addHeadFixedQuadLayer()
         .type = XR_TYPE_COMPOSITION_LAYER_QUAD,
         .next = NULL,
         .layerFlags = 0,
-        .space = m_pimpl->view_space,        //Head fixed
+        .space = m_pimpl->renderInPlaySpace ? m_pimpl->play_space : m_pimpl->view_space,
         .eyeVisibility = XR_EYE_VISIBILITY_BOTH,
         .subImage = {
             .swapchain = XR_NULL_HANDLE,

src/devices/openxrheadset/OpenXrInterface.h

@@ -65,6 +65,7 @@ struct OpenXrInterfaceSettings
 {
     double posesPredictionInMs{0.0};
     bool hideWindow{false};
+    bool renderInPlaySpace{false};
 };
 
 class OpenXrInterface

src/devices/openxrheadset/impl/OpenGLQuadLayer.cpp

@@ -18,6 +18,12 @@ bool OpenGLQuadLayer::initialize(int32_t imageMaxWidth, int32_t imageMaxHeight)
     m_imageMaxWidth = imageMaxWidth;
     m_imageMaxHeight = imageMaxHeight;
 
+    //Random initialization for FOV
+    m_fov.angleLeft = -1.0f;
+    m_fov.angleRight = 1.0f;
+    m_fov.angleUp = 1.0f;
+    m_fov.angleDown = -1.0f;
+
     m_positions = {
         // vertex coords         // texture coords
         -0.5f, -0.5f, 0.0, 1.0f, 0.0f, 0.0f, // 0 (the first 4 numbers of the row are the vertex coordinates, the second 2 numbers are the texture coordinate for that vertex (the bottom left corner of the rectangle is also the bottom left corner of the picture))
@@ -72,15 +78,22 @@ bool OpenGLQuadLayer::initialize(int32_t imageMaxWidth, int32_t imageMaxHeight)
     return true;
 }
 
-void OpenGLQuadLayer::setFOVs(float fovX, float fovY)
+void OpenGLQuadLayer::setFOVs(const XrFovf& fov)
 {
-    float tan_fovY_2 = std::tan(fovY/2);
+    // Calculate horizontal and vertical FOVs
 
-    if (std::abs(tan_fovY_2) < 1e-15)
+    if (std::abs(fov.angleRight - fov.angleLeft) < 1e-15)
+    {
+        yCError(OPENXRHEADSET) << "Horizontal FOV is zero.";
         return;
+    }
 
-    m_fovY = fovY;
-    m_aspectRatio = std::tan(fovX/2) / tan_fovY_2; //See https://en.wikipedia.org/wiki/Field_of_view_in_video_games
+    if (std::abs(fov.angleUp - fov.angleDown) < 1e-15)
+    {
+        yCError(OPENXRHEADSET) << "Vertical FOV is zero.";
+        return;
+    }
+    m_fov = fov;
 }
 
 void OpenGLQuadLayer::setDepthLimits(float zNear, float zFar)
@@ -105,9 +118,26 @@ void OpenGLQuadLayer::render()
     glm::mat4 sca = glm::scale(glm::mat4(1.0f), m_modelScale);
 
     glm::mat4 model = m_offsetTra * modelPose * sca;
-    glm::mat4 proj = glm::perspective(m_fovY, m_aspectRatio, m_zNear, m_zFar);                           // 3D alternative to "ortho" proj type. It allows to define the view frustum by inserting the y FOV, the aspect ratio of the window, where are placed the near and far clipping planes
 
-    glm::mat4 layerTransform = proj * model;
+    // Calculate perspective matrix
+    float left = m_zNear * std::tan(m_fov.angleLeft);
+    float right = m_zNear * std::tan(m_fov.angleRight);
+    float bottom = m_zNear * std::tan(m_fov.angleDown);
+    float top = m_zNear * std::tan(m_fov.angleUp);
+
+    glm::mat4 perspective_matrix = glm::mat4(0.0f);
+    //The sintax for glm::mat4 is [col][row]
+    //Source "Generalized Perspective Projection" By Robert Kooima.
+    perspective_matrix[0][0] = (2.0f * m_zNear) / (right - left);
+    perspective_matrix[1][1] = (2.0f * m_zNear) / (top - bottom);
+    perspective_matrix[2][0] = (right + left) / (right - left);
+    perspective_matrix[2][1] = (top + bottom) / (top - bottom);
+    perspective_matrix[2][2] = -(m_zFar + m_zNear) / (m_zFar - m_zNear);
+    perspective_matrix[2][3] = -1.0f;
+    perspective_matrix[3][2] = -(2.0f * m_zFar * m_zNear) / (m_zFar - m_zNear);
+
+
+    glm::mat4 layerTransform = perspective_matrix * model;
 
     m_shader.bind();                                                                                                  // bind shader
     m_shader.setUniformMat4f("u_H", layerTransform);

src/devices/openxrheadset/impl/OpenGLQuadLayer.h

@@ -52,10 +52,9 @@ class OpenGLQuadLayer : public IOpenXrQuadLayer
 
     glm::vec3 m_modelScale{1.0f, 1.0f, 1.0f};
 
-    float m_fovY = glm::radians(60.0f);                                                      // Field Of View
     float m_zNear = 0.1f;
     float m_zFar = 100.0f;
-    float m_aspectRatio = 1.0f;
+    XrFovf m_fov;
 
 public:
 
@@ -73,7 +72,7 @@ class OpenGLQuadLayer : public IOpenXrQuadLayer
 
     bool initialize(int32_t imageMaxWidth, int32_t imageMaxHeight);
 
-    void setFOVs(float fovX, float fovY);
+    void setFOVs(const XrFovf& fov);
 
     void setDepthLimits(float zNear, float zFar);
 

src/devices/openxrheadset/impl/OpenXrInterfaceImpl.h

@@ -271,6 +271,12 @@ class OpenXrInterface::Implementation
     // array of views, filled by the runtime with current HMD display pose (basically the position of each eye)
     std::vector<XrView> views;
 
+    // position of a frame in the middle of the eyes, oriented as the first eye
+    XrPosef mid_views_pose;
+
+    // position of a frame in the middle of the eyes, oriented as the first eye
+    XrPosef mid_views_pose_inverted;
+
     // List of top level paths to retrieve the state of each action
     std::vector<TopLevelPath> top_level_paths;
 
@@ -360,6 +366,13 @@ class OpenXrInterface::Implementation
 
     // Flag to enable the window visualization
     bool hideWindow{false};
+
+    // Flag to enable the visualization of the layers in the play space instead of the head space
+    bool renderInPlaySpace{false};
+
+    // Depth limits
+    float nearZ = 0.01f;
+    float farZ = 100.0f;
 };