WIP OpenVR code

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();
@@ -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
 }

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();
 };
 
 }