File pose_relay.hpp

File List > offload_rendering_server > pose_relay.hpp
#pragma once

#ifndef ENABLE_MONADO
#    define ENABLE_MONADO
#endif

#define DOUBLE_INCLUDE
#ifdef USING_OPENXR
#    include "illixr/data_format/poses/combined_pose.hpp"
#else
#    include "illixr/data_format/poses/head_pose.hpp"
#endif

#include "illixr/data_format/pose_prediction.hpp"
#include "illixr/switchboard.hpp"
#include "illixr/threadloop.hpp"

#undef DOUBLE_INCLUDE

#include 

#ifdef USING_OPENXR
#    include "auxiliary/os/os_time.h"
#else
#    ifndef XrTime
typedef int64_t XrTime;
#    endif
#endif

namespace ILLIXR {

struct pose_map_entry {
    uint64_t  id{0}; 
    POSE_TYPE pose;
};

#ifdef USING_OPENXR
struct pose_point {
    XrTime             time;
    uint64_t           id; 
    xrt_space_relation pose;
};
#else
typedef ILLIXR::data_format::pose::fast_head_pose_type pose_point;
#endif

struct velocity_filter {
    static constexpr int MAX_WINDOW                  = 16;
    Eigen::Vector3f      linear_samples[MAX_WINDOW]  = {};
    Eigen::Vector3f      angular_samples[MAX_WINDOW] = {};
    int                  head                        = 0;
    int                  count                       = 0;
    bool                 initialized                 = false;
};

class MY_EXPORT_API pose_relay : public threadloop {
public:
    pose_relay(const std::string& name, phonebook* pb);

    POSE_TYPE get_pose() const;

    POSE_TYPE get_pose(POSE_TIME_TYPE future_time) const;

    bool fast_pose_reliable() const;

#ifdef USING_OPENXR
    uint64_t get_pose_id_for_time(XrTime at_time) const;

    uint64_t find_pose_id_by_orientation(const Eigen::Quaternionf& q) const;

    std::chrono::steady_clock::time_point get_pose_time(uint64_t id) const;
#endif

protected:
    threadloop::skip_option _p_should_skip() override;

    void _p_one_iteration() override;

private:
#ifdef USING_OPENXR
    void calibrate_monado_time_offset();
#endif
    std::shared_ptr<spdlog::logger> log_;
    std::shared_ptr<switchboard>    switchboard_;

#ifdef USING_OPENXR
    switchboard::reader<data_format::pose::combined_pose> combined_pose_;

    mutable switchboard::writer<data_format::pose::hand_joint_poses_pair>       hand_tracking_writer_;
    mutable switchboard::writer<data_format::pose::hand_interaction_poses_pair> hand_interaction_writer_;
    mutable switchboard::writer<data_format::pose::palm_poses_pair>             palm_pose_writer_;
#else
    switchboard::reader<POSE_TYPE> render_pose_;
#endif

    // Pipeline latency constants (will be replaced with measured values later)
    static constexpr double ENCODE_LATENCY_NS = 9.0 * 1'000'000.0;
    static constexpr double DECODE_LATENCY_NS = 5.0 * 1'000'000.0;
    static constexpr double UNITY_LATENCY_NS  = 5.0 * 1'000'000.0;

#ifdef USING_OPENXR
    std::vector<pose_point> current_poses_;
    uint64_t                last_pose_id_ = 0;
#else
    POSE_TYPE current_pose_;
#endif

    int64_t windows_epoch_offset_ns_  = 0;
    bool    monado_offset_calibrated_ = false;

    mutable std::map<XrTime, pose_map_entry> pose_map_;

    mutable std::mutex pose_mutex_;

    mutable std::atomic<double> smoothed_rtt_ns_{0.0};

    std::chrono::steady_clock::time_point                     last_offset_calibration_{};
    std::map<uint64_t, std::chrono::steady_clock::time_point> pose_time_{};
#ifdef USING_OPENXR
    bool use_hand_tracking_     = false;
    bool use_palm_poses_        = false;
    bool use_hand_interactions_ = false;
#endif
    bool            do_pose_prediction_{false};
    velocity_filter velocity_filter_;
    int             velocity_window_size_  = 8; // set from env in ctor
    float           velocity_deadband_lin_ = 0.05f;
    float           velocity_deadband_ang_ = 0.03f;
};

} // namespace ILLIXR