diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index c6173a686d387b22961dcd2ee6b90c120dc39631..1b8b8dba134e35b36d87ff5de03a88513a27717c 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -4,15 +4,14 @@
 #
 # Perhaps relevant in future https://gitlab.com/gitlab-org/gitlab-ce/issues/47063
 
+variables:
+  CMAKE_ARGS_WINDOWS: '-DCMAKE_GENERATOR_PLATFORM=x64 -DNVPIPE_DIR="D:/Build/NvPipe" -DEigen3_DIR="C:/Program Files (x86)/Eigen3/share/eigen3/cmake" -DOpenCV_DIR="D:/Build/opencv-4.1.1" -DCUDA_TOOLKIT_ROOT_DIR="C:/Program Files/NVIDIA GPU Computing Toolkit/CUDA/v10.1"'
+
 stages:
  - all
-# - build
-# - test
  - deploy
 
-#cache:
-#  paths:
-#    - build/
+#### Linux
 
 linux:
   stage: all
@@ -43,20 +42,40 @@ webserver-deploy:
     - rsync -vr --delete web-service/ nodejs@${NODE_SERVER}:/srv/nodejs/web-service
     - ssh nodejs@${NODE_SERVER} -- "npm install web-service/server && pm2 restart web-service"
 
-windows:
-  stage: all
-  variables:
-    CMAKE_ARGS: '-DWITH_OPTFLOW=TRUE -DWITH_PCL=FALSE -DCMAKE_GENERATOR_PLATFORM=x64 -DNVPIPE_DIR="D:/Build/NvPipe" -DEigen3_DIR="C:/Program Files (x86)/Eigen3/share/eigen3/cmake" -DOpenCV_DIR="D:/Build/opencv-4.1.1" -DCUDA_TOOLKIT_ROOT_DIR="C:/Program Files/NVIDIA GPU Computing Toolkit/CUDA/v10.1"'
-    DEPLOY_DIR: 'D:/Shared/AutoDeploy'
-  tags:
-    - win
-  script:
+### Windows
+
+.build-windows: &build-windows
     - 'call "C:/Program Files (x86)/Microsoft Visual Studio/2017/Community/VC/Auxiliary/Build/vcvars64.bat"'
     - mkdir build
     - cd build
-    - cmake %CMAKE_ARGS% ..
+    - echo cmake %CMAKE_ARGS% %CMAKE_ARGS_WINDOWS% ..
+    - cmake %CMAKE_ARGS% %CMAKE_ARGS_WINDOWS% ..
     - devenv ftl.utu.fi.sln /build Release
     - rmdir /q /s "%DEPLOY_DIR%/%CI_COMMIT_REF_SLUG%"
     - mkdir "%DEPLOY_DIR%/%CI_COMMIT_REF_SLUG%"
     - 'copy "applications\vision\Release\ftl-vision.exe" "%DEPLOY_DIR%\%CI_COMMIT_REF_SLUG%"'
-    - 'copy "applications\calibration\Release\ftl-calibrate.exe" "%DEPLOY_DIR%\%CI_COMMIT_REF_SLUG%"'
\ No newline at end of file
+    - 'copy "applications\calibration\Release\ftl-calibrate.exe" "%DEPLOY_DIR%\%CI_COMMIT_REF_SLUG%"'
+
+windows-vision:
+  except:
+    - master
+  stage: all
+  variables:
+    CMAKE_ARGS: '-DWITH_OPTFLOW=TRUE -DBUILD_VISION=TRUE -DBUILD_CALIBRATION=FALSE -DBUILDRECONSTRUCT=FALSE -DBUILDRENDERER=FALSE -DBUILD_TESTING=FALSE'
+    DEPLOY_DIR: 'D:/Shared/AutoDeploy'
+  tags:
+    - win
+  script:
+    - *build-windows
+
+windows-master:
+  only:
+    - master
+  stage: all
+  variables:
+    CMAKE_ARGS: '-DWITH_OPTFLOW=TRUE'
+    DEPLOY_DIR: 'D:/Shared/AutoDeploy'
+  tags:
+    - win
+  script:
+    - *build-windows
diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
index 88d69ab9c6d2fdc23bcd8f629df3e8041d6a75d9..b2e1886bbcc0e75750099fd48f6cade7b48c1f1d 100644
--- a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
@@ -13,6 +13,7 @@
 #include <ctime>
 
 #include "calibrate.hpp"
+#include "ftl/exception.hpp"
 
 #include <opencv2/core.hpp>
 #include <opencv2/core/utility.hpp>
