diff --git a/applications/calibration-multi/src/main.cpp b/applications/calibration-multi/src/main.cpp
index 46e2122b2f3a5d2acd8515f98e5e8eee175dfe6c..ddd41ceb1b22f87e4aee84c19e52e9d5b0c8faf0 100644
--- a/applications/calibration-multi/src/main.cpp
+++ b/applications/calibration-multi/src/main.cpp
@@ -116,7 +116,7 @@ void visualizeCalibration(	MultiCameraCalibrationNew &calib, Mat &out,
 		Scalar(64, 255, 64),
 		Scalar(64, 255, 64),
 	};
-	
+
 	vector<int> markers = {cv::MARKER_SQUARE, cv::MARKER_DIAMOND};
 
 	for (size_t c = 0; c < rgb.size(); c++) {
@@ -211,7 +211,7 @@ void calibrateRPC(	ftl::net::Universe* net,
 		Mat P1, P2, Q;
 		Mat R1, R2;
 		Mat R_c1c2, T_c1c2;
-		
+
 		calculateTransform(R[c], t[c], R[c + 1], t[c + 1], R_c1c2, T_c1c2);
 		cv::stereoRectify(K1, D1, K2, D2, params.size, R_c1c2, T_c1c2, R1, R2, P1, P2, Q, 0, params.alpha);
 
@@ -275,12 +275,12 @@ std::vector<cv::Point2d> findCalibrationTarget(const cv::Mat &im, const cv::Mat
 
 	if (corners.size() > 2) { LOG(ERROR) << "Too many ArUco tags in image"; }
 	if (corners.size() != 2) { return {}; }
-	
+
 	const size_t ncorners = 4;
 	const size_t ntags = ids.size();
 
 	std::vector<cv::Point2d> points;
-	
+
 	if (ids[0] == 1) {
 		std::swap(ids[0], ids[1]);
 		std::swap(corners[0], corners[1]);
@@ -314,7 +314,7 @@ void runCameraCalibration(	ftl::Configurable* root,
 
 	net->start();
 	net->waitConnections();
-	
+
 	ftl::stream::Muxer *stream = ftl::create<ftl::stream::Muxer>(root, "muxstream");
 	ftl::stream::Receiver *gen = ftl::create<ftl::stream::Receiver>(root, "receiver");
 	gen->setStream(stream);
@@ -330,10 +330,10 @@ void runCameraCalibration(	ftl::Configurable* root,
 		nstream->set("uri", s);
 		nstreams.push_back(nstream);
 		stream->add(nstream);
-		
+
 		++count;
 	}
-	
+
 	const size_t n_sources = nstreams.size();
 	const size_t n_cameras = n_sources * 2;
 	size_t reference_camera = 0;
@@ -349,7 +349,7 @@ void runCameraCalibration(	ftl::Configurable* root,
 		if (fs.frames.size() != (rgb_new.size()/2)) {
 			// nstreams.size() == (rgb_new.size()/2)
 			LOG(ERROR)	<< "frames.size() != nstreams.size(), "
-						<< fs.frames.size() << " != " << (rgb_new.size()/2); 
+						<< fs.frames.size() << " != " << (rgb_new.size()/2);
 		}
 
 		UNIQUE_LOCK(mutex, CALLBACK);
@@ -370,28 +370,28 @@ void runCameraCalibration(	ftl::Configurable* root,
 
 				auto idx = stream->originStream(0, i);
 				CHECK(idx >= 0) << "negative index";
-				
+
 				fs.frames[i].download(Channel::Left+Channel::Right);
 				Mat &left = fs.frames[i].get<Mat>(Channel::Left);
 				Mat &right = fs.frames[i].get<Mat>(Channel::Right);
-				
+
 				/*
-				// note: also returns empty sometimes 
+				// note: also returns empty sometimes
 				fs.frames[i].upload(Channel::Left+Channel::Right);
 				Mat left, right;
 				fs.frames[i].get<cv::cuda::GpuMat>(Channel::Left).download(left);
 				fs.frames[i].get<cv::cuda::GpuMat>(Channel::Right).download(right);
 				*/
-				
+
 				CHECK(!left.empty() && !right.empty());
 
 				cv::cvtColor(left, rgb_new[2*idx], cv::COLOR_BGRA2BGR);
 				cv::cvtColor(right, rgb_new[2*idx+1], cv::COLOR_BGRA2BGR);
-				
+
 				camera_parameters[2*idx] = ftl::calibration::scaleCameraMatrix(createCameraMatrix(fs.frames[i].getLeftCamera()),
-					rgb_new[2*idx].size(), Size(fs.frames[i].getLeftCamera().width, fs.frames[i].getLeftCamera().height));
+					Size(fs.frames[i].getLeftCamera().width, fs.frames[i].getLeftCamera().height), rgb_new[2*idx].size());
 				camera_parameters[2*idx+1] = ftl::calibration::scaleCameraMatrix(createCameraMatrix(fs.frames[i].getRightCamera()),
-					rgb_new[2*idx].size(), Size(fs.frames[i].getRightCamera().width, fs.frames[i].getRightCamera().height));
+					Size(fs.frames[i].getRightCamera().width, fs.frames[i].getRightCamera().height), rgb_new[2*idx].size());
 
 				if (res.empty()) res = rgb_new[2*idx].size();
 			}
@@ -410,7 +410,7 @@ void runCameraCalibration(	ftl::Configurable* root,
 
 	stream->begin();
 	ftl::timer::start(false);
-	
+
 	while(true) {
 		if (!res.empty()) {
 			params.size = res;
@@ -455,7 +455,7 @@ void runCameraCalibration(	ftl::Configurable* root,
 	while(calib.getMinVisibility() < static_cast<size_t>(n_views)) {
 		loop:
 		cv::waitKey(10);
-		
+
 		while (true) {
 			if (new_frames) {
 				UNIQUE_LOCK(mutex, LOCK);
@@ -465,7 +465,7 @@ void runCameraCalibration(	ftl::Configurable* root,
 			}
 			cv::waitKey(10);
 		}
-		
+
 		for (Mat &im : rgb) {
 			if (im.empty()) {
 				LOG(ERROR) << "EMPTY";
@@ -488,10 +488,10 @@ void runCameraCalibration(	ftl::Configurable* root,
 				n_found++;
 			}
 		}
-		
+
 		if (n_found >= min_visible) {
 			calib.addPoints(points, visible);
-			
+
 			if (save_input) {
 				for (size_t i = 0; i < n_cameras; i++) {
 					cv::imwrite(path + std::to_string(i) + "_" + std::to_string(iter) + ".jpg", rgb[i]);
@@ -508,13 +508,13 @@ void runCameraCalibration(	ftl::Configurable* root,
 						cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, Scalar(64, 64, 255), 1);
 				}
 			}
-			
+
 			// index
 			cv::putText(rgb[i],
 						"Camera " + std::to_string(i),
 						Point2i(10, 30),
 						cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, Scalar(64, 64, 255), 1);
-			
+
 			// resolution
 			cv::putText(rgb[i],
 						"[" + std::to_string(rgb[i].size().width) + "x" + std::to_string(rgb[i].size().height) + "]",
@@ -542,7 +542,7 @@ void runCameraCalibration(	ftl::Configurable* root,
 		cv::imshow("Cameras", show);
 	}
 	cv::destroyWindow("Cameras");
-	
+
 	for (size_t i = 0; i < nstreams.size(); i++) {
 		while(true) {
 			try {
@@ -558,7 +558,7 @@ void runCameraCalibration(	ftl::Configurable* root,
 			catch (...) {}
 		}
 	}
-	
+
 	Mat out;
 	vector<Mat> map1, map2;
 	vector<cv::Rect> roi;
@@ -614,7 +614,7 @@ void runCameraCalibrationPath(	ftl::Configurable* root,
 
 	net->start();
 	net->waitConnections();
-	
+
 	ftl::stream::Muxer *stream = ftl::create<ftl::stream::Muxer>(root, "muxstream");
 	ftl::stream::Receiver *gen = ftl::create<ftl::stream::Receiver>(root, "receiver");
 	gen->setStream(stream);
@@ -630,10 +630,10 @@ void runCameraCalibrationPath(	ftl::Configurable* root,
 		nstream->set("uri", s);
 		nstreams.push_back(nstream);
 		stream->add(nstream);
-		
+
 		++count;
 	}
-	
+
 	const size_t n_sources = nstreams.size();
 	const size_t n_cameras = n_sources * 2;
 	size_t reference_camera = 0;
@@ -649,7 +649,7 @@ void runCameraCalibrationPath(	ftl::Configurable* root,
 		if (fs.frames.size() != (rgb_new.size()/2)) {
 			// nstreams.size() == (rgb_new.size()/2)
 			LOG(ERROR)	<< "frames.size() != nstreams.size(), "
-						<< fs.frames.size() << " != " << (rgb_new.size()/2); 
+						<< fs.frames.size() << " != " << (rgb_new.size()/2);
 		}
 
 		UNIQUE_LOCK(mutex, CALLBACK);
@@ -670,11 +670,11 @@ void runCameraCalibrationPath(	ftl::Configurable* root,
 
 				auto idx = stream->originStream(0, i);
 				CHECK(idx >= 0) << "negative index";
-				
+
 				fs.frames[i].download(Channel::Left+Channel::Right);
 				Mat &left = fs.frames[i].get<Mat>(Channel::Left);
 				Mat &right = fs.frames[i].get<Mat>(Channel::Right);
-				
+
 				CHECK(!left.empty() && !right.empty());
 
 				cv::cvtColor(left, rgb_new[2*idx], cv::COLOR_BGRA2BGR);
@@ -684,7 +684,7 @@ void runCameraCalibrationPath(	ftl::Configurable* root,
 					rgb_new[2*idx].size(), Size(fs.frames[i].getLeftCamera().width, fs.frames[i].getLeftCamera().height));
 				camera_parameters[2*idx+1] = ftl::calibration::scaleCameraMatrix(createCameraMatrix(fs.frames[i].getRightCamera()),
 					rgb_new[2*idx].size(), Size(fs.frames[i].getRightCamera().width, fs.frames[i].getRightCamera().height));
-				
+
 				if (res.empty()) res = rgb_new[2*idx].size();*/
 			}
 		}
@@ -726,7 +726,7 @@ void runCameraCalibrationPath(	ftl::Configurable* root,
 	calib.object_points_ = calibration_target;
 
 	cv::FileStorage fs(path + filename, cv::FileStorage::READ);
-	fs["resolution"] >> params.size; 
+	fs["resolution"] >> params.size;
 	params.idx_cameras.resize(n_cameras);
 	std::iota(params.idx_cameras.begin(), params.idx_cameras.end(), 0);
 	calib.loadInput(path + filename, params.idx_cameras);
@@ -798,7 +798,7 @@ void runCameraCalibrationPath(	ftl::Configurable* root,
 int main(int argc, char **argv) {
 	auto options = ftl::config::read_options(&argv, &argc);
 	auto root = ftl::configure(argc, argv, "registration_default");
-	
+
 	// run calibration from saved input?
 	const bool load_input = root->value<bool>("load_input", false);
 	// should calibration input be saved
@@ -822,7 +822,7 @@ int main(int argc, char **argv) {
 	const string registration_file = root->value<string>("registration_file", FTL_LOCAL_CONFIG_ROOT "/registration.json");
 	// location where extrinsic calibration files saved
 	const string output_directory = root->value<string>("output_directory", "./");
-	
+
 	const bool offline = root->value<bool>("offline", false);
 
 	CalibrationParams params;
@@ -833,7 +833,7 @@ int main(int argc, char **argv) {
 	params.output_path = output_directory;
 	params.registration_file = registration_file;
 	params.reference_camera = ref_camera;
-	
+
 	LOG(INFO)	<< "\n"
 				<< "\n"
 				<< "\n                save_input: " << (int) save_input
@@ -858,4 +858,4 @@ int main(int argc, char **argv) {
 	}
 
 	return 0;
-}
\ No newline at end of file
+}
diff --git a/applications/calibration/src/lens.cpp b/applications/calibration/src/lens.cpp
index 72694ebacda05370a4a32a214e400c72a6d94d0e..ef00ba17779fe3a61327e67b5b3da5e8a4ff0f5e 100644
--- a/applications/calibration/src/lens.cpp
+++ b/applications/calibration/src/lens.cpp
@@ -62,7 +62,7 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 
 	// assume no tangential and thin prism distortion and only estimate first
 	// three radial distortion coefficients
-	
+
 	int calibrate_flags = 	cv::CALIB_FIX_K4 | cv::CALIB_FIX_K5 | cv::CALIB_FIX_K6 |
 							cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_S1_S2_S3_S4 | cv::CALIB_FIX_ASPECT_RATIO;
 
@@ -76,13 +76,13 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 		camera_matrix.clear();
 		vector<Mat> tmp;
 		Size tmp_size;
-		
+
 		loadIntrinsics(filename_intrinsics, camera_matrix, tmp, tmp_size);
 		CHECK(camera_matrix.size() == static_cast<unsigned int>(n_cameras));
 
 		if ((tmp_size != image_size) && (!tmp_size.empty())) {
 			for (Mat &K : camera_matrix) {
-				K = ftl::calibration::scaleCameraMatrix(K, image_size, tmp_size);
+				K = ftl::calibration::scaleCameraMatrix(K, tmp_size, image_size);
 			}
 		}
 
@@ -104,13 +104,13 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 			LOG(ERROR) << "Could not open camera device";
 			return;
 		}
-		camera.set(cv::CAP_PROP_FRAME_WIDTH, image_size.width); 
+		camera.set(cv::CAP_PROP_FRAME_WIDTH, image_size.width);
 		camera.set(cv::CAP_PROP_FRAME_HEIGHT, image_size.height);
 	}
 
 	vector<vector<vector<Vec2f>>> image_points(n_cameras);
 	vector<vector<vector<Vec3f>>> object_points(n_cameras);
-	
+
 	vector<Mat> img(n_cameras);
 	vector<Mat> img_display(n_cameras);
 	vector<int> count(n_cameras, 0);
@@ -124,7 +124,7 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 	std::atomic<bool> ready = false;
 	auto capture = std::thread(
 		[n_cameras, delay, &m, &ready, &count, &calib, &img, &image_points, &object_points]() {
-		
+
 		vector<Mat> tmp(n_cameras);
 		while(true) {
 			if (!ready) {
@@ -138,7 +138,7 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 				img[c].copyTo(tmp[c]);
 			}
 			m.unlock();
-			
+
 			for (int c = 0; c < n_cameras; c++) {
 				vector<Vec2f> points;
 				if (calib.findPoints(tmp[c], points)) {
@@ -168,13 +168,13 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 			ready = true;
 			m.unlock();
 		}
-		
+
 		for (int c = 0; c < n_cameras; c++) {
 			img[c].copyTo(img_display[c]);
 			m.lock();
 
 			if (image_points[c].size() > 0) {
-				
+
 				for (auto &points : image_points[c]) {
 					calib.drawCorners(img_display[c], points);
 				}
@@ -192,13 +192,13 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 	}
 
 	cv::destroyAllWindows();
-	
+
 	//bool calib_ok = true;
 
 	for (int c = 0; c < n_cameras; c++) {
 		LOG(INFO) << "Calculating intrinsic paramters for camera " << std::to_string(c);
 		vector<Mat> rvecs, tvecs;
-		
+
 		double rms = cv::calibrateCamera(
 							object_points[c], image_points[c],
 							image_size, camera_matrix[c], dist_coeffs[c],
@@ -208,9 +208,9 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 		LOG(INFO) << "final reprojection RMS error: " << rms;
 
 		if (!ftl::calibration::validate::distortionCoefficients(dist_coeffs[c], image_size)) {
-			LOG(ERROR)	<< "Calibration failed: invalid distortion coefficients:\n" 
+			LOG(ERROR)	<< "Calibration failed: invalid distortion coefficients:\n"
 						<< dist_coeffs[c];
-			
+
 			LOG(WARNING) << "Estimating only intrinsic parameters for camera " << std::to_string(c);
 
 			dist_coeffs[c] = Mat(Size(8, 1), CV_64FC1, cv::Scalar(0.0));
@@ -228,7 +228,7 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 		// TODO: check for valid aperture width/height; do not run if not valid values
 		cv::calibrationMatrixValues(camera_matrix[c], image_size, aperture_width, aperture_height,
 									fovx, fovy, focal_length, principal_point, aspect_ratio);
-		
+
 		LOG(INFO) << "";
 		LOG(INFO) << "            fovx (deg): " << fovx;
 		LOG(INFO) << "            fovy (deg): " << fovy;
@@ -243,13 +243,13 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 
 	saveIntrinsics(filename_intrinsics, camera_matrix, dist_coeffs, image_size);
 	LOG(INFO) << "intrinsic paramaters saved to: " << filename_intrinsics;
-	
+
 	vector<Mat> map1(n_cameras), map2(n_cameras);
 	for (int c = 0; c < n_cameras; c++) {
 		cv::initUndistortRectifyMap(camera_matrix[c], dist_coeffs[c], Mat::eye(3,3, CV_64F), camera_matrix[c],
 									image_size, CV_16SC2, map1[c], map2[c]);
 	}
-	
+
 	while (cv::waitKey(25) != 27) {
 		for (auto &camera : cameras ) {	camera.grab(); }
 		for (int c = 0; c < n_cameras; c++) {
diff --git a/applications/gui2/src/modules/calibration.cpp b/applications/gui2/src/modules/calibration.cpp
index 2833a9ca261283712f2df51016ff904e0e64d67a..3cfe30fb57ff016f6efbed6f577637db79b44951 100644
--- a/applications/gui2/src/modules/calibration.cpp
+++ b/applications/gui2/src/modules/calibration.cpp
@@ -217,16 +217,16 @@ void IntrinsicCalibration::onFrame(const ftl::data::FrameSetPtr& fs) {
 		if (calibration_enabled_ || calib_data.enabled) {
 			LOG(INFO) << "DISABLING";
 			enableCalibration(frame, false);
-			auto& intrinsics = (channel_ == Channel::Left) ?
-				calib_data.intrinsic_left : calib_data.intrinsic_right;
-
-			cv::Mat K = intrinsics.matrix();
-			cv::Mat D = cv::Mat(intrinsics.distCoeffs);
-
-			LOG(INFO) << K;
-			LOG(INFO) << D;
-			cv::Size resolution = intrinsics.resolution;
-			calib_.setCalibration(K, D, resolution);
+			for (auto& item : calib_data.data) {
+				if (item.channel != channel_) continue;
+
+				auto& intrinsics = item.intrinsic;
+				cv::Mat K = intrinsics.matrix();
+				cv::Mat D = cv::Mat(intrinsics.distCoeffs);
+				cv::Size resolution = intrinsics.resolution;
+				calib_.setCalibration(K, D, resolution);
+				break;
+			}
 			return;
 		}
 	}
@@ -312,14 +312,22 @@ void IntrinsicCalibration::run() {
 
 bool IntrinsicCalibration::save() {
 	auto& frame = current_fs_->frames[id_.source()];
-	CalibrationData params = CalibrationData(frame.get<CalibrationData>(Channel::CalibrationData));
-	if (channel_ == Channel::Left) {
-		params.intrinsic_left = calib_.calibration();
-	}
-	else {
-		params.intrinsic_right = calib_.calibration();
+	CalibrationData calib_data = CalibrationData(frame.get<CalibrationData>(Channel::CalibrationData));
+
+	for (auto& calibration : calib_data.data) {
+		if (calibration.channel != channel_) continue;
+
+		// update existing values and return
+		calibration.intrinsic = calib_.calibration();
+		return update(frame, calib_data);
 	}
-	return update(frame, params);
+
+	// not found: add to calibration structure
+	CalibrationData::Calibration calibration;
+	calibration.channel = channel_;
+	calibration.intrinsic = calib_.calibration();
+	calib_data.data.push_back(calibration);
+	return update(frame, calib_data);
 }
 
 
@@ -348,3 +356,10 @@ float IntrinsicCalibration::sensorWidth() {
 float IntrinsicCalibration::sensorHeight() {
 	return value("sensor_height", 0.0f);
 };
+
+bool IntrinsicCalibration::hasChannel(Channel c) {
+	if (current_fs_) {
+		return (*current_fs_)[id_.source()].hasChannel(c);
+	}
+	return false;
+}
diff --git a/applications/gui2/src/modules/calibration.hpp b/applications/gui2/src/modules/calibration.hpp
index f6c3bddb5fe3f27b09a83cce823701bbbb40147d..086409910c69424ad7582df232af54e2ff2b2d32 100644
--- a/applications/gui2/src/modules/calibration.hpp
+++ b/applications/gui2/src/modules/calibration.hpp
@@ -28,6 +28,7 @@ public:
 
 	void onFrame(const ftl::data::FrameSetPtr& fs);
 
+	bool hasChannel(ftl::codecs::Channel c);
 	/** select channel */
 	void setChannel(ftl::codecs::Channel c) { channel_ = c; }
 	ftl::codecs::Channel channel() { return channel_; }
diff --git a/applications/gui2/src/modules/camera.cpp b/applications/gui2/src/modules/camera.cpp
index 9797ea6f60a1c2f2c13eb4f31a34e8bf5280a4f9..352d2c67b6013a7970fd2a7d9bd3489c33ac7ea7 100644
--- a/applications/gui2/src/modules/camera.cpp
+++ b/applications/gui2/src/modules/camera.cpp
@@ -18,7 +18,7 @@ void Camera::init() {
 		(ftl::create<ftl::overlay::Overlay>(this, "overlay"));
 }
 
-void Camera::_initiate(ftl::data::Frame &frame) {
+void Camera::initiate_(ftl::data::Frame &frame) {
 	if (frame.has(Channel::Capabilities)) {
 		const auto &cap = frame.get<std::unordered_set<Capability>>(Channel::Capabilities);
 		LOG(INFO) << "Camera Capabilities:";
@@ -106,7 +106,7 @@ void Camera::activate(ftl::data::FrameID id) {
 	// Must be done in GUI thread, hence use of cv.
 	std::unique_lock<std::mutex> lk(m);
 	cv.wait_for(lk, 1s, [this](){ return has_seen_frame_; });
-	_initiate(std::atomic_load(&current_fs_)->frames[frame_idx]);
+	initiate_(std::atomic_load(&current_fs_)->frames[frame_idx]);
 }
 
 void Camera::setChannel(Channel c) {
diff --git a/applications/gui2/src/modules/camera.hpp b/applications/gui2/src/modules/camera.hpp
index e73fc4e3b57670e47ff181a3d539999cf50f5c91..3ecdf949b299f498adf74a8fcbd860b97285ac23 100644
--- a/applications/gui2/src/modules/camera.hpp
+++ b/applications/gui2/src/modules/camera.hpp
@@ -49,7 +49,7 @@ private:
 	CameraView* view = nullptr;
 	MediaPanel* panel = nullptr;
 
-	void _initiate(ftl::data::Frame &frame);
+	void initiate_(ftl::data::Frame &frame);
 };
 
 }
diff --git a/applications/gui2/src/views/calibration/intrinsic.cpp b/applications/gui2/src/views/calibration/intrinsic.cpp
index b59f7556b00d2121de23c49de27437c9723f8692..4eccb6b4fbff14c9a10a8944e698db21c763d10e 100644
--- a/applications/gui2/src/views/calibration/intrinsic.cpp
+++ b/applications/gui2/src/views/calibration/intrinsic.cpp
@@ -62,6 +62,7 @@ private:
 	IntrinsicCalibration* ctrl_;
 
 	nanogui::Label* txtnframes_;
+	nanogui::Button* bcalibrate_;
 	nanogui::Button* bresults_;
 	nanogui::Button* bpause_;
 
@@ -146,6 +147,7 @@ CaptureWindow::CaptureWindow(nanogui::Widget* parent, IntrinsicCalibrationView*
 		ctrl->reset();
 		ctrl->setChannel(Channel::Right);
 	});
+	button_right->setEnabled(ctrl_->hasChannel(Channel::Right));
 
 	new nanogui::Label(this, "Capture interval");
 	auto* interval = new nanogui::FloatBox<float>(this, ctrl_->frequency());
@@ -238,11 +240,10 @@ ControlWindow::ControlWindow(nanogui::Widget* parent, IntrinsicCalibrationView*
 		ctrl->setCapture(!ctrl->capturing());
 	});
 
-	auto* button_continue = new nanogui::Button(buttons, "Calibrate");
-	button_continue->setFixedWidth(140);
-	button_continue->setCallback([view = view_](){
+	bcalibrate_ = new nanogui::Button(buttons, "Calibrate");
+	bcalibrate_->setFixedWidth(140);
+	bcalibrate_->setCallback([view = view_](){
 
-		// TODO: implement as setMode() in view
 		view->setMode(Mode::CALIBRATION);
 	});
 }
@@ -250,6 +251,7 @@ ControlWindow::ControlWindow(nanogui::Widget* parent, IntrinsicCalibrationView*
 void ControlWindow::draw(NVGcontext* ctx) {
 	if (ctrl_->capturing())	{ bpause_->setCaption("Pause"); }
 	else 					{ bpause_->setCaption("Continue"); }
+	bcalibrate_->setEnabled(ctrl_->calib().nFrames() > 0);
 	bresults_->setEnabled(ctrl_->calib().calibrated());
 	updateCount();
 	nanogui::Window::draw(ctx);
diff --git a/applications/gui2/src/views/camera.cpp b/applications/gui2/src/views/camera.cpp
index 2308b3b2fe41f2fe6bce2c6f7d3cf1e2f04621ef..806ccc33dbc2620f3c7f5d4997b7096ac9943e9d 100644
--- a/applications/gui2/src/views/camera.cpp
+++ b/applications/gui2/src/views/camera.cpp
@@ -127,6 +127,7 @@ CameraView::CameraView(ftl::gui2::Screen* parent, ftl::gui2::Camera* ctrl) :
 	//LOG(INFO) << __func__ << " (" << this << ")";
 	imview_ = new ftl::gui2::ImageView(this);
 	imview_->setTheme(parent->getTheme("window"));
+	imview_->setSize(parent->viewSize());
 }
 
 CameraView::~CameraView() {
diff --git a/applications/gui2/src/views/thumbnails.cpp b/applications/gui2/src/views/thumbnails.cpp
index e0ec065b15953217110c29dcce05327fcb6e00b6..62867bd3b8a1bb78dfc249673c2eaf970f596590 100644
--- a/applications/gui2/src/views/thumbnails.cpp
+++ b/applications/gui2/src/views/thumbnails.cpp
@@ -76,19 +76,13 @@ void ThumbView::update(ftl::rgbd::Frame &frame, Channel c) {
 
 void ThumbView::draw(NVGcontext *ctx) {
 	fit();
-	center();
-	// Background color
-	/*nvgBeginPath(ctx);
-	nvgRect(ctx, absolutePosition()[0], absolutePosition()[1], width(), height());
-	nvgFillColor(ctx, {0.2f, 0.2f, 0.2f, 1.0f});
-	nvgFill(ctx);*/
 	// Image
 	ftl::gui2::ImageView::draw(ctx);
 	// Label
 	nvgScissor(ctx, mPos.x(), mPos.y(), mSize.x(), mSize.y());
 	nvgFontSize(ctx, 14);
 	nvgFontFace(ctx, "sans-bold");
-	nvgText(ctx, mPos.x() + 10, mPos.y()+mSize.y() - 10, name_.c_str(), NULL);
+	nvgText(ctx, mPos.x() + 10, mPos.y()+mSize.y() - 18, name_.c_str(), NULL);
 	nvgResetScissor(ctx);
 }
 
diff --git a/components/calibration/CMakeLists.txt b/components/calibration/CMakeLists.txt
index e0153a9f0fd3d52dc7404aba2301bd60a0cbc26a..5f2f4f1028a71e145e006136983cf651bea49f3d 100644
--- a/components/calibration/CMakeLists.txt
+++ b/components/calibration/CMakeLists.txt
@@ -11,7 +11,7 @@ endif()
 add_library(ftlcalibration ${CALIBSRC})
 
 target_include_directories(ftlcalibration
-	PUBLIC 
+	PUBLIC
 	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
 	$<INSTALL_INTERFACE:include>
 	PRIVATE
@@ -19,7 +19,8 @@ target_include_directories(ftlcalibration
 	${OpenCV_INCLUDE_DIRS}
 )
 
-target_link_libraries(ftlcalibration ftlcommon Threads::Threads ${OpenCV_LIBS} Eigen3::Eigen ceres)
+# ftlcodecs required for ftl::data::Channel
+target_link_libraries(ftlcalibration ftlcommon ftlcodecs Threads::Threads ${OpenCV_LIBS} Eigen3::Eigen ceres)
 
 if (BUILD_TESTS)
 	ADD_SUBDIRECTORY(test)
diff --git a/components/calibration/include/ftl/calibration/parameters.hpp b/components/calibration/include/ftl/calibration/parameters.hpp
index f261506570921b8c243855e6ace9ea6b2fcfdd95..f0fd0cf291f56f30dbe4c2c332cb8cac742adefc 100644
--- a/components/calibration/include/ftl/calibration/parameters.hpp
+++ b/components/calibration/include/ftl/calibration/parameters.hpp
@@ -80,11 +80,11 @@ bool cameraMatrix(const cv::Mat &M);
  * @param D    distortion coefficients
  * @param size resolution
  * @note Tangential and prism distortion coefficients are not validated.
- * 
+ *
  * Radial distortion is always monotonic for real lenses and distortion
  * function has to be bijective. This is verified by evaluating the distortion
  * function for integer values from 0 to sqrt(width^2+height^2).
- * 
+ *
  * Camera model documented in
  * https://docs.opencv.org/master/d9/d0c/group__calib3d.html#details
  */
@@ -126,19 +126,20 @@ inline void inverse(cv::Mat &R, cv::Mat &t) {
 }
 
 /**
- * @brief Inverse transform inplace
+ * @brief Inverse transform
  * @param T   transformation matrix (4x4)
  */
-inline void inverse(cv::Mat &T) {
+inline cv::Mat inverse(const cv::Mat &T) {
 	cv::Mat rmat;
 	cv::Mat tvec;
 	getRotationAndTranslation(T, rmat, tvec);
-	T = cv::Mat::eye(4, 4, CV_64FC1);
+	cv::Mat T_ = cv::Mat::eye(4, 4, CV_64FC1);
 
-	T(cv::Rect(3, 0, 1, 1)) = -rmat.col(0).dot(tvec);
-	T(cv::Rect(3, 1, 1, 1)) = -rmat.col(1).dot(tvec);
-	T(cv::Rect(3, 2, 1, 1)) = -rmat.col(2).dot(tvec);
-	T(cv::Rect(0, 0, 3, 3)) = rmat.t();
+	T_(cv::Rect(3, 0, 1, 1)) = -rmat.col(0).dot(tvec);
+	T_(cv::Rect(3, 1, 1, 1)) = -rmat.col(1).dot(tvec);
+	T_(cv::Rect(3, 2, 1, 1)) = -rmat.col(2).dot(tvec);
+	T_(cv::Rect(0, 0, 3, 3)) = rmat.t();
+	return T_;
 }
 
 inline cv::Point3d apply(const cv::Point3d& point, const cv::InputArray& R, const cv::InputArray& t) {
@@ -160,10 +161,10 @@ inline cv::Point3d apply(const cv::Point3d& point, const cv::InputArray& T) {
 
 /**
  * @brief Scale camera intrinsic matrix
- * @param size_new	New resolution
  * @param size_old	Original (camera matrix) resolution
+ * @param size_new	New resolution
  */
-cv::Mat scaleCameraMatrix(const cv::Mat &K, const cv::Size &size_new, const cv::Size &size_old);
+cv::Mat scaleCameraMatrix(const cv::Mat &K, const cv::Size &size_old, const cv::Size &size_new);
 
 /**
  * @brief Calculate MSE reprojection error
diff --git a/components/calibration/include/ftl/calibration/structures.hpp b/components/calibration/include/ftl/calibration/structures.hpp
index 032dfb761be79f030ff5318b225bda3cfabc92bc..4b277f2350df0392085ce853d74c792bce067121 100644
--- a/components/calibration/include/ftl/calibration/structures.hpp
+++ b/components/calibration/include/ftl/calibration/structures.hpp
@@ -2,12 +2,14 @@
 #define _FTL_CALIBRATION_STRUCTURES_HPP_
 
 #include <ftl/utility/msgpack.hpp>
+#include <ftl/codecs/channels.hpp>
 
 namespace ftl {
 namespace calibration {
 
 struct CalibrationData {
 	struct Intrinsic {
+		Intrinsic();
 		cv::Mat matrix() const;
 		static Intrinsic fromMatrix(const cv::Mat &K, cv::Size sz);
 		static Intrinsic fromMatrix(const cv::Mat &K, const cv::Mat &D, cv::Size sz);
@@ -21,28 +23,32 @@ struct CalibrationData {
 		MSGPACK_DEFINE(resolution, fx, fy, cx, cy, distCoeffs);
 	};
 	struct Extrinsic {
+		Extrinsic();
 		cv::Mat matrix() const;
 		static Extrinsic fromMatrix(const cv::Mat &T);
 
 		cv::Vec3d rvec;
 		cv::Vec3d tvec;
 		MSGPACK_DEFINE(rvec, tvec);
+	};
 
+	struct Calibration {
+		ftl::codecs::Channel channel;
+		Intrinsic intrinsic;
+		Extrinsic extrinsic;
+		MSGPACK_DEFINE(channel, intrinsic, extrinsic);
 	};
 
+	CalibrationData() : enabled(false) {}
 	bool enabled;
-	Intrinsic intrinsic_left;
-	Intrinsic intrinsic_right;
-	Extrinsic extrinsic_left;
-	Extrinsic extrinsic_right;
-
-	MSGPACK_DEFINE(enabled, intrinsic_left, intrinsic_right, extrinsic_left, extrinsic_right);
+	std::vector<Calibration> data;
 
-	static bool fromFile(const std::string &path, CalibrationData &out);
-	bool toFile(const std::string &path) const;
+	static CalibrationData readFile(const std::string &path);
+	void writeFile(const std::string &path) const;
+	MSGPACK_DEFINE(enabled, data);
 };
 
 }
 }
 
-#endif
\ No newline at end of file
+#endif
diff --git a/components/calibration/src/parameters.cpp b/components/calibration/src/parameters.cpp
index 049e655a262a6f6a14b6b382403fdc9c62b05f88..eca5105d8c6f4578775e3d0f7ac7b5ef728b1ddd 100644
--- a/components/calibration/src/parameters.cpp
+++ b/components/calibration/src/parameters.cpp
@@ -278,7 +278,7 @@ bool ftl::calibration::validate::distortionCoefficients(const Mat &D, Size size)
 	return true;
 }
 
-Mat ftl::calibration::scaleCameraMatrix(const Mat &K, const Size &size_new, const Size &size_old) {
+Mat ftl::calibration::scaleCameraMatrix(const Mat &K, const Size &size_old, const Size &size_new) {
 	Mat S(cv::Size(3, 3), CV_64F, 0.0);
 	double scale_x = ((double) size_new.width) / ((double) size_old.width);
 	double scale_y = ((double) size_new.height) / ((double) size_old.height);
diff --git a/components/calibration/src/structures.cpp b/components/calibration/src/structures.cpp
index dabf71d2c234a4daaafe3feadb6c065ec3e9b075..bbc820f3a926f93231de3a4959f1e5de62287da7 100644
--- a/components/calibration/src/structures.cpp
+++ b/components/calibration/src/structures.cpp
@@ -7,6 +7,9 @@
 
 using ftl::calibration::CalibrationData;
 
+CalibrationData::Intrinsic::Intrinsic() :
+	resolution(0, 0), fx(0.0), fy(0.0), cx(0.0), cy(0.0), distCoeffs(4, 0.0) {}
+
 CalibrationData::Intrinsic CalibrationData::Intrinsic::fromMatrix(const cv::Mat &K, cv::Size size) {
 	CalibrationData::Intrinsic params;
 	params.fx = K.at<double>(0, 0);
@@ -35,6 +38,10 @@ cv::Mat CalibrationData::Intrinsic::matrix() const {
 	return K;
 }
 
+////////////////////////////////////////////////////////////////////////////////
+
+CalibrationData::Extrinsic::Extrinsic() : rvec(0.0, 0.0, 0.0), tvec(0.0, 0.0, 0.0) {}
+
 CalibrationData::Extrinsic CalibrationData::Extrinsic::fromMatrix(const cv::Mat &T) {
 	CalibrationData::Extrinsic params;
 	cv::Rodrigues(T(cv::Rect(0, 0, 3, 3)), params.rvec);
@@ -55,60 +62,58 @@ cv::Mat CalibrationData::Extrinsic::matrix() const {
 	return T;
 }
 
-bool CalibrationData::fromFile(const std::string &path, CalibrationData &out) {
+////////////////////////////////////////////////////////////////////////////////
 
-	cv::Size resolution;
-	std::vector<cv::Mat> intrinsic;
-	std::vector<cv::Mat> distCoeffs;
-	std::vector<cv::Vec3d> rvec;
-	std::vector<cv::Vec3d> tvec;
+CalibrationData CalibrationData::readFile(const std::string &path) {
 
-	try {
-		cv::FileStorage fs;
-
-		fs.open((path).c_str(), cv::FileStorage::READ);
-		if (!fs.isOpened()) {
-			return false;
-		}
-
-		fs["resolution"] >> resolution;
-		fs["K"] >> intrinsic;
-		fs["D"] >> distCoeffs;
-		fs["rvec"] >> rvec;
-		fs["tvec"] >> tvec;
-		fs.release();
+	cv::FileStorage fs;
+	fs.open(path.c_str(), cv::FileStorage::READ);
+	if (!fs.isOpened()) {
+		throw std::exception();
+	}
+	CalibrationData data;
+	fs["enabled"] >> data.enabled;
+
+	for (auto it = fs["calibration"].begin(); it != fs["calibration"].end(); it++) {
+		Calibration calib;
+		(*it)["channel"] >> calib.channel;
+		(*it)["resolution"] >> calib.intrinsic.resolution;
+		(*it)["fx"] >> calib.intrinsic.fx;
+		(*it)["fy"] >> calib.intrinsic.fy;
+		(*it)["cx"] >> calib.intrinsic.cx;
+		(*it)["cy"] >> calib.intrinsic.cy;
+		(*it)["distCoeffs"] >> calib.intrinsic.distCoeffs;
+		(*it)["rvec"] >> calib.extrinsic.rvec;
+		(*it)["tvec"] >> calib.extrinsic.tvec;
+		data.data.push_back(calib);
 	}
-	catch (cv::Exception &e) {
-		return false;
-	} 
-	out.intrinsic_left = CalibrationData::Intrinsic::fromMatrix(intrinsic[0], distCoeffs[0], resolution);
-	out.intrinsic_right = CalibrationData::Intrinsic::fromMatrix(intrinsic[1], distCoeffs[1], resolution);
-	out.extrinsic_left.rvec = rvec[0];
-	out.extrinsic_left.tvec = tvec[0];
-	out.extrinsic_right.rvec = rvec[1];
-	out.extrinsic_right.tvec = tvec[1];
-	return true;
+
+	return data;
 }
 
-bool CalibrationData::toFile(const std::string &path) const {
+void CalibrationData::writeFile(const std::string &path) const {
 	cv::FileStorage fs(path, cv::FileStorage::WRITE);
-	if (!fs.isOpened()) { return false; }
-
-	fs	<< "resolution" << intrinsic_left.resolution
-		<< "K" << std::vector<cv::Mat>
-			{intrinsic_left.matrix(), intrinsic_right.matrix()}
-		
-		<< "D" << std::vector<std::vector<double>>
-			{intrinsic_left.distCoeffs, intrinsic_right.distCoeffs}
-
-		<< "rvec" << std::vector<cv::Vec3d>
-			{extrinsic_left.rvec, extrinsic_right.rvec}
-	
-		<< "tvec" << std::vector<cv::Vec3d>
-			{extrinsic_left.rvec, extrinsic_right.rvec}
-	;
-	
+	if (!fs.isOpened()) {
+		throw std::exception();
+	}
+
+	fs << "enabled" << enabled;
+	fs << "calibration" << "[";
+	for (auto &item : data) {
+		fs	<< "{:"
+			<<	"channel" << int(item.channel)
+			<<	"resolution" << item.intrinsic.resolution
+			<<	"fx" << item.intrinsic.fx
+			<<	"fy" << item.intrinsic.fy
+			<<	"cx" << item.intrinsic.cx
+			<<	"cy" << item.intrinsic.cy
+			<<	"distCoeffs" << item.intrinsic.distCoeffs
+			<< 	"rvec" << item.extrinsic.rvec
+			<< 	"tvec" << item.extrinsic.rvec
+			<< "}";
+	}
+	fs << "]";
+
 	fs.release();
-	return true;
 }
 
diff --git a/components/calibration/test/CMakeLists.txt b/components/calibration/test/CMakeLists.txt
index 0bd1f1f198347c38c74973b14a5253b4bcd93d09..59acdd613fa71a4863863c2bf04714d299e8d4a3 100644
--- a/components/calibration/test/CMakeLists.txt
+++ b/components/calibration/test/CMakeLists.txt
@@ -6,5 +6,5 @@ add_executable(calibration_parameters_unit
 )
 
 target_include_directories(calibration_parameters_unit PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/../include")
-target_link_libraries(calibration_parameters_unit ftlcommon Threads::Threads ${OS_LIBS} ${OpenCV_LIBS})
-add_test(CalibrationValidateTest calibration_parameters_unit)
\ No newline at end of file
+target_link_libraries(calibration_parameters_unit ftlcalibration ftlcommon ftlcodecs Threads::Threads ${OS_LIBS} ${OpenCV_LIBS})
+add_test(CalibrationValidateTest calibration_parameters_unit)
diff --git a/components/calibration/test/test_parameters.cpp b/components/calibration/test/test_parameters.cpp
index 7d19b9d757587ff7e73cde565c3643f8b8d5937e..be5722f79a9d0e764756da80afb9b540e8353f87 100644
--- a/components/calibration/test/test_parameters.cpp
+++ b/components/calibration/test/test_parameters.cpp
@@ -1,5 +1,6 @@
 #include "catch.hpp"
-#include "ftl/calibration/parameters.hpp"
+#include <ftl/calibration/parameters.hpp>
+#include <ftl/calibration/structures.hpp>
 
 using cv::Size;
 using cv::Mat;
@@ -17,7 +18,7 @@ TEST_CASE("Calibration values", "") {
 		D.at<double>(0) = 1.0;
 		D.at<double>(1) = 1.0;
 		REQUIRE(ftl::calibration::validate::distortionCoefficients(D, Size(1920, 1080)));
-		
+
 		D.at<double>(0) =  0.01512461889185869;
 		D.at<double>(1) = -0.1207895096066378;
 		D.at<double>(4) =  0.1582571415357494;
@@ -38,3 +39,32 @@ TEST_CASE("Calibration values", "") {
 		REQUIRE(!ftl::calibration::validate::distortionCoefficients(D, Size(1920, 1080)));
 	}
 }
+
+TEST_CASE("Test reading/writing file") {
+	using ftl::calibration::CalibrationData;
+	using ftl::codecs::Channel;
+
+	CalibrationData::Calibration calib;
+	CalibrationData calib_read;
+	CalibrationData data;
+
+	calib.channel = Channel::Left;
+	calib.intrinsic.resolution = {1, 1};
+	calib.intrinsic.fx = 1.0;
+	calib.intrinsic.fy = 1.0;
+	calib.intrinsic.cx = 0.5;
+	calib.intrinsic.cy = 0.5;
+	data.data.push_back(calib);
+
+	data.writeFile("/tmp/calib.yml");
+	calib_read = CalibrationData::readFile("/tmp/calib.yml");
+	REQUIRE(calib_read.data[0].channel == calib.channel);
+
+	data.writeFile("/tmp/calib.json");
+	calib_read = CalibrationData::readFile("/tmp/calib.json");
+	REQUIRE(calib_read.data[0].channel == calib.channel);
+
+	data.writeFile("/tmp/calib.xml");
+	calib_read = CalibrationData::readFile("/tmp/calib.xml");
+	REQUIRE(calib_read.data[0].channel == calib.channel);
+}
diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt
index 345d065bc231ae3c1fb9473b21f77cae50ee78f4..8fafef6448d82ebdbb34efc3ae1c04c172baf2ef 100644
--- a/components/rgbd-sources/CMakeLists.txt
+++ b/components/rgbd-sources/CMakeLists.txt
@@ -1,5 +1,5 @@
 set(RGBDSRC
-	src/sources/stereovideo/calibrate.cpp
+	src/sources/stereovideo/rectification.cpp
 	src/sources/stereovideo/opencv.cpp
 	src/source.cpp
 	src/frame.cpp
diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
deleted file mode 100644
index 546060127d979f6a2a4402314b9a22a521133b63..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
+++ /dev/null
@@ -1,346 +0,0 @@
-/*
- * Copyright 2019 Nicolas Pope
- */
-
-#include <loguru.hpp>
-#include <ftl/config.h>
-#include <ftl/configuration.hpp>
-#include <ftl/threads.hpp>
-#include <ftl/calibration/parameters.hpp>
-
-#include "calibrate.hpp"
-#include "ftl/exception.hpp"
-
-#include <opencv2/core.hpp>
-#include <opencv2/core/utility.hpp>
-#include <opencv2/imgproc.hpp>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/cudawarping.hpp>
-
-using ftl::rgbd::detail::Calibrate;
-
-using cv::FileStorage;
-
-using cv::INTER_LINEAR;
-
-using cv::FileNode;
-using cv::FileNodeIterator;
-
-using cv::Mat;
-using cv::cuda::GpuMat;
-using cv::cuda::Stream;
-
-using cv::Size;
-
-using cv::Point2f;
-using cv::Point3f;
-using cv::Matx33d;
-using cv::Scalar;
-
-using std::string;
-using std::vector;
-
-Calibrate::Calibrate(nlohmann::json &config, Size image_size, cv::cuda::Stream &stream) :
-		ftl::Configurable(config) {
-	
-	img_size_ = image_size;
-	calib_size_ = image_size;
-
-	K_ = vector<Mat>(2);
-	K_[0] = Mat::eye(Size(3, 3), CV_64FC1);
-	K_[1] = Mat::eye(Size(3, 3), CV_64FC1);
-	D_ = vector<Mat>(2);
-	D_[0] = Mat::zeros(Size(5, 1), CV_64FC1);
-	D_[1] = Mat::zeros(Size(5, 1), CV_64FC1);
-	pose_ = Mat::eye(Size(4, 4), CV_64FC1);
-	pose_adjustment_ = Mat::eye(Size(4, 4), CV_64FC1);
-	Q_ = Mat::eye(Size(4, 4), CV_64FC1);
-	Q_.at<double>(3, 2) = -1;
-	Q_.at<double>(2, 3) = 1;
-
-	setRectify(true);
-}
-
-Mat Calibrate::_getK(size_t idx, Size size) {
-	CHECK(idx < K_.size());
-	CHECK(!size.empty());
-	return ftl::calibration::scaleCameraMatrix(K_[idx], size, calib_size_);
-}
-
-Mat Calibrate::_getK(size_t idx) {
-	return _getK(idx, img_size_);
-}
-
-double Calibrate::getBaseline() const {
-	if (t_.empty()) { return 0.0; }
-	return cv::norm(t_);
-}
-
-double Calibrate::getDoff() const {
-	return -(Q_.at<double>(3,3) * getBaseline());
-}
-
-double Calibrate::getDoff(const Size& size) const {
-	return getDoff() * ((double) size.width / (double) img_size_.width);
-}
-
-Mat Calibrate::getCameraMatrixLeft(const cv::Size res) {
-	if (rectify_) {
-		return ftl::calibration::scaleCameraMatrix(Mat(P1_, cv::Rect(0, 0, 3, 3)), res, img_size_);
-	} else {
-		return ftl::calibration::scaleCameraMatrix(K_[0], res, calib_size_);
-	}
-}
-
-Mat Calibrate::getCameraMatrixRight(const cv::Size res) {
-	if (rectify_) {
-		return ftl::calibration::scaleCameraMatrix(Mat(P2_, cv::Rect(0, 0, 3, 3)), res, img_size_);
-	} else {
-		return ftl::calibration::scaleCameraMatrix(K_[1], res, calib_size_);
-	}
-}
-
-Mat Calibrate::getCameraDistortionLeft() {
-	if (rectify_) {	return Mat::zeros(Size(5, 1), CV_64FC1); }
-	else { return D_[0]; }
-}
-
-Mat Calibrate::getCameraDistortionRight() {
-	if (rectify_) {	return Mat::zeros(Size(5, 1), CV_64FC1); }
-	else { return D_[1]; }
-}
-
-Mat Calibrate::getPose() const {
-	Mat T;
-	if (rectify_) {
-		Mat R1 = Mat::eye(4, 4, CV_64FC1);
-		R1_.copyTo(R1(cv::Rect(0, 0, 3, 3)));
-		T = pose_ * R1.inv();
-	}
-	else {
-		pose_.copyTo(T);
-	}
-	return pose_adjustment_ * T;
-}
-
-bool Calibrate::setRectify(bool enabled) {
-	if (t_.empty() || R_.empty()) { enabled = false; }
-	if (enabled) { 
-		rectify_ = calculateRectificationParameters(); 
-	}
-	else {
-		rectify_ = false;
-	}
-	return rectify_;
-}
-
-bool Calibrate::setDistortion(const vector<Mat> &D) {
-	if (D.size() != 2) { return false; }
-	for (const auto d : D) { if (d.size() != Size(5, 1)) { return false; }}
-	D[0].copyTo(D_[0]);
-	D[1].copyTo(D_[1]);
-	return true;
-}
-
-bool Calibrate::setIntrinsics(const Size &size, const vector<Mat> &K) {
-	if (K.size() != 2) { return false; }
-	if (size.empty() || size.width <= 0 || size.height <= 0) { return false; }
-	for (const auto k : K) {
-		if (!ftl::calibration::validate::cameraMatrix(k)) {
-			return false;
-		}
-	}
-
-	calib_size_ = Size(size);
-	K[0].copyTo(K_[0]);
-	K[1].copyTo(K_[1]);
-	return true;
-}
-
-bool Calibrate::setExtrinsics(const Mat &R, const Mat &t) {
-	if (!ftl::calibration::validate::rotationMatrix(R) ||
-		!ftl::calibration::validate::translationStereo(t)) { return false; }
-	
-	R.copyTo(R_);
-	t.copyTo(t_);
-	return true;
-}
-
-bool Calibrate::setPose(const Mat &P) {
-	if (!ftl::calibration::validate::pose(P)) { return false; }
-	P.copyTo(pose_);
-	return true;
-}
-
-bool Calibrate::setPoseAdjustment(const Mat &T) {
-	if (!ftl::calibration::validate::pose(T)) { return false; }
-	pose_adjustment_ = T * pose_adjustment_;
-	return true;
-}
-
-bool Calibrate::loadCalibration(const string &fname) {
-	ftl::calibration::CalibrationData params;
-	if (ftl::calibration::CalibrationData::fromFile(fname, params)) {
-		setCalibration(params);
-		LOG(INFO) << "calibration loaded from: " << fname;
-		return true;
-	}
-	return false;
-
-	/*FileStorage fs;
-
-	fs.open((fname).c_str(), FileStorage::READ);
-	if (!fs.isOpened()) {
-		LOG(WARNING) << "Could not open calibration file";
-		return false;
-	}
-
-	Size calib_size;
-	vector<Mat> K;
-	vector<Mat> D;
-	Mat R;
-	Mat t;
-	Mat pose;
-	Mat pose_adjustment;
-
-	fs["resolution"] >> calib_size;
-	fs["K"] >> K;
-	fs["D"] >> D;
-	fs["R"] >> R;
-	fs["t"] >> t;
-	fs["P"] >> pose;
-	fs["adjustment"] >> pose_adjustment;
-	fs.release();
-
-	bool retval = true;
-	if (calib_size.empty()) {
-		LOG(ERROR) << "calibration resolution missing in calibration file";
-		retval = false;
-	}
-	if (!setIntrinsics(calib_size, K)) {
-		LOG(ERROR) << "invalid intrinsics in calibration file";
-		retval = false;
-	}
-	if (!setDistortion(D)) {
-		LOG(ERROR) << "invalid distortion parameters in calibration file";
-		retval = false;
-	}
-	if (!setExtrinsics(R, t)) {
-		LOG(ERROR) << "invalid extrinsics in calibration file";
-		retval = false;
-	}
-	if (!setPose(pose)) {
-		LOG(ERROR) << "invalid pose in calibration file";
-		retval = false;
-	}
-	if (!setPoseAdjustment(pose_adjustment)) {
-		LOG(WARNING) << "invalid pose adjustment in calibration file (using identity)";
-	}*/
-}
-
-bool Calibrate::writeCalibration(	const string &fname, const Size &size,
-									const vector<Mat> &K, const vector<Mat> &D, 
-									const Mat &R, const Mat &t, const Mat &pose,
-									const Mat &pose_adjustment) {
-	
-	cv::FileStorage fs(fname, cv::FileStorage::WRITE);
-	if (!fs.isOpened()) { return false; }
-
-	fs	<< "resolution" << size
-		<< "K" << K
-		<< "D" << D
-		<< "R" << R
-		<< "t" << t
-		<< "P" << pose
-		<< "adjustment" << pose_adjustment;
-	;
-	
-	fs.release();
-	return true;
-}
-
-bool Calibrate::saveCalibration(const string &fname) {
-	// note: never write rectified parameters!
-
-	// TODO: make a backup of old file
-	//if (std::filesystem::is_regular_file(fname)) {
-	//	// copy to fname + ".bak"
-	//}
-
-	return writeCalibration(fname, calib_size_, K_, D_, R_, t_, pose_, pose_adjustment_);
-}
-
-bool Calibrate::calculateRectificationParameters() {
-	
-	Mat K1 = _getK(0, img_size_);
-	Mat D1 = D_[0];
-	Mat K2 = _getK(1, img_size_);
-	Mat D2 = D_[1];
-	double alpha = value("alpha", 0.0);
-
-	try {
-		cv::stereoRectify(	K1, D1, K2, D2,
-							img_size_, R_, t_,
-							R1_, R2_, P1_, P2_, Q_, 0, alpha);
-		
-		initUndistortRectifyMap(K1, D1, R1_, P1_, img_size_, CV_32FC1, map1_.first, map2_.first);
-		initUndistortRectifyMap(K2, D2, R2_, P2_, img_size_, CV_32FC1, map1_.second, map2_.second);
-		
-		// CHECK Is this thread safe!!!!
-		map1_gpu_.first.upload(map1_.first);
-		map1_gpu_.second.upload(map1_.second);
-		map2_gpu_.first.upload(map2_.first);
-		map2_gpu_.second.upload(map2_.second);
-
-		Mat map0 = map1_.first.clone();
-		Mat map1 = map2_.first.clone();
-		cv::convertMaps(map0, map1, map1_.first, map2_.first, CV_16SC2);
-
-		map0 = map1_.second.clone();
-		map1 = map2_.second.clone();
-		cv::convertMaps(map0, map1, map1_.second, map2_.second, CV_16SC2);
-	}
-	catch (cv::Exception &ex) {
-		LOG(ERROR) << ex.what();
-		return false;
-	}
-
-	return true;
-}
-
-void Calibrate::rectifyStereo(GpuMat &l, GpuMat &r, Stream &stream) {
-	if (!rectify_) { return; }
-	// cv::cuda::remap() can not use same Mat for input and output
-	// TODO: create tmp buffers only once
-	GpuMat l_tmp(l.size(), l.type());
-	GpuMat r_tmp(r.size(), r.type());
-	cv::cuda::remap(l, l_tmp, map1_gpu_.first, map2_gpu_.first, cv::INTER_LINEAR, 0, cv::Scalar(), stream);
-	cv::cuda::remap(r, r_tmp, map1_gpu_.second, map2_gpu_.second, cv::INTER_LINEAR, 0, cv::Scalar(), stream);
-	stream.waitForCompletion();
-	l = l_tmp;
-	r = r_tmp;
-}
-
-void Calibrate::rectifyStereo(cv::Mat &l, cv::Mat &r) {
-	if (!rectify_) { return; }
-	// cv::cuda::remap() can not use same Mat for input and output
-	cv::remap(l, l, map1_.first, map2_.first, cv::INTER_LINEAR, 0, cv::Scalar());
-	cv::remap(r, r, map1_.second, map2_.second, cv::INTER_LINEAR, 0, cv::Scalar());
-}
-
-void Calibrate::rectifyLeft(cv::Mat &l) {
-	if (!rectify_) { return; }
-	cv::remap(l, l, map1_.first, map2_.first, cv::INTER_LINEAR, 0, cv::Scalar());
-}
-
-void Calibrate::rectifyRight(cv::Mat &r) {
-	if (!rectify_) { return; }
-	cv::remap(r, r, map1_.second, map2_.second, cv::INTER_LINEAR, 0, cv::Scalar());
-}
-
-void Calibrate::setCalibration(ftl::calibration::CalibrationData data) {
-	setIntrinsics(data.intrinsic_left.resolution, {
-		data.intrinsic_left.matrix(),
-		data.intrinsic_right.matrix(),
-	});
-}
\ No newline at end of file
diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
deleted file mode 100644
index b972830b1100947b187475ec654e0ff4ca84c5cc..0000000000000000000000000000000000000000
--- a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
+++ /dev/null
@@ -1,247 +0,0 @@
-/*
- * Copyright 2019 Nicolas Pope
- */
-
-#ifndef _FTL_CALIBRATION_HPP_
-#define _FTL_CALIBRATION_HPP_
-
-#include <opencv2/core.hpp>
-#include <opencv2/core/cuda.hpp>
-#include <string>
-#include <vector>
-#include <ftl/rgbd/camera.hpp>
-#include <ftl/calibration/structures.hpp>
-
-namespace cv {
-class FileStorage;
-class FileNode;
-};
-
-namespace ftl {
-namespace rgbd {
-namespace detail {
-
-/**
- * Manage local calibration details: undistortion, rectification and camera
- * parameters.
- */
-class Calibrate : public ftl::Configurable {
-	public:
-	Calibrate(nlohmann::json &config, cv::Size image_size, cv::cuda::Stream &stream);
-
-	/**
-	 * @brief	Rectify and undistort stereo pair images (GPU)
-	 */
-	void rectifyStereo(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::Stream &stream);
-
-	/**
-	 * @brief	Rectify and undistort stereo pair images (CPU)
-	 */
-	void rectifyStereo(cv::Mat &l, cv::Mat &r);
-
-	/**
-	 * @brief	Rectify and undistort left image (CPU)
-	 */
-	void rectifyLeft(cv::Mat &l);
-
-	/**
-	 * @brief	Rectify and undistort right image (CPU)
-	 */
-	void rectifyRight(cv::Mat &r);
-
-	void updateCalibration(const ftl::rgbd::Camera &p);
-	
-	/**
-	 * @brief Get disparity to depth matrix
-	 * @note Disparity offset is in image_size (scale)
-	 * 
-	 * 2020/01/15:	StereoVideoSource creates a Camera object which is used to
-	 * 				calculate depth from disparity (disp2depth.cu). Seems to be
-	 * 				used only in StereoVideoSource to get doff and baseline
-	 * 				parameter values in updateParameters()
-	 */
-	[[deprecated]]
-	const cv::Mat &getQ() const { return Q_; }
-
-	/**
-	 * @brief Get camera pair baseline
-	 */
-	double getBaseline() const;
-
-	/**
-	 * @brief Get camera pair disparity offset
-	 * @param size (optional) scale to given resolution.
-	 * 
-	 * Returns disparity offset for image_size resolution if size not provided.
-	 */
-	double getDoff() const;
-	double getDoff(const cv::Size& size) const;
-
-	/**
-	 * @brief	Get intrinsic paramters. If rectification is enabled, returns
-	 *			rectified intrinsic parameters, otherwise returns values from
-	 *			calibration. Parameters are scaled for given resolution.
-	 * @param	res		camera resolution
-	 */
-	cv::Mat getCameraMatrixLeft(const cv::Size res);
-	/** @brief	Same as getCameraMatrixLeft() for right camera */
-	cv::Mat getCameraMatrixRight(const cv::Size res);
-
-	/** @brief	Get camera distortion parameters. If rectification is enabled,
-	 * 			returns zeros. Otherwise returns calibrated distortion 
-	 * 			parameters values.
-	 */
-	cv::Mat getCameraDistortionLeft();
-	/** @brief	Same as getCameraDistortionLeft() for right camera */
-	cv::Mat getCameraDistortionRight();
-
-	/**
-	 * @brief	Get camera pose from calibration. Returns pose to rectified
-	 * 			camera if rectification is enabled.
-	 */
-	cv::Mat getPose() const;
-	
-	/**
-	 * @brief	Enable/disable recitification. If disabled, instance returns
-	 *			original camera intrinsic parameters (getCameraMatrixLeft() and
-	 *			getCameraMatrixRight() methods). When enabled (default), those
-	 *			methods return camera parameters for rectified images. Does not
-	 *			enable rectification, if valid parameters are missing.
-	 * @param	Rectification on/off
-	 * @returns	Status after call
-	 */
-	bool setRectify(bool enabled);
-
-	/**
-	 * @brief	Set intrinsic parameters for both cameras.
-	 * 
-	 * @param	size	calibration size
-	 * @param	K		2 camera matricies (3x3)
-	 * @returns	true if valid parameters
-	 */
-	bool setIntrinsics(const cv::Size &size, const std::vector<cv::Mat> &K);
-
-	/**
-	 * @brief	Set lens distortion parameters
-	 * @param	D 		2 distortion parameters (5x1)
-	 */
-	bool setDistortion(const std::vector<cv::Mat> &D);
-
-	/**
-	 * @brief	Set extrinsic parameters.
-	 * 
-	 * @param	R	Rotation matrix (3x3) from left to right camera
-	 * @param	t	Translation vector (1x3) from left to right camera
-	 * @returns	true if valid parameters
-	 */
-	bool setExtrinsics(const cv::Mat &R, const cv::Mat &t);
-
-	/**
-	 * @brief	Set pose
-	 * @param	pose	Pose for left camera
-	 * @returns	true if valid pose
-	 */
-	bool setPose(const cv::Mat &P);
-
-	/**
-	 * @brief	Set adjustment, which is applied to pose: T_update*T_pose 
-	 */
-	bool setPoseAdjustment(const cv::Mat &T);
-
-	/**
-	 * @breif	Set calibration
-	 */
-	void setCalibration(ftl::calibration::CalibrationData data);
-
-	/**
-	 * @brief	Calculate rectification parameters and maps. Can fail if
-	 * 			calibration parameters are invalid.
-	 * @returns	true if successful
-	 */
-	bool calculateRectificationParameters();
-
-	/**
-	 * @brief	Load calibration from file
-	 * @param	fname	File name
-	 */
-	bool loadCalibration(const std::string &fname);
-
-	/**
-	 * @brief	Write calibration parameters to file
-	 * 
-	 * Assumes two cameras and intrinsic calibration parameters have the same
-	 * resolution.
-	 * 
-	 * @todo	Validate loaded values
-	 * 
-	 * @param	fname file name
-	 * @param	size calibration resolution (intrinsic parameters)
-	 * @param	K intrinsic matrices
-	 * @param	D distortion coefficients
-	 * @param	R rotation from first camera to second
-	 * @param	t translation from first camera to second
-	 * @param	pose first camera's pose 
-	 */
-	static bool writeCalibration(const std::string &fname,
-								const cv::Size &size,
-								const std::vector<cv::Mat> &K,
-								const std::vector<cv::Mat> &D,
-								const cv::Mat &R, const cv::Mat &t,
-								const cv::Mat &pose,
-								const cv::Mat &pose_adjustment);
-
-	/*	@brief	Save current calibration to file
-	 *	@param	File name
-	 */
-	bool saveCalibration(const std::string &fname);
-
-private:
-	// rectification enabled/disabled
-	volatile bool rectify_;
-
-	/**
-	 * @brief	Get intrinsic matrix saved in calibration.
-	 * @param	Camera index (0 left, 1 right)
-	 * @param	Resolution
-	 */
-	cv::Mat _getK(size_t idx, cv::Size size);
-	cv::Mat _getK(size_t idx);
-
-	// calibration resolution (loaded from file by loadCalibration)
-	cv::Size calib_size_;
-	// camera resolution
-	cv::Size img_size_;
-
-	// rectification maps
-	std::pair<cv::Mat, cv::Mat> map1_;
-	std::pair<cv::Mat, cv::Mat> map2_;
-	std::pair<cv::cuda::GpuMat, cv::cuda::GpuMat> map1_gpu_;
-	std::pair<cv::cuda::GpuMat, cv::cuda::GpuMat> map2_gpu_;
-
-	// parameters for rectification, see cv::stereoRectify() documentation
-	cv::Mat R1_;
-	cv::Mat P1_;
-	cv::Mat R2_;
-	cv::Mat P2_;
-
-	// disparity to depth matrix
-	cv::Mat Q_;
-	
-	// intrinsic parameters and distortion coefficients
-	std::vector<cv::Mat> K_;
-	std::vector<cv::Mat> D_;
-
-	// transformation from left to right camera: R_ and T_
-	cv::Mat R_;
-	cv::Mat t_;
-	// pose for left camera
-	cv::Mat pose_;
-	cv::Mat pose_adjustment_;
-};
-
-}
-}
-}
-
-#endif // _FTL_CALIBRATION_HPP_
-
diff --git a/components/rgbd-sources/src/sources/stereovideo/device.hpp b/components/rgbd-sources/src/sources/stereovideo/device.hpp
index 7a7fcb45b6cd95e6588b93deee5e0efda4d26c9b..baae8a30821f6232470ab0fb66a7aa12966ef58b 100644
--- a/components/rgbd-sources/src/sources/stereovideo/device.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/device.hpp
@@ -9,7 +9,7 @@ namespace ftl {
 namespace rgbd {
 namespace detail {
 
-class Calibrate;
+class StereoRectification;
 
 struct DeviceDetails {
 	std::string name;
@@ -21,7 +21,7 @@ struct DeviceDetails {
 /**
  * Abstract base class for camera or stereo camera sources. Just wraps the
  * basic grab and retrieve functionality with rectification.
- * 
+ *
  * @see OpenCVDevice
  * @see PylonDevice
  */
@@ -33,7 +33,7 @@ class Device : public Configurable {
 	//virtual const std::vector<DeviceDetails> &listDevices()=0;
 
 	virtual bool grab()=0;
-	virtual bool get(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::GpuMat &h_l, cv::Mat &h_r, Calibrate *c, cv::cuda::Stream &stream)=0;
+	virtual bool get(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::GpuMat &h_l, cv::Mat &h_r, StereoRectification *c, cv::cuda::Stream &stream)=0;
 
 	virtual unsigned int width() const =0;
 	virtual unsigned int height() const =0;
@@ -42,9 +42,9 @@ class Device : public Configurable {
 	virtual unsigned int fullHeight() const =0;
 
 	inline bool hasHigherRes() const { return fullWidth() != width(); }
-	
+
 	virtual double getTimestamp() const =0;
-	
+
 	virtual bool isStereo() const =0;
 };
 
@@ -52,4 +52,4 @@ class Device : public Configurable {
 }
 }
 
-#endif
\ No newline at end of file
+#endif
diff --git a/components/rgbd-sources/src/sources/stereovideo/opencv.cpp b/components/rgbd-sources/src/sources/stereovideo/opencv.cpp
index 23c5165a8d332ea3b72c31c09df9e1be0c1efbe7..5efd1ba3e6fd2b9f6f8545fef94e36f29b9bb7a6 100644
--- a/components/rgbd-sources/src/sources/stereovideo/opencv.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/opencv.cpp
@@ -10,7 +10,7 @@
 #include <ftl/profiler.hpp>
 
 #include "opencv.hpp"
-#include "calibrate.hpp"
+#include "rectification.hpp"
 #include <opencv2/core.hpp>
 #include <opencv2/opencv.hpp>
 #include <opencv2/xphoto.hpp>
@@ -31,7 +31,8 @@
 #endif
 
 using ftl::rgbd::detail::OpenCVDevice;
-using ftl::rgbd::detail::Calibrate;
+using ftl::rgbd::detail::StereoRectification;
+using ftl::codecs::Channel;
 using cv::Mat;
 using cv::VideoCapture;
 using cv::Rect;
@@ -125,7 +126,7 @@ OpenCVDevice::OpenCVDevice(nlohmann::json &config)
 	camera_a_->set(cv::CAP_PROP_FRAME_HEIGHT, value("height", 720));
 	camera_a_->set(cv::CAP_PROP_FPS, 1000 / ftl::timer::getInterval());
 	//camera_a_->set(cv::CAP_PROP_BUFFERSIZE, 0);  // Has no effect
-	
+
 	Mat frame;
 	if (!camera_a_->grab()) LOG(ERROR) << "Could not grab a video frame";
 	camera_a_->retrieve(frame);
@@ -207,7 +208,7 @@ std::vector<ftl::rgbd::detail::DeviceDetails> OpenCVDevice::_selectDevices() {
 		}
 		else
 		{
-			
+
 		}
 	}
 
@@ -219,7 +220,7 @@ std::vector<ftl::rgbd::detail::DeviceDetails> OpenCVDevice::_selectDevices() {
 #else
 
 	int fd;
-    v4l2_capability video_cap;
+	v4l2_capability video_cap;
 	v4l2_frmsizeenum video_fsize;
 
 	LOG(INFO) << "Video Devices:";
@@ -312,8 +313,8 @@ bool OpenCVDevice::grab() {
 }
 
 bool OpenCVDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out,
-	cv::cuda::GpuMat &l_hres_out, cv::Mat &r_hres_out, Calibrate *c, cv::cuda::Stream &stream) {
-	
+	cv::cuda::GpuMat &l_hres_out, cv::Mat &r_hres_out, StereoRectification *c, cv::cuda::Stream &stream) {
+
 	Mat l, r ,hres;
 
 	// Use page locked memory
@@ -337,7 +338,7 @@ bool OpenCVDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out,
 			cv::cvtColor(frame_r_, rfull, cv::COLOR_BGR2BGRA);
 
 			if (stereo_) {
-				c->rectifyRight(rfull);
+				c->rectify(rfull, Channel::Right);
 
 				if (hasHigherRes()) {
 					// TODO: Use threads?
@@ -378,8 +379,8 @@ bool OpenCVDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out,
 	if (stereo_) {
 		//FTL_Profile("Rectification", 0.01);
 		//c->rectifyStereo(lfull, rfull);
-		c->rectifyLeft(lfull);
-		
+		c->rectify(lfull, Channel::Left);
+
 		// Need to resize
 		//if (hasHigherRes()) {
 			// TODO: Use threads?
diff --git a/components/rgbd-sources/src/sources/stereovideo/opencv.hpp b/components/rgbd-sources/src/sources/stereovideo/opencv.hpp
index 1db067b8d5e08078e1d1136eccfc0938bfbe3c32..9915cf701bb60402f150f39eecf65cab5f848913 100644
--- a/components/rgbd-sources/src/sources/stereovideo/opencv.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/opencv.hpp
@@ -18,20 +18,20 @@ class OpenCVDevice : public ftl::rgbd::detail::Device {
 	~OpenCVDevice();
 
 	static std::vector<DeviceDetails> listDevices();
-	
+
 	bool grab() override;
-	bool get(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::GpuMat &h_l, cv::Mat &h_r, Calibrate *c, cv::cuda::Stream &stream) override;
+	bool get(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::GpuMat &h_l, cv::Mat &h_r, StereoRectification *c, cv::cuda::Stream &stream) override;
 
 	unsigned int width() const override { return dwidth_; }
 	unsigned int height() const override { return dheight_; }
 
 	unsigned int fullWidth() const override { return width_; }
 	unsigned int fullHeight() const override { return height_; }
-	
+
 	double getTimestamp() const override;
-	
+
 	bool isStereo() const override;
-	
+
 	private:
 	double timestamp_;
 	//double tps_;
diff --git a/components/rgbd-sources/src/sources/stereovideo/pylon.cpp b/components/rgbd-sources/src/sources/stereovideo/pylon.cpp
index 3156294ea042cadc0fff5ad6299582e196f99a6a..33acac1148e78eca9d1cf352a7630bd109885522 100644
--- a/components/rgbd-sources/src/sources/stereovideo/pylon.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/pylon.cpp
@@ -1,6 +1,7 @@
 #include "pylon.hpp"
 
-#include "calibrate.hpp"
+#include "rectification.hpp"
+
 #include <loguru.hpp>
 #include <ftl/threads.hpp>
 #include <ftl/rgbd/source.hpp>
@@ -13,6 +14,7 @@
 
 #include <nlohmann/json.hpp>
 
+using ftl::rgbd::detail::StereoRectification;
 using ftl::rgbd::detail::PylonDevice;
 using std::string;
 using ftl::codecs::Channel;
@@ -21,7 +23,7 @@ using cv::Mat;
 using namespace Pylon;
 
 PylonDevice::PylonDevice(nlohmann::json &config)
-        : ftl::rgbd::detail::Device(config), ready_(false), lcam_(nullptr), rcam_(nullptr) {
+		: ftl::rgbd::detail::Device(config), ready_(false), lcam_(nullptr), rcam_(nullptr) {
 
 	auto &inst = CTlFactory::GetInstance();
 
@@ -60,7 +62,7 @@ PylonDevice::PylonDevice(nlohmann::json &config)
 	if (dev_left_num == -1) dev_left_num = 0;
 
 	try {
-    	lcam_ = new CBaslerUniversalInstantCamera( CTlFactory::GetInstance().CreateDevice(devices[dev_left_num]));
+		lcam_ = new CBaslerUniversalInstantCamera( CTlFactory::GetInstance().CreateDevice(devices[dev_left_num]));
 		lcam_->RegisterConfiguration( new Pylon::CSoftwareTriggerConfiguration, Pylon::RegistrationMode_ReplaceAll, Pylon::Cleanup_Delete);
 		lcam_->Open();
 
@@ -80,7 +82,7 @@ PylonDevice::PylonDevice(nlohmann::json &config)
 		ready_ = true;
 	} catch (const Pylon::GenericException &e) {
 		// Error handling.
-        LOG(ERROR) << "Pylon: An exception occurred - " << e.GetDescription();
+		LOG(ERROR) << "Pylon: An exception occurred - " << e.GetDescription();
 	}
 
 	// Choose a good default depth res
@@ -171,7 +173,7 @@ bool PylonDevice::grab() {
 	return true;
 }
 
-bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda::GpuMat &h_l, cv::Mat &h_r, Calibrate *c, cv::cuda::Stream &stream) {
+bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda::GpuMat &h_l, cv::Mat &h_r, StereoRectification *c, cv::cuda::Stream &stream) {
 	if (!isReady()) return false;
 
 	Mat l, r ,hres;
@@ -213,7 +215,7 @@ bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda
 				cv::cvtColor(wrap_right, rfull, cv::COLOR_BayerRG2BGRA);
 
 				if (isStereo()) {
-					c->rectifyRight(rfull);
+					c->rectify(rfull, Channel::Right);
 
 					if (hasHigherRes()) {
 						cv::resize(rfull, r, r.size(), 0.0, 0.0, cv::INTER_CUBIC);
@@ -249,7 +251,7 @@ bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda
 		cv::cvtColor(wrap_left, lfull, cv::COLOR_BayerRG2BGRA);
 
 		if (isStereo()) {
-			c->rectifyLeft(lfull);
+			c->rectify(lfull, Channel::Left);
 		}
 
 		if (hasHigherRes()) {
@@ -273,6 +275,6 @@ bool PylonDevice::get(cv::cuda::GpuMat &l_out, cv::cuda::GpuMat &r_out, cv::cuda
 }
 
 bool PylonDevice::isReady() const {
-    return lcam_ && lcam_->IsOpen();
+	return lcam_ && lcam_->IsOpen();
 }
 
diff --git a/components/rgbd-sources/src/sources/stereovideo/pylon.hpp b/components/rgbd-sources/src/sources/stereovideo/pylon.hpp
index 84b0742fb3b141f0785f87de38bcb1b349b69675..7af8ad2bf15aca2905feb8a05f178cd2cb152e58 100644
--- a/components/rgbd-sources/src/sources/stereovideo/pylon.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/pylon.hpp
@@ -21,16 +21,16 @@ class PylonDevice : public ftl::rgbd::detail::Device {
 	static std::vector<DeviceDetails> listDevices();
 
 	bool grab() override;
-	bool get(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::GpuMat &h_l, cv::Mat &h_r, Calibrate *c, cv::cuda::Stream &stream) override;
+	bool get(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::GpuMat &h_l, cv::Mat &h_r, StereoRectification *c, cv::cuda::Stream &stream) override;
 
 	unsigned int width() const override { return width_; }
 	unsigned int height() const override { return height_; };
 
 	unsigned int fullWidth() const override { return fullwidth_; }
 	unsigned int fullHeight() const override { return fullheight_; }
-	
+
 	double getTimestamp() const override { return 0.0; }
-	
+
 	bool isStereo() const override { return lcam_ && rcam_; }
 
 	bool isReady() const;
diff --git a/components/rgbd-sources/src/sources/stereovideo/rectification.cpp b/components/rgbd-sources/src/sources/stereovideo/rectification.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..78c935eccef49bda930e445739dc5abdedae8478
--- /dev/null
+++ b/components/rgbd-sources/src/sources/stereovideo/rectification.cpp
@@ -0,0 +1,216 @@
+/*
+ * Copyright 2019 Nicolas Pope
+ */
+
+#include <loguru.hpp>
+#include <ftl/config.h>
+#include <ftl/configuration.hpp>
+#include <ftl/calibration/parameters.hpp>
+
+#include "rectification.hpp"
+#include "ftl/exception.hpp"
+
+#include <opencv2/core.hpp>
+#include <opencv2/core/utility.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/calib3d.hpp>
+
+using ftl::rgbd::detail::StereoRectification;
+using ftl::calibration::CalibrationData;
+using ftl::codecs::Channel;
+
+StereoRectification::StereoRectification(nlohmann::json &config, cv::Size image_size) :
+	ftl::Configurable(config), image_resolution_(image_size),
+	enabled_(false), valid_(false), interpolation_(cv::INTER_LINEAR),
+	baseline_(0.0) {
+
+}
+
+void StereoRectification::setSize(cv::Size size) {
+	image_resolution_ = size;
+	calculateParameters();
+}
+
+void StereoRectification::setInterpolation(int interpolation) {
+	interpolation_ = interpolation;
+}
+
+void StereoRectification::setEnabled(bool value) {
+	enabled_ = value;
+}
+
+bool StereoRectification::enabled() {
+	return enabled_;
+}
+
+bool StereoRectification::calibrated() {
+	return valid_;
+}
+
+void StereoRectification::setCalibration(const CalibrationData &calib) {
+
+	for (const auto &data : calib.data) {
+		if (data.channel == Channel::Left) {
+			calib_left_ = data;
+		}
+		else if (data.channel == Channel::Right) {
+			calib_right_ = data;
+		}
+	}
+
+	calculateParameters();
+}
+
+void StereoRectification::calculateParameters() {
+	using namespace ftl::calibration;
+	// TODO: lock
+	valid_ = false;
+	bool valid = true;
+	valid_ &= !calib_left_.intrinsic.resolution.empty();
+	valid_ &= !calib_right_.intrinsic.resolution.empty();
+	valid_ &= validate::cameraMatrix(calib_left_.intrinsic.matrix());
+	valid_ &= validate::cameraMatrix(calib_right_.intrinsic.matrix());
+	if (!valid) { return; }
+
+	// create temporary buffers
+	if (tmp_l_.size() != image_resolution_) {
+		//using cv::cuda::HostMem;
+		//tmp_l_ = HostMem(image_resolution_, CV_8UC4, HostMem::AllocType::PAGE_LOCKED);
+		tmp_l_ = cv::Mat(image_resolution_, CV_8UC4);
+	}
+	if (tmp_l_.size() != image_resolution_) {
+		//using cv::cuda::HostMem;
+		//tmp_r_ = HostMem(image_resolution_, CV_8UC4, HostMem::AllocType::PAGE_LOCKED);
+		tmp_r_ = cv::Mat(image_resolution_, CV_8UC4);
+	}
+
+	cv::Mat K_l = calib_left_.intrinsic.matrix();
+	cv::Mat K_r = calib_right_.intrinsic.matrix();
+	cv::Mat dc_l = cv::Mat(calib_left_.intrinsic.distCoeffs);
+	cv::Mat dc_r = cv::Mat(calib_right_.intrinsic.distCoeffs);
+
+	// scale if necessary
+	if (calib_left_.intrinsic.resolution != image_resolution_) {
+		K_l = scaleCameraMatrix(K_l, calib_left_.intrinsic.resolution, image_resolution_);
+	}
+	if (calib_right_.intrinsic.resolution != image_resolution_) {
+		K_r = scaleCameraMatrix(K_r, calib_right_.intrinsic.resolution, image_resolution_);
+	}
+
+	// calculate rotation and translation from left to right using calibration
+	cv::Mat T_l = calib_left_.extrinsic.matrix();
+	cv::Mat T_r = calib_left_.extrinsic.matrix();
+	cv::Mat T = transform::inverse(T_r) * T_l;
+	cv::Mat R;
+	cv::Mat t;
+	transform::getRotationAndTranslation(T, R, t);
+
+	// calculate rectification parameters
+	cv::stereoRectify(	K_l, dc_l, K_r, dc_r, image_resolution_,
+						R, t, R_l_, R_r_, P_l_, P_r_, Q_, 0, 0);
+
+	baseline_ = cv::norm(t);
+
+	// for CPU remap, CV_16SC2 should give best performance
+	// https://docs.opencv.org/master/da/d54/group__imgproc__transform.html
+	cv::initUndistortRectifyMap(K_l, dc_l, R_l_, P_l_, image_resolution_,
+								CV_16SC2, map_l_.first, map_l_.second);
+	cv::initUndistortRectifyMap(K_r, dc_r, R_r_, P_r_, image_resolution_,
+								CV_16SC2, map_r_.first, map_r_.second);
+
+	valid_ = true;
+}
+
+void StereoRectification::rectify(cv::InputOutputArray im, Channel c) {
+
+	if (!enabled_ || !valid_) { return; }
+
+	if (im.size() != image_resolution_) {
+		auto sz = im.size();
+		throw ftl::exception("Input has wrong size");
+	}
+	if (im.isMat()) {
+		cv::Mat &in = im.getMatRef();
+		if (c == Channel::Left) {
+			cv::remap(in, tmp_l_, map_l_.first, map_l_.second, interpolation_);
+			cv::swap(in, tmp_l_);
+		}
+		else if (c == Channel::Right) {
+			cv::remap(in, tmp_r_, map_r_.first, map_r_.second, interpolation_);
+			cv::swap(in, tmp_r_);
+		}
+		else {
+			throw ftl::exception("Bad channel for rectification");
+		}
+	}
+	else if (im.isGpuMat()) {
+		throw ftl::exception("GPU rectification not implemented");
+	}
+	else {
+		throw ftl::exception("Input not Mat/GpuMat");
+	}
+}
+
+cv::Mat StereoRectification::getPose(Channel c) {
+	using ftl::calibration::transform::inverse;
+
+	if (enabled_ && valid_) {
+		cv::Mat T = cv::Mat::eye(4, 4, CV_64FC1);
+		if (c == Channel::Left) {
+			R_l_.copyTo(T(cv::Rect(0, 0, 3, 3)));
+			T = calib_left_.extrinsic.matrix() * inverse(T);
+		}
+		else if (c == Channel::Right) {
+			R_r_.copyTo(T(cv::Rect(0, 0, 3, 3)));
+			T = calib_right_.extrinsic.matrix() * inverse(T);
+		}
+	}
+	else {
+		if (c == Channel::Left) {
+			return calib_left_.extrinsic.matrix();
+		}
+		else if (c == Channel::Right) {
+			return calib_right_.extrinsic.matrix();
+		}
+	}
+	throw ftl::exception("Invalid channel, expected Left or Right");
+}
+
+double StereoRectification::baseline() {
+	return baseline_;
+}
+
+double StereoRectification::doff() {
+	return -(Q_.at<double>(3,3) * baseline_);
+}
+
+double StereoRectification::doff(cv::Size size) {
+	return doff() * double(size.width)/double(image_resolution_.width);
+}
+
+cv::Mat StereoRectification::cameraMatrix(Channel c) {
+	if (enabled_ && valid_) {
+		if (c == Channel::Left) {
+			// P_l_: Left camera is origin in rectified system, there extrinsic
+			// is no rotation and intrinsic matrix can be directly extracted.
+			return cv::Mat(P_l_, cv::Rect(0, 0, 3, 3)).clone();
+		}
+		else if (c == Channel::Right) {
+			// Extrinsics are included in P_r_, can't do same as above
+			throw ftl::exception("Not implemented");
+		}
+	}
+	else {
+		if (c == Channel::Left) {
+			return calib_left_.intrinsic.matrix();
+		}
+		else if (c == Channel::Right) {
+			return calib_right_.intrinsic.matrix();
+		}
+	}
+	throw ftl::exception("Invalid channel, expected Left or Right");
+}
+
+cv::Mat StereoRectification::cameraMatrix(cv::Size size, Channel c) {
+	return ftl::calibration::scaleCameraMatrix(cameraMatrix(c), image_resolution_, size);
+}
diff --git a/components/rgbd-sources/src/sources/stereovideo/rectification.hpp b/components/rgbd-sources/src/sources/stereovideo/rectification.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..948f12c49f4fd066c1b5e818259626512118c309
--- /dev/null
+++ b/components/rgbd-sources/src/sources/stereovideo/rectification.hpp
@@ -0,0 +1,100 @@
+/*
+ * Copyright 2019 Nicolas Pope
+ */
+
+#ifndef _FTL_CALIBRATION_HPP_
+#define _FTL_CALIBRATION_HPP_
+
+#include <opencv2/core.hpp>
+#include <opencv2/core/cuda.hpp>
+
+#include <string>
+#include <vector>
+
+#include <ftl/codecs/channels.hpp>
+#include <ftl/rgbd/camera.hpp>
+#include <ftl/calibration/structures.hpp>
+
+namespace ftl {
+namespace rgbd {
+namespace detail {
+
+/**
+ * Stereo rectification. Performs rectification for left and right channels.
+ * Rectified image is same size as input image.
+ */
+class StereoRectification : public ftl::Configurable {
+public:
+	StereoRectification(nlohmann::json &config, cv::Size image_size);
+
+	void setInterpolation(int interpolation);
+
+	void setSize(cv::Size);
+	/**
+	 * Calculate rectification parameters from given calibration.
+	 */
+	void setCalibration(const ftl::calibration::CalibrationData &calib);
+	bool calibrated();
+
+	void rectify(cv::InputOutputArray im, ftl::codecs::Channel c);
+
+	/**
+	 * Enable/disable rectification.
+	 */
+	void setEnabled(bool enabled);
+	bool enabled();
+
+	/**
+	 * Get camera pose (rectified if enabled and valid)
+	 */
+	cv::Mat getPose(ftl::codecs::Channel c = ftl::codecs::Channel::Left);
+
+	/**
+	 * Get intrinsic matrix.
+	 */
+	cv::Mat cameraMatrix(ftl::codecs::Channel c = ftl::codecs::Channel::Left);
+	cv::Mat cameraMatrix(cv::Size size, ftl::codecs::Channel c = ftl::codecs::Channel::Left);
+
+	/** Stereo baseline */
+	double baseline();
+	/** Disparity offset */
+	double doff();
+	double doff(cv::Size);
+
+protected:
+	void calculateParameters();
+
+private:
+	cv::Size calib_resolution_;
+	ftl::calibration::CalibrationData::Calibration calib_left_;
+	ftl::calibration::CalibrationData::Calibration calib_right_;
+
+	cv::Size image_resolution_;
+
+	// rectification parameters and maps
+	bool enabled_;
+	bool valid_;
+	int interpolation_;
+	double baseline_;
+	cv::Mat Q_;
+	cv::Mat R_l_;
+	cv::Mat R_r_;
+	cv::Mat P_l_;
+	cv::Mat P_r_;
+
+	std::pair<cv::Mat,cv::Mat> map_l_;
+	std::pair<cv::Mat,cv::Mat> map_r_;
+
+	// temporary buffers
+	// cv::cuda::HostMem tmp_l_;
+	// cv::cuda::HostMem tmp_r_;
+	cv::Mat tmp_l_;
+	cv::Mat tmp_r_;
+};
+
+}
+}
+}
+
+#endif // _FTL_CALIBRATION_HPP_
+
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
index eadfefb7f3f4d3cc8e9d7597b99f3360d7ccab9b..815e426577533abf3ee83ca4c96e62ba72c77005 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -1,41 +1,40 @@
 #include <loguru.hpp>
 
+#include <unordered_set>
+
 #include <Eigen/Eigen>
 #include <opencv2/core/eigen.hpp>
 
-#include "stereovideo.hpp"
-
 #include <ftl/configuration.hpp>
 #include <ftl/profiler.hpp>
 
 #include <nlohmann/json.hpp>
 
 #ifdef HAVE_OPTFLOW
-#include "ftl/operators/opticalflow.hpp"
+#include <ftl/operators/opticalflow.hpp>
 #endif
 
-#include "ftl/operators/smoothing.hpp"
-#include "ftl/operators/colours.hpp"
-#include "ftl/operators/normals.hpp"
-#include "ftl/operators/filling.hpp"
-#include "ftl/operators/segmentation.hpp"
-#include "ftl/operators/disparity.hpp"
-#include "ftl/operators/mask.hpp"
+#include <ftl/operators/smoothing.hpp>
+#include <ftl/operators/colours.hpp>
+#include <ftl/operators/normals.hpp>
+#include <ftl/operators/filling.hpp>
+#include <ftl/operators/segmentation.hpp>
+#include <ftl/operators/disparity.hpp>
+#include <ftl/operators/mask.hpp>
 
 #include <ftl/rgbd/capabilities.hpp>
+#include <ftl/calibration/structures.hpp>
 
+#include "stereovideo.hpp"
 #include "ftl/threads.hpp"
-#include "calibrate.hpp"
-#include "opencv.hpp"
-#include <unordered_set>
+#include "rectification.hpp"
 
-#include <ftl/calibration/structures.hpp>
+#include "opencv.hpp"
 
 #ifdef HAVE_PYLON
 #include "pylon.hpp"
 #endif
 
-using ftl::rgbd::detail::Calibrate;
 using ftl::rgbd::detail::StereoVideoSource;
 using ftl::codecs::Channel;
 using std::string;
@@ -67,7 +66,6 @@ StereoVideoSource::StereoVideoSource(ftl::rgbd::Source *host, const string &file
 }
 
 StereoVideoSource::~StereoVideoSource() {
-	delete calib_;
 	delete lsrc_;
 }
 
@@ -118,140 +116,40 @@ void StereoVideoSource::init(const string &file) {
 	#endif
 	pipeline_input_->append<ftl::operators::ColourChannels>("colour");
 
-	calib_ = ftl::create<Calibrate>(host_, "calibration", cv::Size(lsrc_->fullWidth(), lsrc_->fullHeight()), stream_);
+	cv::Size size_full = cv::Size(lsrc_->fullWidth(), lsrc_->fullHeight());
+	rectification_ = std::unique_ptr<StereoRectification>
+		(ftl::create<StereoRectification>(host_, "rectification", size_full));
 
 	string fname_default = "calibration.yml";
-	auto fname_config = calib_->get<string>("calibration");
+	auto fname_config = host_->get<string>("calibration");
 	string fname = fname_config ? *fname_config : fname_default;
 	auto calibf = ftl::locateFile(fname);
 	if (calibf) {
-		fname = *calibf;
-		if (calib_->loadCalibration(fname)) {
-			calib_->calculateRectificationParameters();
-			calib_->setRectify(true);
-		}
+		fname_calib_ = *calibf;
+		calibration_ = ftl::calibration::CalibrationData::readFile(fname_calib_);
 	}
 	else {
-		fname = fname_config ? *fname_config :
-								string(FTL_LOCAL_CONFIG_ROOT) + "/"
-								+ std::string("calibration.yml");
-
-		LOG(ERROR) << "No calibration, default path set to " + fname;
+		fname_calib_ = fname_config ?	*fname_config :
+										string(FTL_LOCAL_CONFIG_ROOT) + "/"
+										+ std::string("calibration.yml");
 
-		// set use config file/set (some) default values
-
-		cv::Mat K = cv::Mat::eye(cv::Size(3, 3), CV_64FC1);
-		K.at<double>(0,0) = host_->value("focal", 800.0);
-		K.at<double>(1,1) = host_->value("focal", 800.0);
-		K.at<double>(0,2) = host_->value("centre_x", color_size_.width/2.0f);
-		K.at<double>(1,2) = host_->value("centre_y", color_size_.height/2.0f);
-
-		calib_->setIntrinsics(color_size_, {K, K});
+		LOG(ERROR) << "No calibration file found, calibration will be saved to " + fname;
 	}
-	fname_calib_ = fname;
-
-	////////////////////////////////////////////////////////////////////////////
-	// RPC callbacks to update calibration
-	// Should only be used by calibration app (interface may change)
-	// Tries to follow interface of ftl::Calibrate
-
-	host_->getNet()->bind("set_pose",
-		[this](cv::Mat pose){
-			if (!calib_->setPose(pose)) {
-				LOG(ERROR) << "invalid pose received (bad value)";
-				return false;
-			}
-			do_update_params_ = true;
-			LOG(INFO) << "new pose";
-			return true;
-	});
-
-	host_->getNet()->bind("set_pose_adjustment",
-		[this](cv::Mat T){
-			if (!calib_->setPoseAdjustment(T)) {
-				LOG(ERROR) << "invalid pose received (bad value)";
-				return false;
-			}
-			do_update_params_ = true;
-			LOG(INFO) << "new pose adjustment";
-			return true;
-	});
-
-
-	host_->getNet()->bind("set_intrinsics",
-		[this](cv::Size size, cv::Mat K_l, cv::Mat D_l, cv::Mat K_r, cv::Mat D_r) {
 
-			if (!calib_->setIntrinsics(size, {K_l, K_r})) {
-				LOG(ERROR) << "bad intrinsic parameters (bad values)";
-				return false;
-			}
-
-			if (!D_l.empty() && !D_r.empty()) {
-				if (!calib_->setDistortion({D_l, D_r})) {
-					LOG(ERROR) << "bad distortion parameters (bad values)";
-					return false;
-				}
-			}
-			do_update_params_ = true;
-			LOG(INFO) << "new intrinsic parameters";
-			return true;
-	});
-
-	host_->getNet()->bind("set_extrinsics",
-		[this](cv::Mat R, cv::Mat t){
-			if (!calib_->setExtrinsics(R, t)) {
-				LOG(ERROR) << "invalid extrinsic parameters (bad values)";
-				return false;
-			}
-			do_update_params_ = true;
-			LOG(INFO) << "new extrinsic (stereo) parameters";
-			return true;
-	});
-
-	host_->getNet()->bind("save_calibration",
-		[this, fname](){
-			LOG(INFO) << "saving calibration to " << fname;
-			return calib_->saveCalibration(fname);
-	});
-
-	host_->getNet()->bind("set_rectify",
-		[this](bool enable){
-			bool retval = calib_->setRectify(enable);
-			do_update_params_ = true;
-			LOG(INFO) << "rectification " << (retval ? "on" : "off");
-			return retval;
-	});
-
-	host_->getNet()->bind("get_distortion", [this]() {
-		return std::vector<cv::Mat>{
-			cv::Mat(calib_->getCameraDistortionLeft()),
-			cv::Mat(calib_->getCameraDistortionRight()) };
-	});
-
-	////////////////////////////////////////////////////////////////////////////
-
-	// Generate camera parameters from camera matrix
-	//updateParameters();
+	// Generate camera parameters for next frame
 	do_update_params_ = true;
 
 	LOG(INFO) << "StereoVideo source ready...";
 	ready_ = true;
-
-	//state_.set("name", host_->value("name", host_->getID()));
 }
 
 ftl::rgbd::Camera StereoVideoSource::parameters(Channel chan) {
-	/*if (chan == Channel::Right) {
-		return state_.getRight();
-	} else {
-		return state_.getLeft();
-	}*/
 	return params_;
 }
 
 void StereoVideoSource::updateParameters(ftl::rgbd::Frame &frame) {
 	Eigen::Matrix4d pose;
-	cv::cv2eigen(calib_->getPose(), pose);
+	cv::cv2eigen(rectification_->getPose(Channel::Left), pose);
 
 	frame.setPose() = pose;
 
@@ -269,33 +167,40 @@ void StereoVideoSource::updateParameters(ftl::rgbd::Frame &frame) {
 		}
 	}
 
-	frame.create<ftl::calibration::CalibrationData>(Channel::CalibrationData) = {true}; // Seb: Add calib_->getData()...
+	frame.create<ftl::calibration::CalibrationData>(Channel::CalibrationData) = calibration_;
 
 	calibration_change_ = frame.onChange(Channel::CalibrationData, [this]
 			(ftl::data::Frame& frame, ftl::codecs::Channel) {
-		
+
 		auto &change = frame.get<ftl::calibration::CalibrationData>(Channel::CalibrationData);
-		change.toFile(fname_calib_);
-		calib_->setRectify(change.enabled);
-		calib_->setCalibration(change);
+		try {
+			change.writeFile(fname_calib_);
+		}
+		catch (...) {
+			LOG(ERROR) << "Saving calibration to file failed";
+		}
+
+		calibration_ = ftl::calibration::CalibrationData(change);
+		rectification_->setCalibration(calibration_);
+		rectification_->setEnabled(change.enabled);
 		return true;
 	});
 
 	cv::Mat K;
 
 	// same for left and right
-	float baseline = static_cast<float>(calib_->getBaseline());
-	float doff = calib_->getDoff(color_size_);
+	float baseline = static_cast<float>(rectification_->baseline());
+	float doff = rectification_->doff(color_size_);
 
 	double d_resolution = this->host_->getConfig().value<double>("depth_resolution", 0.0);
 	float min_depth = this->host_->getConfig().value<double>("min_depth", 0.45);
 	float max_depth = this->host_->getConfig().value<double>("max_depth", 12.0);
 
 	// left
-	
-	K = calib_->getCameraMatrixLeft(color_size_);
+
+	K = rectification_->cameraMatrix(color_size_);
 	float fx = static_cast<float>(K.at<double>(0,0));
-	
+
 	if (d_resolution > 0.0) {
 		// Learning OpenCV p. 442
 		// TODO: remove, should not be used here
@@ -315,16 +220,17 @@ void StereoVideoSource::updateParameters(ftl::rgbd::Frame &frame) {
 		baseline,
 		doff
 	};
-	
+
 	host_->getConfig()["focal"] = params_.fx;
 	host_->getConfig()["centre_x"] = params_.cx;
 	host_->getConfig()["centre_y"] = params_.cy;
 	host_->getConfig()["baseline"] = params_.baseline;
 	host_->getConfig()["doffs"] = params_.doffs;
-	
+
 	// right
+	// Not used anymore?
 
-	K = calib_->getCameraMatrixRight(color_size_);
+	K = rectification_->cameraMatrix(color_size_, Channel::Right);
 	frame.setRight() = {
 		static_cast<float>(K.at<double>(0,0)),	// Fx
 		static_cast<float>(K.at<double>(1,1)),	// Fy
@@ -346,7 +252,7 @@ bool StereoVideoSource::capture(int64_t ts) {
 
 bool StereoVideoSource::retrieve(ftl::rgbd::Frame &frame) {
 	FTL_Profile("Stereo Retrieve", 0.03);
-	
+
 	if (do_update_params_) {
 		updateParameters(frame);
 		do_update_params_ = false;
@@ -360,7 +266,7 @@ bool StereoVideoSource::retrieve(ftl::rgbd::Frame &frame) {
 	if (lsrc_->isStereo()) {
 		cv::cuda::GpuMat &left = frame.create<cv::cuda::GpuMat>(Channel::Left);
 		cv::cuda::GpuMat &right = frame.create<cv::cuda::GpuMat>(Channel::Right);
-		if (!lsrc_->get(left, right, hres, hres_r, calib_, stream2_)) {
+		if (!lsrc_->get(left, right, hres, hres_r, rectification_.get(), stream2_)) {
 			frame.remove(Channel::Left);
 			frame.remove(Channel::Right);
 		}
@@ -368,7 +274,7 @@ bool StereoVideoSource::retrieve(ftl::rgbd::Frame &frame) {
 	else {
 		cv::cuda::GpuMat &left = frame.create<cv::cuda::GpuMat>(Channel::Left);
 		cv::cuda::GpuMat right;
-		if (!lsrc_->get(left, right, hres, hres_r, calib_, stream2_)) {
+		if (!lsrc_->get(left, right, hres, hres_r, rectification_.get(), stream2_)) {
 			frame.remove(Channel::Left);
 		}
 	}
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp
index 24ff7ef09b37b41ee9d7041974cd7469ac635bd4..db10d8cc5c4bf3c5c0c7b586c5727b390a89e4c8 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp
@@ -4,7 +4,9 @@
 
 #include "../../basesource.hpp"
 #include <ftl/operators/operator.hpp>
+#include <ftl/calibration/structures.hpp>
 #include <string>
+#include <memory>
 
 namespace ftl {
 
@@ -12,15 +14,15 @@ namespace rgbd {
 namespace detail {
 
 class Device;
-class Calibrate;
+class StereoRectification;
 class Disparity;
 
 /**
  * RGBD source from either a stereo video file with left + right images, or
- * direct from two camera devices. 
+ * direct from two camera devices.
  */
 class StereoVideoSource : public BaseSourceImpl {
-	public:
+public:
 	explicit StereoVideoSource(ftl::rgbd::Source*);
 	StereoVideoSource(ftl::rgbd::Source*, const std::string &);
 	~StereoVideoSource();
@@ -31,11 +33,13 @@ class StereoVideoSource : public BaseSourceImpl {
 
 	Camera parameters(ftl::codecs::Channel chan) override;
 
-	private:
+private:
 	void updateParameters(ftl::rgbd::Frame &);
 
 	Device *lsrc_;
-	Calibrate *calib_;
+	std::unique_ptr<StereoRectification> rectification_;
+	ftl::calibration::CalibrationData calibration_;
+
 	int64_t capts_;
 
 	cv::Size color_size_;
@@ -49,12 +53,12 @@ class StereoVideoSource : public BaseSourceImpl {
 
 	bool ready_;
 	bool do_update_params_ = false;
-	
+
 	cv::cuda::Stream stream_;
 	cv::cuda::Stream stream2_;
 
 	cv::Mat mask_l_;
-	
+
 	ftl::Handle calibration_change_;
 	std::string fname_calib_;