File head_pose.hpp

File List > data_format > poses > head_pose.hpp

Go to the documentation of this file

#pragma once

#include "illixr/data_format/poses/pose_base.hpp"
#include "illixr/switchboard.hpp"

#ifdef USING_OPENXR
#    include "openxr_defines.hpp"
#else // USING_OPENXR
#    if __has_include()
#        include 
#    else // __has_include()
#        include 
#    endif // __has_include()
#endif     // USING_OPENXR
#include 
#include 

namespace ILLIXR::data_format::pose {

#ifdef USING_OPENXR
typedef POSE_DATA_TYPE head_pose_data;
#    define TIME_POINT int64_t
#else
#    define TIME_POINT time_point

struct head_pose_data : public pose_base {
    head_pose_data() = default;

    head_pose_data(Eigen::Vector3f position_, Eigen::Quaternionf orientation_, bool valid_ = true, const float confidence_ = 0.)
        : pose_base{std::move(position_), std::move(orientation_), confidence_, valid_} { }
};
#endif

#ifdef USING_OPENXR
typedef xrt_space_relation head_pose_type;

#else
struct [[maybe_unused]] head_pose_type
    : public switchboard::event
    , public head_pose_data {
    time_point      sensor_time;            
    Eigen::Vector3f linear_velocity;        
    Eigen::Vector3f angular_velocity;       
    bool            linear_velocity_valid;  
    bool            angular_velocity_valid; 

    head_pose_type()
        : head_pose_data{}
        , sensor_time{time_point{}}
        , linear_velocity{Eigen::Vector3f::Zero()}
        , angular_velocity{Eigen::Vector3f::Zero()}
        , linear_velocity_valid{false}
        , angular_velocity_valid{false} { }

    head_pose_type(time_point sensor_time_, Eigen::Vector3f& position_, Eigen::Quaternionf& orientation_,
                   Eigen::Vector3f linear_velocity_  = Eigen::Vector3f::Zero(),
                   Eigen::Vector3f angular_velocity_ = Eigen::Vector3f::Zero(), bool linear_velocity_valid_ = false,
                   bool angular_velocity_valid_ = false, bool valid_ = true, const float confidence_ = 0.)
        : head_pose_data{position_, orientation_, valid_, confidence_}
        , sensor_time{sensor_time_}
        , linear_velocity{std::move(linear_velocity_)}
        , angular_velocity{std::move(angular_velocity_)}
        , linear_velocity_valid{linear_velocity_valid_}
        , angular_velocity_valid{angular_velocity_valid_} { }

    head_pose_type(time_point sensor_time_, const Eigen::Vector3f& position_, const Eigen::Quaternionf& orientation_,
                   Eigen::Vector3f linear_velocity_  = Eigen::Vector3f::Zero(),
                   Eigen::Vector3f angular_velocity_ = Eigen::Vector3f::Zero(), bool linear_velocity_valid_ = false,
                   bool angular_velocity_valid_ = false, bool valid_ = true, const float confidence_ = 0.)
        : head_pose_data{position_, orientation_, valid_, confidence_}
        , sensor_time{sensor_time_}
        , linear_velocity{std::move(linear_velocity_)}
        , angular_velocity{std::move(angular_velocity_)}
        , linear_velocity_valid{linear_velocity_valid_}
        , angular_velocity_valid{angular_velocity_valid_} { }

    head_pose_type(time_point sensor_time_, head_pose_data& other)
        : head_pose_data{other.position, other.orientation, other.valid, other.confidence}
        , sensor_time{sensor_time_}
        , linear_velocity{Eigen::Vector3f::Zero()}
        , angular_velocity{Eigen::Vector3f::Zero()}
        , linear_velocity_valid{false}
        , angular_velocity_valid{false} { }
};
#endif

[[maybe_unused]] typedef std::map<side, head_pose_type> head_pose_map;

struct fast_head_pose_type : public switchboard::event {
    head_pose_type pose;                  
    time_point     predict_computed_time; 
    TIME_POINT     predict_target_time;   

    fast_head_pose_type()
        : pose{}
        , predict_computed_time{time_point{}}
#ifdef USING_OPENXR
        , predict_target_time{0} {}
#else
        , predict_target_time{time_point{}} {
    }
#endif

        fast_head_pose_type(head_pose_type pose_, time_point predict_computed_time_, TIME_POINT predict_target_time_)
        : pose{std::move(pose_)}
        , predict_computed_time{predict_computed_time_}
        , predict_target_time{predict_target_time_} {
    }

#ifdef USING_OPENXR
    [[nodiscard]] bool is_valid() const {
#    ifdef ENABLE_MONADO
        return (pose.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0u &&
            (pose.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0u;
#    else
        return (pose.relation_flags & XR_SPACE_LOCATION_POSITION_VALID_BIT) != 0u &&
            (pose.relation_flags & XR_SPACE_LOCATION_ORIENTATION_VALID_BIT) != 0u;
#    endif
    }
#else
    [[nodiscard]] bool is_valid() const {
        return pose.valid;
    }
#endif
};

struct [[maybe_unused]] texture_pose : public switchboard::event {
    duration           offload_duration{};
    unsigned char*     image{};
    time_point         pose_time{};
    Eigen::Vector3f    position;
    Eigen::Quaternionf latest_quaternion;
    Eigen::Quaternionf render_quaternion;

    texture_pose() = default;

    texture_pose(duration offload_duration_, unsigned char* image_, time_point pose_time_, Eigen::Vector3f position_,
                 Eigen::Quaternionf latest_quaternion_, Eigen::Quaternionf render_quaternion_)
        : offload_duration{offload_duration_}
        , image{image_}
        , pose_time{pose_time_}
        , position{std::move(position_)}
        , latest_quaternion{std::move(latest_quaternion_)}
        , render_quaternion{std::move(render_quaternion_)} { }
};

} // namespace ILLIXR::data_format::pose