diff --git a/applications/vision/src/main.cpp b/applications/vision/src/main.cpp
index 1a07dbdfcad41e329c1425fc7a6730ae90700f43..14be06f2c5f21f065a0e63a7d3b553b2a46dd289 100644
--- a/applications/vision/src/main.cpp
+++ b/applications/vision/src/main.cpp
@@ -58,7 +58,7 @@ static void run(const string &file) {
 	Universe net(config["net"]);
 	LOG(INFO) << "Net started.";
 
-	RGBDSource *source = nullptr;
+	StereoVideoSource *source = nullptr;
 	source = new StereoVideoSource(config, file);
 	
 	// Allow remote users to access camera calibration matrix
@@ -143,6 +143,7 @@ static void run(const string &file) {
 
 		// Send RGB+Depth images for local rendering
 		if (prgb.rows > 0) display.render(prgb, pdepth, source->getParameters());
+		if (config["display"]["right"]) cv::imshow("Right: ", source->getRight());
 		display.wait(1);
 
 		// Wait for both pipelines to complete
diff --git a/components/rgbd-sources/include/ftl/stereovideo_source.hpp b/components/rgbd-sources/include/ftl/stereovideo_source.hpp
index d59908722779bf7d5c513c8f870db44dcfb60ed0..998c3532e2c75bb7694c8bf2447cdaed971c476a 100644
--- a/components/rgbd-sources/include/ftl/stereovideo_source.hpp
+++ b/components/rgbd-sources/include/ftl/stereovideo_source.hpp
@@ -29,6 +29,8 @@ class StereoVideoSource : public RGBDSource {
 	void getRGBD(cv::Mat &rgb, cv::Mat &depth);
 	bool isReady();
 
+	const cv::Mat &getRight() const { return right_; }
+
 	static inline RGBDSource *create(nlohmann::json &config, ftl::net::Universe *net) {
 		return new StereoVideoSource(config, net);
 	}
diff --git a/components/rgbd-sources/src/calibrate.cpp b/components/rgbd-sources/src/calibrate.cpp
index ea6cd7d276c9d43bf39fd0b360921d3fb0ff83dc..f0b55daa1efa483d9b168119fc06c9c3268cc87b 100644
--- a/components/rgbd-sources/src/calibrate.cpp
+++ b/components/rgbd-sources/src/calibrate.cpp
@@ -274,9 +274,15 @@ bool Calibrate::_loadCalibration() {
     		map1_[1], map2_[1]);
 
 	// Re-distort
-	initUndistortRectifyMap(M1, Mat(), R1.t(), P1,
+	Mat P1_cam = (cv::Mat_<double>(3,3) << P1.at<double>(0, 0), P1.at<double>(0, 1) , P1.at<double>(0, 2),
+		P1.at<double>(1, 0), P1.at<double>(1, 1), P1.at<double>(1, 2),
+		P1.at<double>(2, 0), P1.at<double>(2, 1), P1.at<double>(2, 2));
+	Mat M1_trans = (cv::Mat_<double>(3, 4) << M1.at<double>(0, 0), M1.at<double>(0, 1), M1.at<double>(0, 2), -P1.at<double>(0, 3),
+		M1.at<double>(1, 0), M1.at<double>(1, 1), M1.at<double>(1, 2), -P1.at<double>(1, 3),
+		M1.at<double>(2, 0), M1.at<double>(2, 1), M1.at<double>(2, 2), -P1.at<double>(2, 3));
+	initUndistortRectifyMap(P1_cam, Mat(), R1.t(), P1,
 			img_size, CV_16SC2, imap1_, imap2_);
-	r1_ = R1.t();
+	r1_ = P1;
     return true;
 }
 
diff --git a/components/rgbd-sources/src/calibrate.hpp b/components/rgbd-sources/src/calibrate.hpp
index 78e8fde6af0dc28d327f0034e5eb915f1bdad0fc..2e12f0e0efca21577ad543db8a286d7bc13f9b4d 100644
--- a/components/rgbd-sources/src/calibrate.hpp
+++ b/components/rgbd-sources/src/calibrate.hpp
@@ -108,6 +108,7 @@ class Calibrate {
 	 * a 3D point cloud.
 	 */
 	const cv::Mat &getQ() const { return Q_; }
+	const cv::Mat &getCameraMatrix() const { return r1_; }
 
 	private:
 	bool _recalibrate(std::vector<std::vector<cv::Point2f>> *imagePoints,
diff --git a/components/rgbd-sources/src/stereovideo_source.cpp b/components/rgbd-sources/src/stereovideo_source.cpp
index 457b7727c35bb98ec8655c18a8fa2185bf9c5259..a904cfc4eab86e0dd7805d1cd9f75d46be85bdae 100644
--- a/components/rgbd-sources/src/stereovideo_source.cpp
+++ b/components/rgbd-sources/src/stereovideo_source.cpp
@@ -46,11 +46,12 @@ StereoVideoSource::StereoVideoSource(nlohmann::json &config, const string &file)
 	else LOG(INFO) << "Calibration initiated.";
 
 	// Generate camera parameters from Q matrix
-	cv::Mat q = calib_->getQ();
+	cv::Mat q = calib_->getCameraMatrix();
 	params_ = {
-		q.at<double>(2,3),	// F
-		q.at<double>(0,3),	// Cx
-		q.at<double>(1,3),	// Cy
+		// TODO(Nick) Add fx and fy
+		q.at<double>(0,0),	// F
+		q.at<double>(0,2),	// Cx
+		q.at<double>(1,2),	// Cy
 		(unsigned int)left_.cols,  // TODO (Nick)
 		(unsigned int)left_.rows,
 		0.0f,	// 0m min
@@ -106,5 +107,5 @@ void StereoVideoSource::getRGBD(cv::Mat &rgb, cv::Mat &depth) {
 	disp_->compute(left_, right_, disp);
 	rgb = left_;
 	disparityToDepth(disp, depth, calib_->getQ());
-	calib_->distort(rgb,depth);
+	//calib_->distort(rgb,depth);
 }