@@ -43,26 +44,23 @@ using std::string;
 using std::vector;
 
 Calibrate::Calibrate(nlohmann::json &config, cv::Size image_size, cv::cuda::Stream &stream) : ftl::Configurable(config) {
-	std::pair<Mat, Mat> map1, map2;
-	calibrated_ = _loadCalibration(image_size, map1, map2);
-
-	if (calibrated_) {
-		_updateIntrinsics();
-		LOG(INFO) << "Calibration loaded from file";
-	}
-	else {
-		LOG(WARNING) << "Calibration not loaded";
+	if (!_loadCalibration()) {
+		throw ftl::exception("Loading calibration failed");
 	}
+	_calculateRectificationParameters(image_size);
+
+	LOG(INFO) << "Calibration loaded from file";
+	rectify_ = value("rectify", true);;
 
 	this->on("use_intrinsics", [this](const ftl::config::Event &e) {
-		_updateIntrinsics();
+		rectify_ = value("rectify", true);
 	});
 }
 
-bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, std::pair<Mat, Mat> &map2) {
+bool Calibrate::_loadCalibration() {
 	FileStorage fs;
 
-	// reading intrinsic parameters
+	// read intrinsic parameters
 	auto ifile = ftl::locateFile(value("intrinsics", std::string("intrinsics.yml")));
 	if (ifile) {
 		fs.open((*ifile).c_str(), FileStorage::READ);
@@ -78,131 +76,90 @@ bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, s
 		return false;
 	}
 
+	fs["K"] >> K_;
+	fs["D"] >> D_;
+	fs["resolution"] >> calib_size_;
 
-	cv::Size calib_size;
-	{
-		vector<Mat> K, D;
-		fs["K"] >> K;
-		fs["D"] >> D;
-		fs["resolution"] >> calib_size;
-
-		K[0].copyTo(K1_);
-		K[1].copyTo(K2_);
-		D[0].copyTo(D1_);
-		D[1].copyTo(D2_);
+	if ((K_.size() != 2) || (D_.size() != 2)) {
+		LOG(ERROR) << "Not enough intrinsic paramters, expected 2";
+		return false;
 	}
 
 	fs.release();
 
-	CHECK(K1_.size() == Size(3, 3));
-	CHECK(K2_.size() == Size(3, 3));
-	CHECK(D1_.size() == Size(5, 1));
-	CHECK(D2_.size() == Size(5, 1));
+	if (calib_size_.empty()) {
+		LOG(ERROR) << "Calibration resolution missing";
+		return false;
+	}
 
+	for (const Mat &K : K_) {
+		if (K.size() != Size(3, 3)) {
+			LOG(ERROR) << "Invalid intrinsic parameters";
+			return false;
+		}
+	}
+	for (const Mat &D : D_) {
+		if (D.size() != Size(5, 1)) {
+			LOG(ERROR) << "Invalid intrinsic parameters";
+			return false;
+		}
+	}
+
+	// read extrinsic parameters
 	auto efile = ftl::locateFile(value("extrinsics", std::string("extrinsics.yml")));
 	if (efile) {
 		fs.open((*efile).c_str(), FileStorage::READ);
 		if (!fs.isOpened()) {
-			LOG(WARNING) << "Could not open extrinsics file";
+			LOG(ERROR) << "Could not open extrinsics file";
 			return false;
 		}
 
 		LOG(INFO) << "Extrinsics from: " << *efile;
 	}
 	else {
-		LOG(WARNING) << "Calibration extrinsics file not found";
+		LOG(ERROR) << "Calibration extrinsics file not found";
 		return false;
 	}
 
 	fs["R"] >> R_;
 	fs["T"] >> T_;
-	
-	/* re-calculate rectification from camera parameters
-	fs["R1"] >> R1_;
-	fs["R2"] >> R2_;
-	fs["P1"] >> P1_;
-	fs["P2"] >> P2_;
-	fs["Q"] >> Q_;
-	*/
-	fs.release();
+	fs["pose"] >> pose_;
 
-	img_size_ = img_size;
+	if (pose_.size() != Size(4, 4)) {
+		LOG(ERROR) << "Pose not in calibration (using identity)";
+		// TODO: return false (raises exception in constructor)
+		//		 use config option to make pose optional (and not return false)
 
-	if (calib_size.empty())
-	{
-		LOG(WARNING) << "Calibration resolution missing!";
+		pose_ = Mat::eye(Size(4, 4), CV_64FC1);
 	}
-	else
-	{
-		double scale_x = ((double) img_size.width) / ((double) calib_size.width);
-		double scale_y = ((double) img_size.height) / ((double) calib_size.height);
-	
-		Mat scale(cv::Size(3, 3), CV_64F, 0.0);
-		scale.at<double>(0, 0) = scale_x;
-		scale.at<double>(1, 1) = scale_y;
-		scale.at<double>(2, 2) = 1.0;
 
-		K1_ = scale * K1_;
-		K2_ = scale * K2_;
-	}
-
-	double alpha = value("alpha", 0.0);
-	cv::stereoRectify(K1_, D1_, K2_, D2_, img_size_, R_, T_, R1_, R2_, P1_, P2_, Q_, 0, alpha);
+	if ((R_.size() != Size(3, 3)) ||
+		(T_.size() != Size(1, 3))) {
 
-	/* scaling not required as rectification is performed from scaled values
-	Q_.at<double>(0, 3) = Q_.at<double>(0, 3) * scale_x;
-	Q_.at<double>(1, 3) = Q_.at<double>(1, 3) * scale_y;
-	Q_.at<double>(2, 3) = Q_.at<double>(2, 3) * scale_x; // TODO: scaling?
-	Q_.at<double>(3, 3) = Q_.at<double>(3, 3) * scale_x;
-	*/
-
-	// cv::cuda::remap() works only with CV_32FC1
-	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);
+		LOG(ERROR) << "Invalid extrinsic parameters";
+		return false;
+	}
+	fs.release();
 
 	return true;
 }
 
-void Calibrate::updateCalibration(const ftl::rgbd::Camera &p) {
-	Q_.at<double>(3, 2) = 1.0 / p.baseline;
-	Q_.at<double>(2, 3) = p.fx;
-	Q_.at<double>(0, 3) = p.cx;
-	Q_.at<double>(1, 3) = p.cy;
-
-	// FIXME:(Nick) Update camera matrix also...
-	_updateIntrinsics();
-}
-
-void Calibrate::_updateIntrinsics() {
-	// TODO: pass parameters?
-
-	Mat R1, R2, P1, P2;
-	ftl::rgbd::Camera params();
-
-	if (this->value("use_intrinsics", true)) {
-		// rectify
-		R1 = R1_;
-		R2 = R2_;
-		P1 = P1_;
-		P2 = P2_;
-	}
-	else {
-		// no rectification
-		R1 = Mat::eye(Size(3, 3), CV_64FC1);
-		R2 = R1;
-		P1 = Mat::zeros(Size(4, 3), CV_64FC1);
-		P2 = Mat::zeros(Size(4, 3), CV_64FC1);
-		K1_.copyTo(Mat(P1, cv::Rect(0, 0, 3, 3)));
-		K2_.copyTo(Mat(P2, cv::Rect(0, 0, 3, 3)));
-	}
-
-	// Set correct camera matrices for
-	// getCameraMatrix(), getCameraMatrixLeft(), getCameraMatrixRight()
-	Kl_ = Mat(P1, cv::Rect(0, 0, 3, 3));
-	Kr_ = Mat(P2, cv::Rect(0, 0, 3, 3));
+void Calibrate::_calculateRectificationParameters(Size img_size) {
+	
+	img_size_ = img_size;
+	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);
 
-	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);
+	cv::stereoRectify(	K1, D1, K2, D2,
+						img_size, R_, T_,
+						R1_, R2_, P1_, P2_, Q_, 0, alpha);
+	
+	// TODO use fixed point maps for CPU (gpu remap() requires floating point)
+	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);
@@ -211,29 +168,19 @@ void Calibrate::_updateIntrinsics() {
 	map2_gpu_.second.upload(map2_.second);
 }
 
-cv::Mat Calibrate::getCameraMatrixLeft(const cv::Size res) {
-	double scale_x = ((double) res.width) / ((double) img_size_.width);
-	double scale_y = ((double) res.height) / ((double) img_size_.height);
-	Mat scale(cv::Size(3, 3), CV_64F, 0.0);
-	scale.at<double>(0, 0) = scale_x;
-	scale.at<double>(1, 1) = scale_y;
-	scale.at<double>(2, 2) = 1.0;
-	return scale * Kl_;
-}
+void Calibrate::updateCalibration(const ftl::rgbd::Camera &p) {
+	Q_.at<double>(3, 2) = 1.0 / p.baseline;
+	Q_.at<double>(2, 3) = p.fx;
+	Q_.at<double>(0, 3) = p.cx;
+	Q_.at<double>(1, 3) = p.cy;
 
-cv::Mat Calibrate::getCameraMatrixRight(const cv::Size res) {
-	double scale_x = ((double) res.width) / ((double) img_size_.width);
-	double scale_y = ((double) res.height) / ((double) img_size_.height);
-	Mat scale(cv::Size(3, 3), CV_64F, 0.0);
-	scale.at<double>(0, 0) = scale_x;
-	scale.at<double>(1, 1) = scale_y;
-	scale.at<double>(2, 2) = 1.0;
-	return scale * Kr_;
+	// FIXME:(Nick) Update camera matrix also...
 }
 
 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);
@@ -244,20 +191,49 @@ void Calibrate::rectifyStereo(GpuMat &l, GpuMat &r, Stream &stream) {
 }
 
 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());
