Skip to content
Snippets Groups Projects
Commit 56ad9f5f authored by Nicolas Pope's avatar Nicolas Pope
Browse files

WIP OpenVR code

parent edc384b1
No related branches found
No related tags found
1 merge request!316Resolves #343 GUI and Frame Refactor
......@@ -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();
......@@ -110,6 +172,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);
rgbdframe.create<cv::cuda::GpuMat>(Channel::Depth).create(height, width, CV_32F);
......@@ -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
}
......@@ -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();
};
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment