Merge branch 'feature/121/vr' into 'master'

OpenVR support

See merge request nicolas.pope/ftl!138

CMakeLists.txt

@@ -13,6 +13,7 @@ enable_testing()
 option(WITH_PCL "Use PCL if available" OFF)
 option(WITH_NVPIPE "Use NvPipe for compression if available" ON)
 option(WITH_OPTFLOW "Use NVIDIA Optical Flow if available" OFF)
+option(WITH_OPENVR "Build with OpenVR support" OFF)
 option(WITH_FIXSTARS "Use Fixstars libSGM if available" ON)
 option(BUILD_VISION "Enable the vision component" ON)
 option(BUILD_RECONSTRUCT "Enable the reconstruction component" ON)
@@ -42,19 +43,20 @@ if (LibArchive_FOUND)
 	set(HAVE_LIBARCHIVE true)
 endif()
 
-## OpenVR API path
-find_library(OPENVR_LIBRARIES
-  NAMES
-    openvr_api
-)
-set(OPENVR_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../headers)
-
-if (OPENVR_LIBRARIES)
-	message(STATUS "Found OpenVR: ${OPENVR_LIBRARIES}")
-	set(HAVE_OPENVR true)
+if (WITH_OPENVR)
+	## OpenVR API path
+	find_library(OPENVR_LIBRARIES
+		NAMES
+			openvr_api
+	)
+	set(OPENVR_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../headers)
+
+	if (OPENVR_LIBRARIES)
+		message(STATUS "Found OpenVR: ${OPENVR_LIBRARIES}")
+		set(HAVE_OPENVR true)
+	endif()
 endif()
 
-
 if (WITH_FIXSTARS)
 	find_package( LibSGM )
 	if (LibSGM_FOUND)

applications/gui/CMakeLists.txt

@@ -15,6 +15,10 @@ set(GUISRC
 	src/thumbview.cpp
 )
 
+if (HAVE_OPENVR)
+	list(APPEND GUISRC "src/vr.cpp")
+endif()
+
 add_executable(ftl-gui ${GUISRC})
 
 target_include_directories(ftl-gui PUBLIC

applications/gui/src/camera.cpp

 #include "camera.hpp"
 #include "pose_window.hpp"
 #include "screen.hpp"
-
 #include <nanogui/glutil.h>
 
+#ifdef HAVE_OPENVR
+#include "vr.hpp"
+#endif
+
 using ftl::rgbd::isValidDepth;
 using ftl::gui::GLTexture;
 using ftl::gui::PoseWindow;
@@ -116,13 +119,13 @@ void StatisticsImage::getValidRatio(cv::Mat &out) {
 }
 
 static Eigen::Affine3d create_rotation_matrix(float ax, float ay, float az) {
-  Eigen::Affine3d rx =
-      Eigen::Affine3d(Eigen::AngleAxisd(ax, Eigen::Vector3d(1, 0, 0)));
-  Eigen::Affine3d ry =
-      Eigen::Affine3d(Eigen::AngleAxisd(ay, Eigen::Vector3d(0, 1, 0)));
-  Eigen::Affine3d rz =
-      Eigen::Affine3d(Eigen::AngleAxisd(az, Eigen::Vector3d(0, 0, 1)));
-  return rz * rx * ry;
+	Eigen::Affine3d rx =
+		Eigen::Affine3d(Eigen::AngleAxisd(ax, Eigen::Vector3d(1, 0, 0)));
+	Eigen::Affine3d ry =
+		Eigen::Affine3d(Eigen::AngleAxisd(ay, Eigen::Vector3d(0, 1, 0)));
+	Eigen::Affine3d rz =
+		Eigen::Affine3d(Eigen::AngleAxisd(az, Eigen::Vector3d(0, 0, 1)));
+	return rz * rx * ry;
 }
 
 ftl::gui::Camera::Camera(ftl::gui::Screen *screen, ftl::rgbd::Source *src) : screen_(screen), src_(src) {
@@ -145,14 +148,17 @@ ftl::gui::Camera::Camera(ftl::gui::Screen *screen, ftl::rgbd::Source *src) : scr
 	posewin_->setTheme(screen->windowtheme);
 	posewin_->setVisible(false);
 
-	src->setCallback([this](int64_t ts, cv::Mat &rgb, cv::Mat &depth) {
+	src->setCallback([this](int64_t ts, cv::Mat &channel1, cv::Mat &channel2) {
 		UNIQUE_LOCK(mutex_, lk);
-		rgb_.create(rgb.size(), rgb.type());
-		depth_.create(depth.size(), depth.type());
-		cv::swap(rgb_,rgb);
-		cv::swap(depth_, depth);
-		cv::flip(rgb_,rgb_,0);
-		cv::flip(depth_,depth_,0);
+		im1_.create(channel1.size(), channel1.type());
+		im2_.create(channel2.size(), channel2.type());
+
+		//cv::swap(channel1, im1_);
+		//cv::swap(channel2, im2_);
+		
+		// OpenGL (0,0) bottom left
+		cv::flip(channel1, im1_, 0);
+		cv::flip(channel2, im2_, 0);
 	});
 }
 
@@ -231,7 +237,34 @@ void ftl::gui::Camera::showSettings() {
 
 }
 
+#ifdef HAVE_OPENVR
+bool ftl::gui::Camera::setVR(bool on) {
+	if (on == vr_mode_) {
+		LOG(WARNING) << "VR mode already enabled";
+		return on;
+	}
+
+	if (on) {
+		setChannel(Channel::Right);
+		vr_mode_ = true;
+	}
+	else {
+		vr_mode_ = false;
+		setChannel(Channel::Left); // reset to left channel
+	}
+	
+	return vr_mode_;
+}
+#endif
+
 void ftl::gui::Camera::setChannel(Channel c) {
+#ifdef HAVE_OPENVR
+	if (isVR()) {
+		LOG(ERROR) << "Changing channel in VR mode is not possible.";
+		return;
+	}
+#endif
+
 	channel_ = c;
 	switch (c) {
 	case Channel::Energy:
@@ -256,17 +289,6 @@ void ftl::gui::Camera::setChannel(Channel c) {
 	}
 }
 
-static Eigen::Matrix4d ConvertSteamVRMatrixToMatrix4( const vr::HmdMatrix34_t &matPose )
-{
-	Eigen::Matrix4d matrixObj;
-	matrixObj <<
-		matPose.m[0][0], matPose.m[1][0], matPose.m[2][0], 0.0,
-		matPose.m[0][1], matPose.m[1][1], matPose.m[2][1], 0.0,
-		matPose.m[0][2], matPose.m[1][2], matPose.m[2][2], 0.0,
-		matPose.m[0][3], matPose.m[1][3], matPose.m[2][3], 1.0f;
-	return matrixObj;
-}
-
 static void visualizeDepthMap(	const cv::Mat &depth, cv::Mat &out,
 								const float max_depth)
 {
@@ -308,11 +330,12 @@ static void drawEdges(	const cv::Mat &in, cv::Mat &out,
 bool ftl::gui::Camera::thumbnail(cv::Mat &thumb) {
 	UNIQUE_LOCK(mutex_, lk);
 	src_->grab(1,9);
-	if (rgb_.empty()) return false;
-	cv::resize(rgb_, thumb, cv::Size(320,180));
+	if (im1_.empty()) return false;
+	cv::resize(im1_, thumb, cv::Size(320,180));
+	cv::flip(thumb, thumb, 0);
 	return true;
 }
-
+#include <math.h>
 const GLTexture &ftl::gui::Camera::captureFrame() {
 	float now = (float)glfwGetTime();
 	delta_ = now - ftime_;
@@ -320,30 +343,49 @@ const GLTexture &ftl::gui::Camera::captureFrame() {
 
 	if (src_ && src_->isReady()) {
 		UNIQUE_LOCK(mutex_, lk);
-
-		if (screen_->hasVR()) {
+		
+		if (isVR()) {
 			#ifdef HAVE_OPENVR
-			src_->setChannel(Channel::Right);
-
 			vr::VRCompositor()->WaitGetPoses(rTrackedDevicePose_, vr::k_unMaxTrackedDeviceCount, NULL, 0 );
 
-			if ( rTrackedDevicePose_[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid )
+			if ((channel_ == Channel::Right) && rTrackedDevicePose_[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid )
 			{
-				auto pose = ConvertSteamVRMatrixToMatrix4( rTrackedDevicePose_[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking );
-				pose.inverse();
-
-				// Lerp the Eye
-				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::Matrix4d eye_l = ConvertSteamVRMatrixToMatrix4(
+					vr::VRSystem()->GetEyeToHeadTransform(vr::Eye_Left));
+				
+				// assumes eye_l(3, 0) = -eye_r(3, 0)
+				// => baseline = abs(2*eye_l(3, 0))
+				float baseline_in = 2.0 * -eye_l(0, 3);
+				if (baseline_in != baseline_) {
+					baseline_ = baseline_in;
+					// TODO: update baseline on ftl-reconstruct
+				}
+				
+				Eigen::Matrix4d pose = ConvertSteamVRMatrixToMatrix4(rTrackedDevicePose_[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking);
+				
+				// translate to L eye
 				Eigen::Translation3d trans(eye_);
 				Eigen::Affine3d t(trans);
 				Eigen::Matrix4d viewPose = t.matrix() * pose;
 
+				// flip rotation (different coordinate axis on OpenGL/ftl)
+				// NOTE: image also flipped!
+
+				Eigen::Vector3d ea = viewPose.block<3, 3>(0, 0).eulerAngles(0, 1, 2);
+				//double rd = 180.0 / M_PI;
+				//LOG(INFO) << "Camera X: " << ea[0] *rd << ", Y: " << ea[1] * rd << ", Z: " << ea[2] * rd;
+
+				// 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()); 
+				viewPose.block<3, 3>(0, 0) = R;
+
 				if (src_->hasCapabilities(ftl::rgbd::kCapMovable)) src_->setPose(viewPose);
+				
 			} else {
-				LOG(ERROR) << "No VR Pose";
+				//LOG(ERROR) << "No VR Pose";
 			}
 			#endif
 		} else {
@@ -360,10 +402,11 @@ const GLTexture &ftl::gui::Camera::captureFrame() {
 		}
 
 		src_->grab();
-		//src_->getFrames(rgb, depth);
 
 		// When switching from right to depth, client may still receive
 		// right images from previous batch (depth.channels() == 1 check)
+
+		/* todo: does not work
 		if (channel_ == Channel::Deviation &&
 			depth_.rows > 0 && depth_.channels() == 1)
 		{
@@ -372,60 +415,68 @@ const GLTexture &ftl::gui::Camera::captureFrame() {
 			}
 			
 			stats_->update(depth_);
-		}
+		}*/
 
 		cv::Mat tmp;
 
 		switch(channel_) {
 			case Channel::Confidence:
-				if (depth_.rows == 0) { break; }
-				visualizeEnergy(depth_, tmp, 1.0);
-				texture_.update(tmp);
+				if (im2_.rows == 0) { break; }
+				visualizeEnergy(im2_, tmp, 1.0);
+				texture2_.update(tmp);
 				break;
+			
 			case Channel::Density:
 			case Channel::Energy:
-				if (depth_.rows == 0) { break; }
-				visualizeEnergy(depth_, tmp, 10.0);
-				texture_.update(tmp);
+				if (im2_.rows == 0) { break; }
+				visualizeEnergy(im2_, tmp, 10.0);
+				texture2_.update(tmp);
 				break;
+			
 			case Channel::Depth:
-				if (depth_.rows == 0) { break; }
-				visualizeDepthMap(depth_, tmp, 7.0);
-				if (screen_->root()->value("showEdgesInDepth", false)) drawEdges(rgb_, tmp);
-				texture_.update(tmp);
+				if (im2_.rows == 0) { break; }
+				visualizeDepthMap(im2_, tmp, 7.0);
+				if (screen_->root()->value("showEdgesInDepth", false)) drawEdges(im1_, tmp);
+				texture2_.update(tmp);
 				break;
 			
 			case Channel::Deviation:
-				if (depth_.rows == 0) { break; }
+				if (im2_.rows == 0) { break; }/*
 				//imageSize = Vector2f(depth.cols, depth.rows);
 				stats_->getStdDev(tmp);
 				tmp.convertTo(tmp, CV_8U, 1000.0);
 				applyColorMap(tmp, tmp, cv::COLORMAP_HOT);
-				texture_.update(tmp);
+				texture2_.update(tmp);*/
 				break;
 
 		case Channel::Flow:
 		case Channel::Normals:
 		case Channel::Right:
-				if (depth_.rows == 0 || depth_.type() != CV_8UC3) { break; }
-				texture_.update(depth_);
+				if (im2_.rows == 0 || im2_.type() != CV_8UC3) { break; }
+				texture2_.update(im2_);
 				break;
 
 			default:
-				if (rgb_.rows == 0) { break; }
+				break;
+				/*if (rgb_.rows == 0) { break; }
 				//imageSize = Vector2f(rgb.cols,rgb.rows);
 				texture_.update(rgb_);
-
 				#ifdef HAVE_OPENVR
 				if (screen_->hasVR() && depth_.channels() >= 3) {
 					LOG(INFO) << "DRAW RIGHT";
 					textureRight_.update(depth_);
 				}
 				#endif
+				*/
+		}
+
+		if (im1_.rows != 0) {
+			//imageSize = Vector2f(rgb.cols,rgb.rows);
+			texture1_.update(im1_);
 		}
 	}
 
-	return texture_;
+	return texture1_;
 }
 
 nlohmann::json ftl::gui::Camera::getMetaData() {

applications/gui/src/camera.hpp

@@ -39,14 +39,15 @@ class Camera {
 	void showSettings();
 
 	void setChannel(ftl::codecs::Channel c);
-
+	const ftl::codecs::Channel getChannel() { return channel_; }
+	
 	void togglePause();
 	void isPaused();
 	const ftl::codecs::Channels &availableChannels();
 
 	const GLTexture &captureFrame();
-	const GLTexture &getLeft() const { return texture_; }
-	const GLTexture &getRight() const { return textureRight_; }
+	const GLTexture &getLeft() const { return texture1_; }
+	const GLTexture &getRight() const { return texture2_; }
 
 	bool thumbnail(cv::Mat &thumb);
 
@@ -54,12 +55,21 @@ class Camera {
 
 	StatisticsImage *stats_ = nullptr;
 
+
+#ifdef HAVE_OPENVR
+	bool isVR() { return vr_mode_; }
+	bool setVR(bool on);
+#else
+	bool isVR() { return false; }
+#endif
+
 	private:
 	Screen *screen_;
 	ftl::rgbd::Source *src_;
 	GLTexture thumb_;
-	GLTexture texture_;
-	GLTexture textureRight_;
+	GLTexture texture1_; // first channel (always left at the moment)
+	GLTexture texture2_; // second channel ("right")
+
 	ftl::gui::PoseWindow *posewin_;
 	nlohmann::json meta_;
 	Eigen::Vector4d neye_;
@@ -73,12 +83,15 @@ class Camera {
 	bool pause_;
 	ftl::codecs::Channel channel_;
 	ftl::codecs::Channels channels_;
-	cv::Mat rgb_;
-	cv::Mat depth_;
+	cv::Mat im1_; // first channel (left)
+	cv::Mat im2_; // second channel ("right")
+
 	MUTEX mutex_;
 
 	#ifdef HAVE_OPENVR
 	vr::TrackedDevicePose_t rTrackedDevicePose_[ vr::k_unMaxTrackedDeviceCount ];
+	bool vr_mode_;
+	float baseline_;
 	#endif
 };
 

applications/gui/src/ctrl_window.cpp

@@ -26,8 +26,8 @@ ControlWindow::ControlWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl)
 	_updateDetails();
 
 	auto tools = new Widget(this);
-    tools->setLayout(new BoxLayout(Orientation::Horizontal,
-                                       Alignment::Middle, 0, 6));
+	tools->setLayout(new BoxLayout(	Orientation::Horizontal,
+									Alignment::Middle, 0, 6));
 
 	auto button = new Button(tools, "", ENTYPO_ICON_PLUS);
 	button->setCallback([this] {
@@ -35,6 +35,9 @@ ControlWindow::ControlWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl)
 		_addNode();
 	});
 	button->setTooltip("Add new node");
+	
+	// commented-out buttons not working/useful
+	/*
 	button = new Button(tools, "", ENTYPO_ICON_CYCLE);
 	button->setCallback([this] {
 		ctrl_->restart();
@@ -43,7 +46,7 @@ ControlWindow::ControlWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl)
 	button->setCallback([this] {
 		ctrl_->pause();
 	});
-	button->setTooltip("Pause all nodes");
+	button->setTooltip("Pause all nodes");*/
 
 	new Label(this, "Select Node","sans-bold");
 	auto select = new ComboBox(this, node_titles_);
@@ -55,14 +58,14 @@ ControlWindow::ControlWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl)
 	new Label(this, "Node Options","sans-bold");
 
 	tools = new Widget(this);
-    tools->setLayout(new BoxLayout(Orientation::Horizontal,
-                                       Alignment::Middle, 0, 6));
+	tools->setLayout(new BoxLayout(	Orientation::Horizontal,
+									Alignment::Middle, 0, 6));
 
-	button = new Button(tools, "", ENTYPO_ICON_INFO);
+	/*button = new Button(tools, "", ENTYPO_ICON_INFO);
 	button->setCallback([this] {
 		
 	});
-	button->setTooltip("Node status information");
+	button->setTooltip("Node status information");*/
 
 	button = new Button(tools, "", ENTYPO_ICON_COG);
 	button->setCallback([this,parent] {
@@ -71,17 +74,17 @@ ControlWindow::ControlWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl)
 	});
 	button->setTooltip("Edit node configuration");
 
-	button = new Button(tools, "", ENTYPO_ICON_CYCLE);
+	/*button = new Button(tools, "", ENTYPO_ICON_CYCLE);
 	button->setCallback([this] {
 		ctrl_->restart(_getActiveID());
 	});
-	button->setTooltip("Restart this node");
+	button->setTooltip("Restart this node");*/
 
-	button = new Button(tools, "", ENTYPO_ICON_CONTROLLER_PAUS);
+	/*button = new Button(tools, "", ENTYPO_ICON_CONTROLLER_PAUS);
 	button->setCallback([this] {
 		ctrl_->pause(_getActiveID());
 	});
-	button->setTooltip("Pause node processing");
+	button->setTooltip("Pause node processing");*/
 
 	ctrl->getNet()->onConnect([this,select](ftl::net::Peer *p) {
 		_updateDetails();

applications/gui/src/media_panel.cpp

@@ -19,6 +19,7 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
 
 	paused_ = false;
 	writer_ = nullptr;
+	disable_switch_channels_ = false;
 
 	setLayout(new BoxLayout(Orientation::Horizontal,
 									Alignment::Middle, 5, 10));
@@ -77,6 +78,7 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
 
 	//button = new Button(this, "", ENTYPO_ICON_CONTROLLER_RECORD);
 
+	/* Doesn't work at the moment
  #ifdef HAVE_LIBARCHIVE
 	auto button_snapshot = new Button(this, "", ENTYPO_ICON_IMAGES);
 	button_snapshot->setTooltip("Screen capture");
@@ -102,103 +104,133 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen) : nanogui::Window(screen, ""),
 	});
 #endif
 
-    auto popbutton = new PopupButton(this, "", ENTYPO_ICON_LAYERS);
-    popbutton->setSide(Popup::Side::Right);
-	popbutton->setChevronIcon(ENTYPO_ICON_CHEVRON_SMALL_RIGHT);
-    Popup *popup = popbutton->popup();
-    popup->setLayout(new GroupLayout());
+	
+	// not very useful (l/r)
+
+	auto button_dual = new Button(this, "", ENTYPO_ICON_MAP);
+	button_dual->setCallback([this]() {
+		screen_->setDualView(!screen_->getDualView());
+	});
+	*/
+
+#ifdef HAVE_OPENVR
+	if (this->screen_->hasVR()) {
+		auto button_vr = new Button(this, "VR");
+		button_vr->setFlags(Button::ToggleButton);
+		button_vr->setChangeCallback([this, button_vr](bool state) {
+			if (!screen_->useVR()) {
+				if (screen_->switchVR(true) == true) {
+					button_vr->setTextColor(nanogui::Color(0.5f,0.5f,1.0f,1.0f));
+					this->button_channels_->setEnabled(false);
+				}
+			}
+			else {
+				if (screen_->switchVR(false) == false) {
+					button_vr->setTextColor(nanogui::Color(1.0f,1.0f,1.0f,1.0f));
+					this->button_channels_->setEnabled(true);
+				}
+			}
+		});
+	}
+#endif
+
+	button_channels_ = new PopupButton(this, "", ENTYPO_ICON_LAYERS);
+	button_channels_->setSide(Popup::Side::Right);
+	button_channels_->setChevronIcon(ENTYPO_ICON_CHEVRON_SMALL_RIGHT);
+	Popup *popup = button_channels_->popup();
+	popup->setLayout(new GroupLayout());
 	popup->setTheme(screen->toolbuttheme);
-    popup->setAnchorHeight(150);
-
-    button = new Button(popup, "Left");
-    button->setFlags(Button::RadioButton);
-    button->setPushed(true);
-    button->setCallback([this]() {
-        ftl::gui::Camera *cam = screen_->activeCamera();
-        if (cam) {
-            cam->setChannel(Channel::Left);
-        }
-    });
+	popup->setAnchorHeight(150);
+
+	button = new Button(popup, "Left");
+	button->setFlags(Button::RadioButton);
+	button->setPushed(true);
+	button->setCallback([this]() {
+		ftl::gui::Camera *cam = screen_->activeCamera();
+		if (cam) {
+			cam->setChannel(Channel::Left);
+		}
+	});
 
 	right_button_ = new Button(popup, "Right");
-    right_button_->setFlags(Button::RadioButton);
-    right_button_->setCallback([this]() {
-        ftl::gui::Camera *cam = screen_->activeCamera();
-        if (cam) {
-            cam->setChannel(Channel::Right);
-        }
-    });
-
-    depth_button_ = new Button(popup, "Depth");
-    depth_button_->setFlags(Button::RadioButton);
-    depth_button_->setCallback([this]() {
-        ftl::gui::Camera *cam = screen_->activeCamera();
-        if (cam) {
-            cam->setChannel(Channel::Depth);
-        }
-    });
-
-	popbutton = new PopupButton(popup, "More");
-    popbutton->setSide(Popup::Side::Right);
+	right_button_->setFlags(Button::RadioButton);
+	right_button_->setCallback([this]() {
+		ftl::gui::Camera *cam = screen_->activeCamera();
+		if (cam) {
+			cam->setChannel(Channel::Right);
+		}
+	});
+
+	depth_button_ = new Button(popup, "Depth");
+	depth_button_->setFlags(Button::RadioButton);
+	depth_button_->setCallback([this]() {
+		ftl::gui::Camera *cam = screen_->activeCamera();
+		if (cam) {
+			cam->setChannel(Channel::Depth);
+		}
+	});
+
+	auto *popbutton = new PopupButton(popup, "More");
+	popbutton->setSide(Popup::Side::Right);
 	popbutton->setChevronIcon(ENTYPO_ICON_CHEVRON_SMALL_RIGHT);
-    popup = popbutton->popup();
-    popup->setLayout(new GroupLayout());
+	popup = popbutton->popup();
+	popup->setLayout(new GroupLayout());
 	popup->setTheme(screen->toolbuttheme);
-    popup->setAnchorHeight(150);
+	popup->setAnchorHeight(150);
 
-    button = new Button(popup, "Deviation");
-    button->setFlags(Button::RadioButton);
-    button->setCallback([this]() {
-        ftl::gui::Camera *cam = screen_->activeCamera();
-        if (cam) {
-            cam->setChannel(Channel::Deviation);
-        }
-    });
+	button = new Button(popup, "Deviation");
+	button->setFlags(Button::RadioButton);
+	button->setCallback([this]() {
+		ftl::gui::Camera *cam = screen_->activeCamera();
+		if (cam) {
+			cam->setChannel(Channel::Deviation);
+		}
+	});
 
 	button = new Button(popup, "Normals");
-    button->setFlags(Button::RadioButton);
-    button->setCallback([this]() {
-        ftl::gui::Camera *cam = screen_->activeCamera();
-        if (cam) {
-            cam->setChannel(Channel::Normals);
-        }
-    });
+	button->setFlags(Button::RadioButton);
+	button->setCallback([this]() {
+		ftl::gui::Camera *cam = screen_->activeCamera();
+		if (cam) {
+			cam->setChannel(Channel::Normals);
+		}
+	});
 
 	button = new Button(popup, "Flow");
-    button->setFlags(Button::RadioButton);
-    button->setCallback([this]() {
-        ftl::gui::Camera *cam = screen_->activeCamera();
-        if (cam) {
-            cam->setChannel(Channel::Flow);
-        }
-    });
+	button->setFlags(Button::RadioButton);
+	button->setCallback([this]() {
+		ftl::gui::Camera *cam = screen_->activeCamera();
+		if (cam) {
+			cam->setChannel(Channel::Flow);
+		}
+	});
 
 	button = new Button(popup, "Confidence");
-    button->setFlags(Button::RadioButton);
-    button->setCallback([this]() {
-        ftl::gui::Camera *cam = screen_->activeCamera();
-        if (cam) {
-            cam->setChannel(Channel::Confidence);
-        }
-    });
-
-    button = new Button(popup, "Energy");
-    button->setFlags(Button::RadioButton);
-    button->setCallback([this]() {
-        ftl::gui::Camera *cam = screen_->activeCamera();
-        if (cam) {
-            cam->setChannel(Channel::Energy);
-        }
-    });
+	button->setFlags(Button::RadioButton);
+	button->setCallback([this]() {
+		ftl::gui::Camera *cam = screen_->activeCamera();
+		if (cam) {
+			cam->setChannel(Channel::Confidence);
+		}
+	});
+
+	button = new Button(popup, "Energy");
+	button->setFlags(Button::RadioButton);
+	button->setCallback([this]() {
+		ftl::gui::Camera *cam = screen_->activeCamera();
+		if (cam) {
+			cam->setChannel(Channel::Energy);
+		}
+	});
 
 	button = new Button(popup, "Density");
-    button->setFlags(Button::RadioButton);
-    button->setCallback([this]() {
-        ftl::gui::Camera *cam = screen_->activeCamera();
-        if (cam) {
-            cam->setChannel(Channel::Density);
-        }
-    });
+	button->setFlags(Button::RadioButton);
+	button->setCallback([this]() {
+		ftl::gui::Camera *cam = screen_->activeCamera();
+		if (cam) {
+			cam->setChannel(Channel::Density);
+		}
+	});
 
 }
 
@@ -208,12 +240,12 @@ MediaPanel::~MediaPanel() {
 
 // Update button enabled status
 void MediaPanel::cameraChanged() {
-    ftl::gui::Camera *cam = screen_->activeCamera();
-    if (cam) {
-        if (cam->source()->hasCapabilities(ftl::rgbd::kCapStereo)) {
-            right_button_->setEnabled(true);
-        } else {
-            right_button_->setEnabled(false);
-        }
-    }
+	ftl::gui::Camera *cam = screen_->activeCamera();
+	if (cam) {
+		if (cam->source()->hasCapabilities(ftl::rgbd::kCapStereo)) {
+			right_button_->setEnabled(true);
+		} else {
+			right_button_->setEnabled(false);
+		}
+	}
 }

applications/gui/src/media_panel.hpp

@@ -14,18 +14,22 @@ namespace gui {
 class Screen;
 
 class MediaPanel : public nanogui::Window {
-    public:
-    explicit MediaPanel(ftl::gui::Screen *);
-    ~MediaPanel();
-
-    void cameraChanged();
-
-    private:
-    ftl::gui::Screen *screen_;
-    bool paused_;
-    ftl::rgbd::SnapshotStreamWriter *writer_;
-    nanogui::Button *right_button_;
-    nanogui::Button *depth_button_;
+	public:
+	explicit MediaPanel(ftl::gui::Screen *);
+	~MediaPanel();
+
+	void cameraChanged();
+
+	private:
+	ftl::gui::Screen *screen_;
+
+	bool paused_;
+	bool disable_switch_channels_;
+
+	ftl::rgbd::SnapshotStreamWriter *writer_;
+	nanogui::PopupButton *button_channels_;
+	nanogui::Button *right_button_;
+	nanogui::Button *depth_button_;
 };
 
 }

applications/gui/src/screen.cpp

@@ -20,6 +20,10 @@
 #include "camera.hpp"
 #include "media_panel.hpp"
 
+#ifdef HAVE_OPENVR
+#include "vr.hpp"
+#endif
+
 using ftl::gui::Screen;
 using ftl::gui::Camera;
 using std::string;
@@ -27,28 +31,28 @@ using ftl::rgbd::Source;
 using ftl::rgbd::isValidDepth;
 
 namespace {
-    constexpr char const *const defaultImageViewVertexShader =
-        R"(#version 330
-        uniform vec2 scaleFactor;
-        uniform vec2 position;
-        in vec2 vertex;
-        out vec2 uv;
-        void main() {
-            uv = vertex;
-            vec2 scaledVertex = (vertex * scaleFactor) + position;
-            gl_Position  = vec4(2.0*scaledVertex.x - 1.0,
-                                2.0*scaledVertex.y - 1.0,
-                                0.0, 1.0);
-        })";
-
-    constexpr char const *const defaultImageViewFragmentShader =
-        R"(#version 330
-        uniform sampler2D image;
-        out vec4 color;
-        in vec2 uv;
-        void main() {
-            color = texture(image, uv);
-        })";
+	constexpr char const *const defaultImageViewVertexShader =
+		R"(#version 330
+		uniform vec2 scaleFactor;
+		uniform vec2 position;
+		in vec2 vertex;
+		out vec2 uv;
+		void main() {
+			uv = vertex;
+			vec2 scaledVertex = (vertex * scaleFactor) + position;
+			gl_Position  = vec4(2.0*scaledVertex.x - 1.0,
+								2.0*scaledVertex.y - 1.0,
+								0.0, 1.0);
+		})";
+
+	constexpr char const *const defaultImageViewFragmentShader =
+		R"(#version 330
+		uniform sampler2D image;
+		out vec4 color;
+		in vec2 uv;
+		void main() {
+			color = texture(image, uv);
+		})";
 }
 
 ftl::gui::Screen::Screen(ftl::Configurable *proot, ftl::net::Universe *pnet, ftl::ctrl::Master *controller) :
@@ -60,6 +64,11 @@ ftl::gui::Screen::Screen(ftl::Configurable *proot, ftl::net::Universe *pnet, ftl
 	root_ = proot;
 	camera_ = nullptr;
 
+	#ifdef HAVE_OPENVR
+	HMD_ = nullptr;
+	has_vr_ = vr::VR_IsHmdPresent();
+	#endif
+
 	setSize(Vector2i(1280,720));
 
 	toolbuttheme = new Theme(*theme());
@@ -83,7 +92,7 @@ ftl::gui::Screen::Screen(ftl::Configurable *proot, ftl::net::Universe *pnet, ftl
 	mediatheme->mButtonGradientTopFocused = nanogui::Color(80,230);
 	mediatheme->mButtonGradientBotFocused = nanogui::Color(80,230);
 	mediatheme->mIconColor = nanogui::Color(255,255);
-    mediatheme->mTextColor = nanogui::Color(1.0f,1.0f,1.0f,1.0f);
+	mediatheme->mTextColor = nanogui::Color(1.0f,1.0f,1.0f,1.0f);
 	mediatheme->mBorderDark = nanogui::Color(0,0);
 	mediatheme->mBorderMedium = nanogui::Color(0,0);
 	mediatheme->mBorderLight = nanogui::Color(0,0);
@@ -161,9 +170,9 @@ ftl::gui::Screen::Screen(ftl::Configurable *proot, ftl::net::Universe *pnet, ftl
 	popbutton->setSide(Popup::Side::Right);
 	popbutton->setChevronIcon(0);
 	Popup *popup = popbutton->popup();
-    popup->setLayout(new GroupLayout());
+	popup->setLayout(new GroupLayout());
 	popup->setTheme(toolbuttheme);
-    //popup->setAnchorHeight(100);
+	//popup->setAnchorHeight(100);
 
 	auto itembutton = new Button(popup, "Add Camera", ENTYPO_ICON_CAMERA);
 	itembutton->setCallback([this,popup]() {
@@ -185,7 +194,7 @@ ftl::gui::Screen::Screen(ftl::Configurable *proot, ftl::net::Universe *pnet, ftl
 	popbutton->setSide(Popup::Side::Right);
 	popbutton->setChevronIcon(0);
 	popup = popbutton->popup();
-    popup->setLayout(new GroupLayout());
+	popup->setLayout(new GroupLayout());
 	popup->setTheme(toolbuttheme);
 	//popbutton->setCallback([this]() {
 	//	cwindow_->setVisible(true);
@@ -244,30 +253,60 @@ ftl::gui::Screen::Screen(ftl::Configurable *proot, ftl::net::Universe *pnet, ftl
 
 	setVisible(true);
 	performLayout();
+}
 
+#ifdef HAVE_OPENVR
+bool ftl::gui::Screen::initVR() {
+	if (!vr::VR_IsHmdPresent()) {
+		return false;
+	}
 
-	#ifdef HAVE_OPENVR
-	if (vr::VR_IsHmdPresent()) {
-		// Loading the SteamVR Runtime
-		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 );
-		}
-	} else {
+	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);
 	}
-	#endif
+
+	uint32_t size_x, size_y;
+	HMD_->GetRecommendedRenderTargetSize(&size_x, &size_y);
+	LOG(INFO) << size_x << ", " << size_y;
+	LOG(INFO) << "\n" << getCameraMatrix(HMD_, vr::Eye_Left);
+	return true;
+}
+
+bool ftl::gui::Screen::useVR() {
+	auto *cam = activeCamera();
+	if (HMD_ == nullptr || cam == nullptr) { return false; }
+	return cam->isVR();
 }
 
+bool ftl::gui::Screen::switchVR(bool on) {
+	if (useVR() == on) { return on; }
+
+	if (on && (HMD_ == nullptr) && !initVR()) {
+		return false;
+	}
+
+	if (on) {
+		activeCamera()->setVR(true);
+	} else {
+		activeCamera()->setVR(false);
+	}
+	
+	return useVR();
+}
+#endif
+
 ftl::gui::Screen::~Screen() {
 	mShader.free();
 
 	#ifdef HAVE_OPENVR
-	vr::VR_Shutdown();
+	if (HMD_ != nullptr) {
+		vr::VR_Shutdown();
+	}
 	#endif
 }
 
@@ -361,13 +400,19 @@ void ftl::gui::Screen::draw(NVGcontext *ctx) {
 		leftEye_ = mImageID;
 		rightEye_ = camera_->getRight().texture();
 
+		if (camera_->getChannel() != ftl::codecs::Channel::Left) { mImageID = rightEye_; }
+
 		#ifdef HAVE_OPENVR
-		if (hasVR() && imageSize[0] > 0 && camera_->getLeft().isValid() && camera_->getRight().isValid()) {
+		if (useVR() && imageSize[0] > 0 && camera_->getLeft().isValid() && camera_->getRight().isValid()) {
+			
 			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 );
+			
+			mImageID = leftEye_;
 		}
 		#endif
 
@@ -400,4 +445,4 @@ void ftl::gui::Screen::draw(NVGcontext *ctx) {
 	/* Draw the user interface */
 	screen()->performLayout(ctx);
 	nanogui::Screen::draw(ctx);
-}
\ No newline at end of file
+}

applications/gui/src/screen.hpp

@@ -43,11 +43,27 @@ class Screen : public nanogui::Screen {
 	void setActiveCamera(ftl::gui::Camera*);
 	ftl::gui::Camera *activeCamera() { return camera_; }
 
-	#ifdef HAVE_OPENVR
-	bool hasVR() const { return HMD_ != nullptr; }
-	#else
+#ifdef HAVE_OPENVR
+	// initialize OpenVR
+	bool initVR();
+
+	// is VR available (HMD was found at initialization)
+	bool hasVR() const { return has_vr_; }
+
+	// is VR mode on/off
+	bool useVR();
+
+	// toggle VR on/off
+	bool switchVR(bool mode);
+
+	vr::IVRSystem* getVR() { return HMD_; }
+
+#else
 	bool hasVR() const { return false; }
-	#endif
+#endif
+
+	void setDualView(bool v) { show_two_images_ = v; LOG(INFO) << "CLICK"; }
+	bool getDualView() { return show_two_images_; }
 
 	nanogui::Theme *windowtheme;
 	nanogui::Theme *specialtheme;
@@ -58,10 +74,11 @@ class Screen : public nanogui::Screen {
 	ftl::gui::SourceWindow *swindow_;
 	ftl::gui::ControlWindow *cwindow_;
 	ftl::gui::MediaPanel *mwindow_;
+
 	//std::vector<SourceViews> sources_;
 	ftl::net::Universe *net_;
 	nanogui::GLShader mShader;
-    GLuint mImageID;
+	GLuint mImageID;
 	//Source *src_;
 	GLTexture texture_;
 	Eigen::Vector3f eye_;
@@ -82,7 +99,10 @@ class Screen : public nanogui::Screen {
 	GLuint leftEye_;
 	GLuint rightEye_;
 
+	bool show_two_images_ = false;
+
 	#ifdef HAVE_OPENVR
+	bool has_vr_;
 	vr::IVRSystem *HMD_;
 	#endif
 };

applications/gui/src/src_window.cpp

@@ -61,10 +61,9 @@ SourceWindow::SourceWindow(ftl::gui::Screen *screen)
 
 	screen->net()->onConnect([this](ftl::net::Peer *p) {
 		UNIQUE_LOCK(mutex_, lk);
-		//select->setItems(available_);
 		_updateCameras(screen_->net()->findAll<string>("list_streams"));
 	});
-
+	
 	UNIQUE_LOCK(mutex_, lk);
 
 	std::vector<ftl::rgbd::Source*> srcs = ftl::createArray<ftl::rgbd::Source>(screen_->root(), "sources", screen_->net());

applications/gui/src/vr.cpp

+#include "loguru.hpp"
+#include "vr.hpp"
+
+Eigen::Matrix3d getCameraMatrix(const double tanx1,
+								const double tanx2,
+								const double tany1,
+								const double tany2,
+								const double size_x,
+								const double size_y) {
+	
+	Eigen::Matrix3d C = Eigen::Matrix3d::Identity();
+	
+	CHECK(tanx1 < 0 && tanx2 > 0 && tany1 < 0 && tany2 > 0);
+	CHECK(size_x > 0 && size_y > 0);
+
+	double fx = size_x / (-tanx1 + tanx2);
+	double fy = size_y / (-tany1 + tany2);
+	C(0,0) = fx;
+	C(1,1) = fy;
+	C(0,2) = tanx1 * fx;
+	C(1,2) = tany1 * fy;
+
+	// safe to remove
+	CHECK((int) (abs(tanx1 * fx) + abs(tanx2 * fx)) == (int) size_x);
+	CHECK((int) (abs(tany1 * fy) + abs(tany2 * fy)) == (int) size_y);
+
+	return C;
+}
+
+Eigen::Matrix3d getCameraMatrix(vr::IVRSystem *vr, const vr::Hmd_Eye &eye) {
+	float tanx1, tanx2, tany1, tany2;
+	uint32_t size_x, size_y;
+	vr->GetProjectionRaw(eye, &tanx1, &tanx2, &tany1, &tany2);
+	vr->GetRecommendedRenderTargetSize(&size_x, &size_y);
+	return getCameraMatrix(tanx1, tanx2, tany1, tany2, size_x, size_y);
+}
\ No newline at end of file

applications/gui/src/vr.hpp

+#include <openvr/openvr.h>
+#include <Eigen/Eigen>
+#include <openvr/openvr.h>
+
+/* @brief	Calculate (pinhole camera) intrinsic matrix from OpenVR parameters
+ * @param	Tangent of left half angle (negative) from center view axis
+ * @param	Tangent of right half angle from center view axis
+ * @param	Tangent of top half angle (negative) from center view axis
+ * @param	Tangent of bottom half angle from center view axis
+ * @param	Image width
+ * @param	Image height
+ * 
+ * Parameters are provided by IVRSystem::GetProjectionRaw and
+ * IVRSystem::GetRecommendedRenderTargetSize.
+ * 
+ * tanx1 = x1 / fx		(1)
+ * tanx2 = x2 / fy		(2)
+ * x1 + x2 = size_x		(3)
+ * 
+ * :. fx = size_x / (-tanx1 + tanx2)
+ * 
+ * fy can be calculated in same way
+ */
+Eigen::Matrix3d getCameraMatrix(const double tanx1,
+								const double tanx2,
+								const double tany1,
+								const double tany2,
+								const double size_x,
+								const double size_y);
+
+/*
+ * @brief	Same as above, but uses given IVRSystem and eye.
+ */
+Eigen::Matrix3d getCameraMatrix(vr::IVRSystem *vr, const vr::Hmd_Eye &eye);
+
+
+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], 0.0,
+		matPose.m[1][0], matPose.m[1][1], matPose.m[1][2], 0.0,
+		matPose.m[2][0], matPose.m[2][1], matPose.m[2][2], 0.0,
+		matPose.m[3][0], matPose.m[3][1], matPose.m[3][2], 1.0f;
+	return matrixObj.transpose();
+}
+
+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.transpose();
+}
\ No newline at end of file

components/codecs/src/nvpipe_decoder.cpp

@@ -21,11 +21,36 @@ NvPipeDecoder::~NvPipeDecoder() {
 	}
 }
 
+void cropAndScaleUp(cv::Mat &in, cv::Mat &out) {
+	CHECK(in.type() == out.type());
+
+	auto isize = in.size();
+	auto osize = out.size();
+	cv::Mat tmp;
+	
+	if (isize != osize) {
+		double x_scale = ((double) isize.width) / osize.width;
+		double y_scale = ((double) isize.height) / osize.height;
+		double x_scalei = 1.0 / x_scale;
+		double y_scalei = 1.0 / y_scale;
+		cv::Size sz_crop;
+
+		// assume downscaled image
+		if (x_scalei > y_scalei) {
+			sz_crop = cv::Size(isize.width, isize.height * x_scale);
+		} else {
+			sz_crop = cv::Size(isize.width * y_scale, isize.height);
+		}
+
+		tmp = in(cv::Rect(cv::Point2i(0, 0), sz_crop));
+		cv::resize(tmp, out, osize);
+	}
+}
+
 bool NvPipeDecoder::decode(const ftl::codecs::Packet &pkt, cv::Mat &out) {
 	cudaSetDevice(0);
 	UNIQUE_LOCK(mutex_,lk);
 	if (pkt.codec != codec_t::HEVC) return false;
-
 	bool is_float_frame = out.type() == CV_32F;
 
 	// Is the previous decoder still valid for current resolution and type?
@@ -53,7 +78,7 @@ bool NvPipeDecoder::decode(const ftl::codecs::Packet &pkt, cv::Mat &out) {
 
 		seen_iframe_ = false;
 	}
-
+	
 	// TODO: (Nick) Move to member variable to prevent re-creation
 	cv::Mat tmp(cv::Size(ftl::codecs::getWidth(pkt.definition),ftl::codecs::getHeight(pkt.definition)), (is_float_frame) ? CV_16U : CV_8UC4);
 
@@ -72,8 +97,11 @@ bool NvPipeDecoder::decode(const ftl::codecs::Packet &pkt, cv::Mat &out) {
 		if (out.rows == ftl::codecs::getHeight(pkt.definition)) {
 			tmp.convertTo(out, CV_32FC1, 1.0f/1000.0f);
 		} else {
-			tmp.convertTo(tmp, CV_32FC1, 1.0f/1000.0f);
+			cv::cvtColor(tmp, tmp, cv::COLOR_BGRA2BGR);
 			cv::resize(tmp, out, out.size());
+			//cv::Mat tmp2;
+			//tmp.convertTo(tmp2, CV_32FC1, 1.0f/1000.0f);
+			//cropAndScaleUp(tmp2, out);
 		}
 	} else {
 		// Is the received frame the same size as requested output?
@@ -82,6 +110,9 @@ bool NvPipeDecoder::decode(const ftl::codecs::Packet &pkt, cv::Mat &out) {
 		} else {
 			cv::cvtColor(tmp, tmp, cv::COLOR_BGRA2BGR);
 			cv::resize(tmp, out, out.size());
+			//cv::Mat tmp2;
+			//cv::cvtColor(tmp, tmp2, cv::COLOR_BGRA2BGR);
+			//cropAndScaleUp(tmp2, out);
 		}
 	}
 

components/codecs/src/nvpipe_encoder.cpp

@@ -16,14 +16,14 @@ using ftl::codecs::Packet;
 
 NvPipeEncoder::NvPipeEncoder(definition_t maxdef,
 			definition_t mindef) : Encoder(maxdef, mindef, ftl::codecs::device_t::Hardware) {
-    nvenc_ = nullptr;
-    current_definition_ = definition_t::HD1080;
-    is_float_channel_ = false;
+	nvenc_ = nullptr;
+	current_definition_ = definition_t::HD1080;
+	is_float_channel_ = false;
 	was_reset_ = false;
 }
 
 NvPipeEncoder::~NvPipeEncoder() {
-    if (nvenc_) NvPipe_Destroy(nvenc_);
+	if (nvenc_) NvPipe_Destroy(nvenc_);
 }
 
 void NvPipeEncoder::reset() {
@@ -43,87 +43,119 @@ definition_t NvPipeEncoder::_verifiedDefinition(definition_t def, const cv::Mat
 	return def;
 }
 
+void scaleDownAndPad(cv::Mat &in, cv::Mat &out) {
+	const auto isize = in.size();
+	const auto osize = out.size();
+	cv::Mat tmp;
+	
+	if (isize != osize) {
+		double x_scale = ((double) isize.width) / osize.width;
+		double y_scale = ((double) isize.height) / osize.height;
+		double x_scalei = 1.0 / x_scale;
+		double y_scalei = 1.0 / y_scale;
+
+		if (x_scale > 1.0 || y_scale > 1.0) {
+			if (x_scale > y_scale) {
+				cv::resize(in, tmp, cv::Size(osize.width, osize.height * x_scalei));
+			} else {
+				cv::resize(in, tmp, cv::Size(osize.width * y_scalei, osize.height));
+			}
+		}
+		else { tmp = in; }
+		
+		if (tmp.size().width < osize.width || tmp.size().height < osize.height) {
+			tmp.copyTo(out(cv::Rect(cv::Point2i(0, 0), tmp.size())));
+		}
+		else { out = tmp; }
+	}
+}
+
 bool NvPipeEncoder::encode(const cv::Mat &in, definition_t odefinition, bitrate_t bitrate, const std::function<void(const ftl::codecs::Packet&)> &cb) {
-    cudaSetDevice(0);
+	cudaSetDevice(0);
 	auto definition = _verifiedDefinition(odefinition, in);
 
 	//LOG(INFO) << "Definition: " << ftl::codecs::getWidth(definition) << "x" << ftl::codecs::getHeight(definition);
 
-    if (in.empty()) {
-        LOG(ERROR) << "Missing data for Nvidia encoder";
-        return false;
-    }
-    if (!_createEncoder(in, definition, bitrate)) return false;
+	if (in.empty()) {
+		LOG(ERROR) << "Missing data for Nvidia encoder";
+		return false;
+	}
+	if (!_createEncoder(in, definition, bitrate)) return false;
 
-    //LOG(INFO) << "NvPipe Encode: " << int(definition) << " " << in.cols;
+	//LOG(INFO) << "NvPipe Encode: " << int(definition) << " " << in.cols;
 
-    cv::Mat tmp;
-    if (in.type() == CV_32F) {
+	cv::Mat tmp;
+	if (in.type() == CV_32F) {
 		in.convertTo(tmp, CV_16UC1, 1000);
-    } else if (in.type() == CV_8UC3) {
-        cv::cvtColor(in, tmp, cv::COLOR_BGR2BGRA);
+	} else if (in.type() == CV_8UC3) {
+		cv::cvtColor(in, tmp, cv::COLOR_BGR2BGRA);
 	} else {
-		tmp = in;
+		in.copyTo(tmp);
 	}
 
+	// scale/pad to fit output format
+	//cv::Mat tmp2 = cv::Mat::zeros(getHeight(odefinition), getWidth(odefinition), tmp.type());
+	//scaleDownAndPad(tmp, tmp2);
+	//std::swap(tmp, tmp2);
+
 	Packet pkt;
 	pkt.codec = codec_t::HEVC;
 	pkt.definition = definition;
 	pkt.block_total = 1;
 	pkt.block_number = 0;
 
-    pkt.data.resize(ftl::codecs::kVideoBufferSize);
-    uint64_t cs = NvPipe_Encode(
-        nvenc_,
-        tmp.data,
-        tmp.step,
-        pkt.data.data(),
-        ftl::codecs::kVideoBufferSize,
-        in.cols,
-        in.rows,
-        was_reset_		// Force IFrame!
-    );
-    pkt.data.resize(cs);
+	pkt.data.resize(ftl::codecs::kVideoBufferSize);
+	uint64_t cs = NvPipe_Encode(
+		nvenc_,
+		tmp.data,
+		tmp.step,
+		pkt.data.data(),
+		ftl::codecs::kVideoBufferSize,
+		tmp.cols,
+		tmp.rows,
+		was_reset_		// Force IFrame!
+	);
+	pkt.data.resize(cs);
 	was_reset_ = false;
 
-    if (cs == 0) {
-        LOG(ERROR) << "Could not encode video frame: " << NvPipe_GetError(nvenc_);
-        return false;
-    } else {
+	if (cs == 0) {
+		LOG(ERROR) << "Could not encode video frame: " << NvPipe_GetError(nvenc_);
+		return false;
+	} else {
 		cb(pkt);
-        return true;
-    }
+		return true;
+	}
 }
 
 bool NvPipeEncoder::_encoderMatch(const cv::Mat &in, definition_t def) {
-    return ((in.type() == CV_32F && is_float_channel_) ||
-        ((in.type() == CV_8UC3 || in.type() == CV_8UC4) && !is_float_channel_)) && current_definition_ == def;
+	return ((in.type() == CV_32F && is_float_channel_) ||
+		((in.type() == CV_8UC3 || in.type() == CV_8UC4) && !is_float_channel_)) && current_definition_ == def;
 }
 
 bool NvPipeEncoder::_createEncoder(const cv::Mat &in, definition_t def, bitrate_t rate) {
-    if (_encoderMatch(in, def) && nvenc_) return true;
-
-    if (in.type() == CV_32F) is_float_channel_ = true;
-    else is_float_channel_ = false;
-    current_definition_ = def;
-
-    if (nvenc_) NvPipe_Destroy(nvenc_);
-    const int fps = 1000/ftl::timer::getInterval();
-    nvenc_ = NvPipe_CreateEncoder(
-        (is_float_channel_) ? NVPIPE_UINT16 : NVPIPE_RGBA32,
-        NVPIPE_HEVC,
-        (is_float_channel_) ? NVPIPE_LOSSLESS : NVPIPE_LOSSY,
-        16*1000*1000,
-        fps,				// FPS
-        ftl::codecs::getWidth(def),	// Output Width
-        ftl::codecs::getHeight(def)	// Output Height
-    );
-
-    if (!nvenc_) {
-        LOG(ERROR) << "Could not create video encoder: " << NvPipe_GetError(NULL);
-        return false;
-    } else {
-        LOG(INFO) << "NvPipe encoder created";
-        return true;
-    }
+	if (_encoderMatch(in, def) && nvenc_) return true;
+
+	if (in.type() == CV_32F) is_float_channel_ = true;
+	else is_float_channel_ = false;
+	current_definition_ = def;
+
+	if (nvenc_) NvPipe_Destroy(nvenc_);
+	const int fps = 1000/ftl::timer::getInterval();
+	nvenc_ = NvPipe_CreateEncoder(
+		(is_float_channel_) ? NVPIPE_UINT16 : NVPIPE_RGBA32,
+		NVPIPE_HEVC,
+		(is_float_channel_) ? NVPIPE_LOSSLESS : NVPIPE_LOSSY,
+		16*1000*1000,
+		fps,				// FPS
+		ftl::codecs::getWidth(def),	// Output Width
+		ftl::codecs::getHeight(def)	// Output Height
+	);
+
+	if (!nvenc_) {
+		LOG(ERROR) << "Could not create video encoder: " << NvPipe_GetError(NULL);
+		return false;
+	} else {
+		LOG(INFO) << "NvPipe encoder created";
+		return true;
+	}
 }

components/renderers/cpp/src/splat_render.cpp

@@ -311,10 +311,10 @@ bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out) {
 	scene_->upload(Channel::Colour + Channel::Depth, stream_);
 
 	const auto &camera = src->parameters();
-
 	//cudaSafeCall(cudaSetDevice(scene_->getCUDADevice()));
 
 	// Create all the required channels
+	
 	out.create<GpuMat>(Channel::Depth, Format<float>(camera.width, camera.height));
 	out.create<GpuMat>(Channel::Colour, Format<uchar4>(camera.width, camera.height));
 
@@ -338,7 +338,6 @@ bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out) {
 	params.m_viewMatrix = MatrixConversion::toCUDA(src->getPose().cast<float>().inverse());
 	params.m_viewMatrixInverse = MatrixConversion::toCUDA(src->getPose().cast<float>());
 	params.camera = camera;
-
 	// Clear all channels to 0 or max depth
 
 	out.get<GpuMat>(Channel::Depth).setTo(cv::Scalar(1000.0f), cvstream);
@@ -384,7 +383,7 @@ bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out) {
 		}
 
 		if (!f.hasChannel(Channel::Normals)) {
-			Eigen::Matrix4f matrix =  s->getPose().cast<float>();
+			Eigen::Matrix4f matrix =  s->getPose().cast<float>().transpose();
 			auto pose = MatrixConversion::toCUDA(matrix);
 
 			auto &g = f.get<GpuMat>(Channel::Colour);
@@ -458,8 +457,14 @@ bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out) {
 	}
 	else if (chan == Channel::Right)
 	{
-		Eigen::Affine3f transform(Eigen::Translation3f(camera.baseline,0.0f,0.0f));
-		Eigen::Matrix4f matrix =  src->getPose().cast<float>() * transform.matrix();
+		float baseline = camera.baseline;
+		
+		Eigen::Translation3f translation(baseline, 0.0f, 0.0f);
+		LOG(INFO) << translation.vector();
+		Eigen::Affine3f transform(translation);
+		LOG(INFO) << "\n" << transform.matrix();
+		LOG(INFO) << "baseline " << baseline;
+		Eigen::Matrix4f matrix = transform.matrix() * src->getPose().cast<float>();
 		params.m_viewMatrix = MatrixConversion::toCUDA(matrix.inverse());
 		params.m_viewMatrixInverse = MatrixConversion::toCUDA(matrix);
 		
@@ -468,6 +473,18 @@ bool Splatter::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out) {
 
 		_dibr(stream_); // Need to re-dibr due to pose change
 		_renderChannel(out, Channel::Left, Channel::Right, stream_);
+		
+		// renderFrame() expects to render right frame from left as well; Should
+		// possibly add channel_in and channel_out parameters to renderFrame()?
+		// (l/r swap as temporary fix)
+		/*
+		auto &tmp = out.get<GpuMat>(Channel::Left);
+		swap(out.get<GpuMat>(Channel::Right), tmp);
+		_renderChannel(params, out, Channel::Left, stream_);
+		swap(tmp, out.get<GpuMat>(Channel::Right));
+		*/
+
+		_renderChannel(out, Channel::Left, Channel::Right, stream_);
 	} else if (chan != Channel::None) {
 		if (ftl::codecs::isFloatChannel(chan)) {
 			out.create<GpuMat>(chan, Format<float>(camera.width, camera.height));

components/rgbd-sources/src/sources/virtual/virtual.cpp

@@ -10,6 +10,15 @@ class VirtualImpl : public ftl::rgbd::detail::Source {
 		params_ = params;
 		capabilities_ = ftl::rgbd::kCapVideo | ftl::rgbd::kCapStereo;
 		if (!host->value("locked", false)) capabilities_ |= ftl::rgbd::kCapMovable;
+		host->on("baseline", [this](const ftl::config::Event&) {
+			params_.baseline = host_->value("baseline", 0.0f);
+		});
+		
+		host->on("focal", [this](const ftl::config::Event&) {
+			params_.fx = host_->value("focal", 700.0f);
+			params_.fy = params_.fx;
+		});
+
 	}
 
 	~VirtualImpl() {