diff --git a/components/streams/src/renderers/openvr_render.cpp b/components/streams/src/renderers/openvr_render.cpp
index 1827e3f248574f54ea5c822c3a69a83e46df3da3..add3c24c2428a653912bb792990037b7d03a4aa2 100644
--- a/components/streams/src/renderers/openvr_render.cpp
+++ b/components/streams/src/renderers/openvr_render.cpp
@@ -17,15 +17,17 @@ using ftl::render::OpenVRRender;
using ftl::codecs::Channel;
using ftl::rgbd::Capability;
+#ifdef HAVE_OPENVR
+static vr::IVRSystem *HMD = nullptr;
+#endif
+
OpenVRRender::OpenVRRender(ftl::render::Source *host, ftl::stream::Feed *feed)
: ftl::render::BaseSourceImpl(host), feed_(feed), my_id_(0) {
- /*host->restore("device:render", {
- "renderer",
- "source",
- "intrinsics",
- "name"
- });*/
+ #ifdef HAVE_OPENVR
+ if (HMD) throw FTL_Error("Can only have one OpenVR device");
+ #endif
+ initVR();
renderer_ = std::unique_ptr<ftl::render::CUDARender>(
ftl::create<ftl::render::CUDARender>(host_, "renderer")
@@ -58,6 +60,36 @@ OpenVRRender::OpenVRRender(ftl::render::Source *host, ftl::stream::Feed *feed)
OpenVRRender::~OpenVRRender() {
if (filter_) filter_->remove();
delete intrinsics_;
+
+ #ifdef HAVE_OPENVR
+ if (HMD != nullptr) {
+ vr::VR_Shutdown();
+ }
+ #endif
+}
+
+bool OpenVRRender::initVR() {
+ #ifdef HAVE_OPENVR
+ if (!vr::VR_IsHmdPresent()) {
+ return false;
+ }
+
+ if (HMD) return true;
+
+ vr::EVRInitError eError = vr::VRInitError_None;
+ HMD = vr::VR_Init( &eError, vr::VRApplication_Scene );
+
+ if (eError != vr::VRInitError_None)
+ {
+ HMD = nullptr;
+ LOG(ERROR) << "Unable to init VR runtime: " << vr::VR_GetVRInitErrorAsEnglishDescription(eError);
+ return false;
+ }
+
+ return true;
+ #else
+ return false;
+ #endif
}
bool OpenVRRender::supported() {
@@ -69,14 +101,44 @@ bool OpenVRRender::supported() {
}
bool OpenVRRender::isReady() {
- return true;
+ #ifdef HAVE_OPENVR
+ return HMD != nullptr;
+ #else
+ return false;
+ #endif
}
bool OpenVRRender::capture(int64_t ts) {
return true;
}
+#ifdef HAVE_OPENVR
+static inline Eigen::Matrix4d ConvertSteamVRMatrixToMatrix4( const vr::HmdMatrix34_t &matPose )
+{
+ Eigen::Matrix4d matrixObj;
+ matrixObj <<
+ matPose.m[0][0], matPose.m[0][1], matPose.m[0][2], matPose.m[0][3],
+ matPose.m[1][0], matPose.m[1][1], matPose.m[1][2], matPose.m[1][3],
+ matPose.m[2][0], matPose.m[2][1], matPose.m[2][2], matPose.m[2][3],
+ 0.0, 0.0, 0.0, 1.0;
+ return matrixObj;
+}
+
+static inline Eigen::Matrix4d ConvertSteamVRMatrixToMatrix4( const vr::HmdMatrix44_t &matPose )
+{
+ Eigen::Matrix4d matrixObj;
+ matrixObj <<
+ matPose.m[0][0], matPose.m[0][1], matPose.m[0][2], matPose.m[0][3],
+ matPose.m[1][0], matPose.m[1][1], matPose.m[1][2], matPose.m[1][3],
+ matPose.m[2][0], matPose.m[2][1], matPose.m[2][2], matPose.m[2][3],
+ matPose.m[3][0], matPose.m[3][1], matPose.m[3][2], matPose.m[3][3];
+ return matrixObj;
+}
+#endif
+
bool OpenVRRender::retrieve(ftl::data::Frame &frame_out) {
+
+ #ifdef HAVE_OPENVR
//auto input = std::atomic_load(&input_);
my_id_ = frame_out.frameset();
@@ -109,6 +171,60 @@ bool OpenVRRender::retrieve(ftl::data::Frame &frame_out) {
int width = rgbdframe.getLeft().width;
int height = rgbdframe.getLeft().height;
+
+ vr::VRCompositor()->SetTrackingSpace(vr::TrackingUniverseStanding);
+ vr::VRCompositor()->WaitGetPoses(rTrackedDevicePose_, vr::k_unMaxTrackedDeviceCount, NULL, 0 );
+
+ if (rTrackedDevicePose_[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid ) {
+ Eigen::Matrix4d eye_l = ConvertSteamVRMatrixToMatrix4(
+ vr::VRSystem()->GetEyeToHeadTransform(vr::Eye_Left));
+
+ //Eigen::Matrix4d eye_r = ConvertSteamVRMatrixToMatrix4(
+ // vr::VRSystem()->GetEyeToHeadTransform(vr::Eye_Left));
+
+ float baseline_in = 2.0 * eye_l(0, 3);
+
+ if (baseline_in != baseline_) {
+ baseline_ = baseline_in;
+ //src_->set("baseline", baseline_);
+ state_.getLeft().baseline = baseline_;
+ state_.getRight().baseline = baseline_;
+ }
+ Eigen::Matrix4d pose = ConvertSteamVRMatrixToMatrix4(rTrackedDevicePose_[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking);
+ Eigen::Vector3d ea = pose.block<3, 3>(0, 0).eulerAngles(0, 1, 2);
+
+ Eigen::Vector3d vreye;
+ vreye[0] = pose(0, 3);
+ vreye[1] = -pose(1, 3);
+ vreye[2] = -pose(2, 3);
+
+ // NOTE: If modified, should be verified with VR headset!
+ Eigen::Matrix3d R;
+ R = Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitX()) *
+ Eigen::AngleAxisd(-ea[1], Eigen::Vector3d::UnitY()) *
+ Eigen::AngleAxisd(-ea[2], Eigen::Vector3d::UnitZ());
+
+ //double rd = 180.0 / 3.141592;
+ //LOG(INFO) << "rotation x: " << ea[0] *rd << ", y: " << ea[1] * rd << ", z: " << ea[2] * rd;
+ // pose.block<3, 3>(0, 0) = R;
+
+ rotmat_.block(0, 0, 3, 3) = R;
+
+ // TODO: Apply a rotation to orient also
+
+ eye_[0] += (neye_[0] - eye_[0]) * lerpSpeed_ * delta_;
+ eye_[1] += (neye_[1] - eye_[1]) * lerpSpeed_ * delta_;
+ eye_[2] += (neye_[2] - eye_[2]) * lerpSpeed_ * delta_;
+
+ Eigen::Translation3d trans(eye_ + vreye);
+ Eigen::Affine3d t(trans);
+ auto viewPose = t.matrix() * rotmat_;
+
+ rgbdframe.setPose() = viewPose;
+
+ } else {
+ //LOG(ERROR) << "No VR Pose";
+ }
// FIXME: Create opengl buffers here and map to cuda?
rgbdframe.create<cv::cuda::GpuMat>(Channel::Colour).create(height, width, CV_8UC4);
@@ -137,9 +253,27 @@ bool OpenVRRender::retrieve(ftl::data::Frame &frame_out) {
} catch (const ftl::exception &e) {
LOG(ERROR) << "Render exception: " << e.what();
}
- return true;
+ //return true;
} else {
//LOG(INFO) << "Render fail";
- return true;
+ //return true;
}
+
+ //if (HMD) {
+ //glBindTexture(GL_TEXTURE_2D, leftEye_);
+ vr::Texture_t leftEyeTexture = {(void*)(uintptr_t)leftEye_, vr::TextureType_OpenGL, vr::ColorSpace_Gamma };
+ vr::VRCompositor()->Submit(vr::Eye_Left, &leftEyeTexture );
+
+ //glBindTexture(GL_TEXTURE_2D, rightEye_);
+ vr::Texture_t rightEyeTexture = {(void*)(uintptr_t)rightEye_, vr::TextureType_OpenGL, vr::ColorSpace_Gamma };
+ vr::VRCompositor()->Submit(vr::Eye_Right, &rightEyeTexture );
+
+ glFlush();
+ //}
+
+ return true;
+
+ #else
+ return false;
+ #endif
}
diff --git a/components/streams/src/renderers/openvr_render.hpp b/components/streams/src/renderers/openvr_render.hpp
index 9462141314485cfed81c28af31ffae7113acd3d3..3b4b4891b84ceabf944605f0c91ff0efbbe95244 100644
--- a/components/streams/src/renderers/openvr_render.hpp
+++ b/components/streams/src/renderers/openvr_render.hpp
@@ -31,6 +31,8 @@ class OpenVRRender : public ftl::render::BaseSourceImpl {
std::unique_ptr<ftl::render::CUDARender> renderer_;
ftl::Configurable *intrinsics_;
uint32_t my_id_;
+
+ bool initVR();
};
}