+}
 
-	/*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;*/
+static Mat scaleCameraIntrinsics(Mat K, Size size_new, Size size_old) {
+	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);
+
+	S.at<double>(0, 0) = scale_x;
+	S.at<double>(1, 1) = scale_y;
+	S.at<double>(2, 2) = 1.0;
+	return S * K;
+}
+
+Mat Calibrate::_getK(size_t idx, Size size) {
+	CHECK(idx < K_.size());
+	CHECK(!size.empty());
+	return scaleCameraIntrinsics(K_[idx], size, calib_size_);
 }
 
-bool Calibrate::isCalibrated() {
-	return calibrated_;
-}
\ No newline at end of file
+Mat Calibrate::_getK(size_t idx) {
+	return _getK(idx, img_size_);
+}
+
+cv::Mat Calibrate::getCameraMatrixLeft(const cv::Size res) {
+	Mat M;
+	if (rectify_) {
+		M = Mat(P1_, cv::Rect(0, 0, 3, 3));
+	} else {
+		M = K_[0];
+	}
+	return scaleCameraIntrinsics(M, res, img_size_);
+}
+
+cv::Mat Calibrate::getCameraMatrixRight(const cv::Size res) {
+	Mat M;
+	if (rectify_) {
+		M = Mat(P2_, cv::Rect(0, 0, 3, 3));
+	} else {
+		M = K_[1];
+	}
+	return scaleCameraIntrinsics(M, res, img_size_);
+}
diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
index 39ec301a98d8419cc19dfb0bb60747e20a6327ff..82a06e756591ced2fe11535011b7d422320b7597 100644
--- a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
@@ -22,64 +22,100 @@ namespace rgbd {
 namespace detail {
 
 /**
- * Manage local calibration details: undistortion, rectification and camera parameters.
- * Calibration parameters can be acquired using ftl-calibrate app.
+ * 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);
-	
-	/**
-	 * Get both left and right images from local source, but with intrinsic
-	 * and extrinsic stereo calibration already applied.
-	 */
-	bool rectified(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::Stream &stream);
 
-	/**
-	 * Rectify and remove distortions from from images l and r using cv::remap()
+	/* @brief	Rectify and undistort stereo pair images (GPU)
 	 */
 	void rectifyStereo(cv::cuda::GpuMat &l, cv::cuda::GpuMat &r, cv::cuda::Stream &stream);
 
-	/**
-	 * Rectify and remove distortions from from images l and r using cv::remap()
+	/* @brief	Rectify and undistort stereo pair images (CPU)
+	 * @todo	Uses same rectification maps as GPU version, according to OpenCV
+	 * 			documentation for remap(), fixed point versions faster for CPU
 	 */
 	void rectifyStereo(cv::Mat &l, cv::Mat &r);
 
-	bool isCalibrated();
-
 	void updateCalibration(const ftl::rgbd::Camera &p);
 	
-	// Get disparity to depth matrix.
+	/* @brief Get disparity to depth matrix
+	 *
+	 * 2020/01/15:	Not used, StereoVideoSource creates a Camera object which
+	 * 				is used to calculate depth from disparity (disp2depth.cu)
+	 */
 	const cv::Mat &getQ() const { return Q_; }
 
+	/* @brief	Get intrinsic paramters for rectified camera
+	 * @param	Camera resolution
+	 */
 	cv::Mat getCameraMatrixLeft(const cv::Size res);
 	cv::Mat getCameraMatrixRight(const cv::Size res);
 
+	/* @brief	Get camera pose from calibration
+	 */
+	cv::Mat getPose() { return pose_; };
+	
+	/* @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.
+	 * @param	Rectification on/off
+	 */
+	void setRectify(bool enabled) { rectify_ = enabled; }
+
 private:
-	void _updateIntrinsics();
-	bool _loadCalibration(cv::Size img_size, std::pair<cv::Mat, cv::Mat> &map1, std::pair<cv::Mat, cv::Mat> &map2);
+	// rectification enabled/disabled
+	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);
+
+	/* @brief	Calculate rectification parameters and maps
+	 * @param	Camera resolution
+	 */
+	void _calculateRectificationParameters(cv::Size img_size);
+
+	/* @brief	Load calibration from file
+	 * @todo	File names as arguments
+	 */
+	bool _loadCalibration();
 	
-	private:
-	bool calibrated_;
+	// calibration resolution (loaded from file by _loadCalibration)
+	cv::Size calib_size_;
+	// camera resolution (set by _calculateRecitificationParameters)
+	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_;
 
+	// transformation from left to right camera: R_ and T_
+	cv::Mat R_;
+	cv::Mat T_;
+	// pose for left camera
+	cv::Mat pose_;
+
 	// parameters for rectification, see cv::stereoRectify() documentation
-	cv::Mat R_, T_, R1_, P1_, R2_, P2_;
+	cv::Mat R1_;
+	cv::Mat P1_;
+	cv::Mat R2_;
+	cv::Mat P2_;
 
 	// disparity to depth matrix
 	cv::Mat Q_;
 	
-	// intrinsic paramters and distortion coefficients
-	cv::Mat K1_, D1_, K2_, D2_;
-
-	cv::Mat Kl_;
-	cv::Mat Kr_;
-
-	cv::Size img_size_;
+	// intrinsic parameters and distortion coefficients
+	std::vector<cv::Mat> K_;
+	std::vector<cv::Mat> D_;
 };
 
 }
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
index 8858726c8672a4286eb5fb98a397858d81d3d069..330a5035c46fd38cc4f1ea9ccb6b61bbc12828d8 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -96,8 +96,7 @@ void StereoVideoSource::init(const string &file) {
 	pipeline_depth_->append<ftl::operators::AggreMLS>("mls");  // Perform MLS (using smoothing channel)*/
 
 	calib_ = ftl::create<Calibrate>(host_, "calibration", cv::Size(lsrc_->fullWidth(), lsrc_->fullHeight()), stream_);
-	if (!calib_->isCalibrated()) LOG(WARNING) << "Cameras are not calibrated!";
-
+	
 	// Generate camera parameters from camera matrix
 	cv::Mat K = calib_->getCameraMatrixLeft(color_size_);
 	params_ = {
@@ -121,7 +120,10 @@ void StereoVideoSource::init(const string &file) {
 	host_->getConfig()["baseline"] = params_.baseline;
 	host_->getConfig()["doffs"] = params_.doffs;
 
-	// Add event handlers to allow calibration changes...
+	// TODO: remove (not used, fx/fy/baseline/.. do not change)
+	//		 in case they are modified, update using Calibrate
+	//		 (requires new method)
+
 	host_->on("baseline", [this](const ftl::config::Event &e) {
 		params_.baseline = host_->value("baseline", params_.baseline);
 		UNIQUE_LOCK(host_->mutex(), lk);
@@ -134,13 +136,15 @@ void StereoVideoSource::init(const string &file) {
 		UNIQUE_LOCK(host_->mutex(), lk);
 		calib_->updateCalibration(params_);
 	});
+	//
 
 	host_->on("doffs", [this](const ftl::config::Event &e) {
 		params_.doffs = host_->value("doffs", params_.doffs);
 	});
 	
+
 	// left and right masks (areas outside rectified images)
-	// only left mask used (not used)
+	// TODO: remove mask
 	cv::cuda::GpuMat mask_r_gpu(lsrc_->height(), lsrc_->width(), CV_8U, 255);
 	cv::cuda::GpuMat mask_l_gpu(lsrc_->height(), lsrc_->width(), CV_8U, 255);
 	calib_->rectifyStereo(mask_l_gpu, mask_r_gpu, stream_);
@@ -149,6 +153,7 @@ void StereoVideoSource::init(const string &file) {
 	mask_l_gpu.download(mask_l);
 	mask_l_ = (mask_l == 0);
 	
+
 	LOG(INFO) << "StereoVideo source ready...";
 	ready_ = true;
 }
@@ -162,7 +167,7 @@ ftl::rgbd::Camera StereoVideoSource::parameters(Channel chan) {
 		K = calib_->getCameraMatrixLeft(color_size_);
 	}
 
-	// TODO: remove hardcoded values (min/max)
+	// TODO: remove hardcoded values (min/max), move to Calibrate?
 	ftl::rgbd::Camera params = {
 		K.at<double>(0,0),	// Fx
 		K.at<double>(1,1),	// Fy