diff --git a/applications/calibration-multi/src/main.cpp b/applications/calibration-multi/src/main.cpp
index 4f1f24eb5cfa3cfc3240a27349c2c08a45a29862..ea770f69b03981164c2b6c97f95aa8841f1d8ae7 100644
--- a/applications/calibration-multi/src/main.cpp
+++ b/applications/calibration-multi/src/main.cpp
@@ -98,7 +98,7 @@ bool loadRegistration(const string &ifile, map<string, Eigen::Matrix4d> &data) {
 
 //
 
-bool saveIntrinsics(const string &ofile, const vector<Mat> &M) {
+bool saveIntrinsics(const string &ofile, const vector<Mat> &M, const Size &size) {
 	vector<Mat> D;
 	{
 		cv::FileStorage fs(ofile, cv::FileStorage::READ);
@@ -108,6 +108,7 @@ bool saveIntrinsics(const string &ofile, const vector<Mat> &M) {
 	{
 		cv::FileStorage fs(ofile, cv::FileStorage::WRITE);
 		if (fs.isOpened()) {
+			fs << "resolution" << size;
 			fs << "K" << M << "D" << D;
 			fs.release();
 			return true;
@@ -196,6 +197,7 @@ struct CalibrationParams {
 	bool optimize_intrinsic = false;
 	int reference_camera = -1;
 	double alpha = 0.0;
+	Size size;
 };
 
 void calibrate(	MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
@@ -233,8 +235,8 @@ void calibrate(	MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
 		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, Size(1280, 720), R_c1c2, T_c1c2, R1, R2, P1, P2, Q, 0, params.alpha);
-		
+		cv::stereoRectify(K1, D1, K2, D2, params.size, R_c1c2, T_c1c2, R1, R2, P1, P2, Q, 0, params.alpha);
+
 		Mat _t = Mat(Size(1, 3), CV_64FC1, Scalar(0.0));
 		Rt_out[c] = getMat4x4(R[c], t[c]) * getMat4x4(R1, _t).inv();
 		Rt_out[c + 1] = getMat4x4(R[c + 1], t[c + 1]) * getMat4x4(R2, _t).inv();
@@ -244,15 +246,20 @@ void calibrate(	MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
 			size_t pos1 = uri_cameras[c/2].find("node");
 			size_t pos2 = uri_cameras[c/2].find("#", pos1);
 			node_name = uri_cameras[c/2].substr(pos1, pos2 - pos1);
-			//LOG(INFO) << c << ":" << calib.getCameraMatNormalized(c, 1280, 720);
-			//LOG(INFO) << c + 1 << ":" << calib.getCameraMatNormalized(c + 1, 1280, 720);
+			
 			if (params.save_extrinsic) {
+				// TODO:	only R and T required, rectification performed on vision node,
+				//			consider saving global extrinsic calibration?
 				saveExtrinsics(params.output_path + node_name + "-extrinsic.yml", R_c1c2, T_c1c2, R1, R2, P1, P2, Q);
 				LOG(INFO) << "Saved: " << params.output_path + node_name + "-extrinsic.yml";
 			}
 			if (params.save_intrinsic) {
-				saveIntrinsics(params.output_path + node_name + "-intrinsic.yml",
-					{calib.getCameraMatNormalized(c, 1280, 720), calib.getCameraMatNormalized(c + 1, 1280, 720)}
+				saveIntrinsics(
+					params.output_path + node_name + "-intrinsic.yml",
+					{calib.getCameraMat(c),
+					 calib.getCameraMat(c + 1)},
+					params.size
+
 				);
 				LOG(INFO) << "Saved: " << params.output_path + node_name + "-intrinsic.yml";
 			}
@@ -260,9 +267,9 @@ void calibrate(	MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
 
 		// for visualization
 		Size new_size;
-		cv::stereoRectify(K1, D1, K2, D2, Size(1280, 720), R_c1c2, T_c1c2, R1, R2, P1, P2, Q, 0, 1.0, new_size, &roi[c], &roi[c + 1]);
-		cv::initUndistortRectifyMap(K1, D1, R1, P1, Size(1280, 720), CV_16SC2, map1[c], map2[c]);
-		cv::initUndistortRectifyMap(K2, D2, R2, P2, Size(1280, 720), CV_16SC2, map1[c + 1], map2[c + 1]);
+		cv::stereoRectify(K1, D1, K2, D2, params.size, R_c1c2, T_c1c2, R1, R2, P1, P2, Q, 0, 1.0, new_size, &roi[c], &roi[c + 1]);
+		cv::initUndistortRectifyMap(K1, D1, R1, P1, params.size, CV_16SC2, map1[c], map2[c]);
+		cv::initUndistortRectifyMap(K2, D2, R2, P2, params.size, CV_16SC2, map1[c + 1], map2[c + 1]);
 	}
 
 	{
@@ -298,6 +305,7 @@ void calibrateFromPath(	const string &path,
 	vector<string> uri_cameras;
 	cv::FileStorage fs(path + filename, cv::FileStorage::READ);
 	fs["uri"] >> uri_cameras;
+	fs["resolution"] >> params.size;
 
 	//params.idx_cameras = {2, 3};//{0, 1, 4, 5, 6, 7, 8, 9, 10, 11};
 	params.idx_cameras.resize(uri_cameras.size() * 2);
@@ -362,11 +370,11 @@ void runCameraCalibration(	ftl::Configurable* root,
 	const size_t n_sources = sources.size();
 	const size_t n_cameras = n_sources * 2;
 	size_t reference_camera = 0;
-	Size resolution;
+	
 	{
-		auto params = sources[0]->parameters();
-		resolution = Size(params.width, params.height);
-		LOG(INFO) << "Camera resolution: " << resolution;
+		auto camera = sources[0]->parameters();
+		params.size = Size(camera.width, camera.height);
+		LOG(INFO) << "Camera resolution: " << params.size;
 	}
 
 	params.idx_cameras.resize(n_cameras);
@@ -374,22 +382,22 @@ void runCameraCalibration(	ftl::Configurable* root,
 
 	// TODO: parameter for calibration target type
 	auto calib = MultiCameraCalibrationNew(	n_cameras, reference_camera,
-											resolution, CalibrationTarget(0.250)
+											params.size, CalibrationTarget(0.250)
 	);
 
 	for (size_t i = 0; i < n_sources; i++) {
-		auto params_r = sources[i]->parameters(Channel::Right);
-		auto params_l = sources[i]->parameters(Channel::Left);
+		auto camera_r = sources[i]->parameters(Channel::Right);
+		auto camera_l = sources[i]->parameters(Channel::Left);
 
-		CHECK(resolution == Size(params_r.width, params_r.height));
-		CHECK(resolution == Size(params_l.width, params_l.height));
+		CHECK(params.size == Size(camera_r.width, camera_r.height));
+		CHECK(params.size == Size(camera_l.width, camera_l.height));
 		
 		Mat K;
-		K = getCameraMatrix(params_r);
+		K = getCameraMatrix(camera_r);
 		LOG(INFO) << "K[" << 2 * i + 1 << "] = \n" << K;
 		calib.setCameraParameters(2 * i + 1, K);
 
-		K = getCameraMatrix(params_l);
+		K = getCameraMatrix(camera_l);
 		LOG(INFO) << "K[" << 2 * i << "] = \n" << K;
 		calib.setCameraParameters(2 * i, K);
 	}
@@ -551,7 +559,8 @@ int main(int argc, char **argv) {
 	params.optimize_intrinsic = optimize_intrinsic;
 	params.output_path = output_directory;
 	params.registration_file = registration_file;
-
+	params.reference_camera = ref_camera;
+	
 	LOG(INFO)	<< "\n"
 				<< "\nIMPORTANT: Remeber to set \"use_intrinsics\" to false for nodes!"
 				<< "\n"
diff --git a/applications/calibration-multi/src/multicalibrate.cpp b/applications/calibration-multi/src/multicalibrate.cpp
index 2292d178d70f0f7c68b79cbbf1cb9172cdd173be..9d85d1af7ae938981bb445a852fd85a913c6e26e 100644
--- a/applications/calibration-multi/src/multicalibrate.cpp
+++ b/applications/calibration-multi/src/multicalibrate.cpp
@@ -651,6 +651,7 @@ double MultiCameraCalibrationNew::calibrateAll(int reference_camera) {
 		if (calibratePair(c1, c2, R, t) > 16.0) {
 			LOG(ERROR)	<< "Pairwise calibration failed, skipping cameras "
 						<< c1 << " and " << c2;
+			visibility_graph_.deleteEdge(c1, c2);
 			continue;
 		}
 
diff --git a/applications/calibration-multi/src/visibility.cpp b/applications/calibration-multi/src/visibility.cpp
index bd001f60371d02aa7e5b76a8f060950f05b42db3..a4c16165b47decadfa9ca18ccf641a32b1cb16c1 100644
--- a/applications/calibration-multi/src/visibility.cpp
+++ b/applications/calibration-multi/src/visibility.cpp
@@ -49,6 +49,12 @@ int Visibility::getOptimalCamera() {
 	return best_i;
 }
 
+void Visibility::deleteEdge(int camera1, int camera2)
+{
+	visibility_.at<int>(camera1, camera2) = 0;
+	visibility_.at<int>(camera2, camera1) = 0;
+}
+
 int Visibility::getMinVisibility() {
 	int min_i;
 	int min_count = INT_MAX;
diff --git a/applications/calibration-multi/src/visibility.hpp b/applications/calibration-multi/src/visibility.hpp
index dcf7e71ddb29304c5a0825e310e566ef3622cd55..3521435dd62e2cd6eeea417f926b02f8049eb560 100644
--- a/applications/calibration-multi/src/visibility.hpp
+++ b/applications/calibration-multi/src/visibility.hpp
@@ -27,6 +27,7 @@ public:
 	vector<vector<pair<int, int>>> findShortestPaths(int reference);
 
 	vector<int> getClosestCameras(int c);
+	void deleteEdge(int camera1, int camera2);
 	int getOptimalCamera();
 	int getMinVisibility();
 	int getViewsCount(int camera);
diff --git a/applications/calibration/src/common.cpp b/applications/calibration/src/common.cpp
index 0098bba58c5e6cbcbc0b75185f4286894447f16c..8a678e4b9ba808dccea146c2ce4c8c1c6942a7f0 100644
--- a/applications/calibration/src/common.cpp
+++ b/applications/calibration/src/common.cpp
@@ -62,20 +62,24 @@ bool saveExtrinsics(const string &ofile, Mat &R, Mat &T, Mat &R1, Mat &R2, Mat &
 	return false;
 }
 
-bool saveIntrinsics(const string &ofile, const vector<Mat> &M, const vector<Mat>& D) {
+bool saveIntrinsics(const string &ofile, const vector<Mat> &M, const vector<Mat>& D, const Size &size)
+{
 	cv::FileStorage fs(ofile, cv::FileStorage::WRITE);
-	if (fs.isOpened()) {
+	if (fs.isOpened())
+	{
+		fs << "resolution" << size;
 		fs << "K" << M << "D" << D;
 		fs.release();
 		return true;
 	}
-	else {
+	else
+	{
 		LOG(ERROR) << "Error: can not save the intrinsic parameters to '" << ofile << "'";
 	}
 	return false;
 }
 
-bool loadIntrinsics(const string &ifile, vector<Mat> &K1, vector<Mat> &D1) {
+bool loadIntrinsics(const string &ifile, vector<Mat> &K1, vector<Mat> &D1, Size &size) {
 	using namespace cv;
 
 	FileStorage fs;
@@ -89,9 +93,10 @@ bool loadIntrinsics(const string &ifile, vector<Mat> &K1, vector<Mat> &D1) {
 	
 	LOG(INFO) << "Intrinsics from: " << ifile;
 
+	fs["resolution"] >> size;
 	fs["K"] >> K1;
 	fs["D"] >> D1;
-
+	
 	return true;
 }
 
@@ -211,6 +216,23 @@ bool CalibrationChessboard::findPoints(Mat &img, vector<Vec2f> &points) {
 	return cv::findChessboardCornersSB(img, pattern_size_, points, chessboard_flags_);
 }
 
+
+void CalibrationChessboard::drawCorners(Mat &img, const vector<Vec2f> &points) {
+	using cv::Point2i;
+	vector<Point2i> corners(4);
+	corners[1] = Point2i(points[0]);
+	corners[0] = Point2i(points[pattern_size_.width - 1]);
+	corners[2] = Point2i(points[pattern_size_.width * (pattern_size_.height - 1)]);
+	corners[3] = Point2i(points.back());
+	
+	cv::Scalar color = cv::Scalar(200, 200, 200);
+	
+	for (int i = 0; i <= 4; i++)
+	{
+		cv::line(img, corners[i % 4], corners[(i + 1) % 4], color, 2);
+	}
+}
+
 void CalibrationChessboard::drawPoints(Mat &img, const vector<Vec2f> &points) {
 	cv::drawChessboardCorners(img, pattern_size_, points, true);
 }
diff --git a/applications/calibration/src/common.hpp b/applications/calibration/src/common.hpp
index 80d31f1452b40069c312a650425598e82f976f33..c84f25d249b49eee094e3e898090ffb9ff129f03 100644
--- a/applications/calibration/src/common.hpp
+++ b/applications/calibration/src/common.hpp
@@ -15,8 +15,8 @@ int getOptionInt(const std::map<std::string, std::string> &options, const std::s
 double getOptionDouble(const std::map<std::string, std::string> &options, const std::string &opt, double default_value);
 std::string getOptionString(const std::map<std::string, std::string> &options, const std::string &opt, std::string default_value);
 
-bool loadIntrinsics(const std::string &ifile, std::vector<cv::Mat> &K, std::vector<cv::Mat> &D);
-bool saveIntrinsics(const std::string &ofile, const std::vector<cv::Mat> &K, const std::vector<cv::Mat> &D);
+bool loadIntrinsics(const std::string &ifile, std::vector<cv::Mat> &K, std::vector<cv::Mat> &D, cv::Size &size);
+bool saveIntrinsics(const std::string &ofile, const std::vector<cv::Mat> &K, const std::vector<cv::Mat> &D, const cv::Size &size);
 
 // TODO loadExtrinsics()
 bool saveExtrinsics(const std::string &ofile, cv::Mat &R, cv::Mat &T, cv::Mat &R1, cv::Mat &R2, cv::Mat &P1, cv::Mat &P2, cv::Mat &Q);
@@ -94,6 +94,7 @@ public:
 	void objectPoints(std::vector<cv::Vec3f> &out);
 	bool findPoints(cv::Mat &in, std::vector<cv::Vec2f> &out);
 	void drawPoints(cv::Mat &img, const std::vector<cv::Vec2f> &points);
+	void drawCorners(cv::Mat &img, const std::vector<cv::Vec2f> &points);
 
 private:
 	int chessboard_flags_ = 0;
diff --git a/applications/calibration/src/lens.cpp b/applications/calibration/src/lens.cpp
index f793dbfc6907b79ac65dfcbcf4a921884e6ca5cc..b69b26cc6e4cfec9dab04d337d75b21721d2fbae 100644
--- a/applications/calibration/src/lens.cpp
+++ b/applications/calibration/src/lens.cpp
@@ -14,6 +14,8 @@
 #include <opencv2/highgui.hpp>
 
 #include <vector>
+#include <atomic>
+#include <thread>
 
 using std::map;
 using std::string;
@@ -30,8 +32,8 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 	LOG(INFO) << "Begin intrinsic calibration";
 
 	// TODO PARAMETERS TO CONFIG FILE
-	const Size image_size = Size(	getOptionInt(opt, "width", 1280),
-							getOptionInt(opt, "height", 720));
+	const Size image_size = Size(	getOptionInt(opt, "width", 1920),
+							getOptionInt(opt, "height", 1080));
 	const int n_cameras = getOptionInt(opt, "n_cameras", 2);
 	const int iter = getOptionInt(opt, "iter", 40);
 	const int delay = getOptionInt(opt, "delay", 1000);
@@ -39,18 +41,21 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 	const double aperture_height = getOptionDouble(opt, "aperture_height", 4.6);
 	const string filename_intrinsics = getOptionString(opt, "profile", FTL_LOCAL_CONFIG_ROOT "/intrinsics.yml");
 	CalibrationChessboard calib(opt);
-	const bool use_guess = getOptionInt(opt, "use_guess", 1);
+	bool use_guess = getOptionInt(opt, "use_guess", 1);
+	//bool use_guess_distortion = getOptionInt(opt, "use_guess_distortion", 0);
 
 	LOG(INFO) << "Intrinsic calibration parameters";
-	LOG(INFO) << "         profile: " << filename_intrinsics;
-	LOG(INFO) << "       n_cameras: " << n_cameras;
-	LOG(INFO) << "           width: " << image_size.width;
-	LOG(INFO) << "          height: " << image_size.height;
-	LOG(INFO) << "            iter: " << iter;
-	LOG(INFO) << "           delay: " << delay;
-	LOG(INFO) << "  aperture_width: " << aperture_width;
-	LOG(INFO) << " aperture_height: " << aperture_height;
-	LOG(INFO) << "       use_guess: " << use_guess;
+	LOG(INFO) << "               profile: " << filename_intrinsics;
+	LOG(INFO) << "             n_cameras: " << n_cameras;
+	LOG(INFO) << "                 width: " << image_size.width;
+	LOG(INFO) << "                height: " << image_size.height;
+	LOG(INFO) << "                  iter: " << iter;
+	LOG(INFO) << "                 delay: " << delay;
+	LOG(INFO) << "        aperture_width: " << aperture_width;
+	LOG(INFO) << "       aperture_height: " << aperture_height;
+	LOG(INFO) << "             use_guess: " << use_guess;
+	//LOG(INFO) << "  use_guess_distortion: " << use_guess_distortion;
+
 	LOG(INFO) << "-----------------------------------";
 
 	int calibrate_flags =	cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_ASPECT_RATIO;
@@ -61,18 +66,39 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 
 	vector<Mat> camera_matrix(n_cameras), dist_coeffs(n_cameras);
 
-	if (use_guess) {
+	for (Mat &d : dist_coeffs)
+	{
+		d = Mat(Size(5, 1), CV_64FC1, cv::Scalar(0.0));
+	}
+
+	if (use_guess)
+	{
 		camera_matrix.clear();
 		vector<Mat> tmp;
-		//dist_coeffs.clear();
-		loadIntrinsics(filename_intrinsics, camera_matrix, tmp);
+		Size tmp_size;
+		
+		loadIntrinsics(filename_intrinsics, camera_matrix, tmp, tmp_size);
 		CHECK(camera_matrix.size() == n_cameras); // (camera_matrix.size() == dist_coeffs.size())
+		if ((tmp_size != image_size) && (!tmp_size.empty()))
+		{
+			Mat scale = Mat::eye(Size(3, 3), CV_64FC1);
+			scale.at<double>(0, 0) = ((double) image_size.width) / ((double) tmp_size.width);
+			scale.at<double>(1, 1) = ((double) image_size.height) / ((double) tmp_size.height);
+			for (Mat &K : camera_matrix) { K = scale * K; }
+		}
+
+		if (tmp_size.empty())
+		{
+			use_guess = false;
+			LOG(FATAL) << "No valid calibration found.";
+		}
 	}
 
 	vector<cv::VideoCapture> cameras;
 	cameras.reserve(n_cameras);
 	for (int c = 0; c < n_cameras; c++) { cameras.emplace_back(c); }
-	for (auto &camera : cameras) {
+	for (auto &camera : cameras)
+	{
 		if (!camera.isOpened()) {
 			LOG(ERROR) << "Could not open camera device";
 			return;
@@ -83,41 +109,102 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 
 	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);
+	Mat display(Size(image_size.width * n_cameras, image_size.height), CV_8UC3);
 
-	while (iter > *std::min_element(count.begin(), count.end())) {
+	for (int c = 0; c < n_cameras; c++)
+	{
+		img_display[c] = Mat(display, cv::Rect(c * image_size.width, 0, image_size.width, image_size.height));
+	}
 
-		for (auto &camera : cameras) { camera.grab(); }
+	std::mutex m;
+	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)
+			{
+				std::this_thread::sleep_for(std::chrono::milliseconds(delay));
+				continue;
+			}
 
-		for (int c = 0; c < n_cameras; c++) {
-			vector<Vec2f> points;
-			cameras[c].retrieve(img[c]);
-		
-			if (calib.findPoints(img[c], points)) {
-				calib.drawPoints(img[c], points);
-				count[c]++;
+			m.lock();
+			ready = false;
+			for (int c = 0; c < n_cameras; c++)
+			{
+				img[c].copyTo(tmp[c]);
+			}
+			m.unlock();
+			
+			for (int c = 0; c < n_cameras; c++)
+			{
+				vector<Vec2f> points;
+				if (calib.findPoints(tmp[c], points))
+				{
+					count[c]++;
+				}
+				else { continue; }
+
+				vector<Vec3f> points_ref;
+				calib.objectPoints(points_ref);
+				Mat camera_matrix, dist_coeffs;
+				image_points[c].push_back(points);
+				object_points[c].push_back(points_ref);
 			}
-			else { continue; }
-		
-			vector<Vec3f> points_ref;
-			calib.objectPoints(points_ref);
 
-			Mat camera_matrix, dist_coeffs;
-			vector<Mat> rvecs, tvecs;
-		
-			image_points[c].push_back(points);
-			object_points[c].push_back(points_ref);
+			std::this_thread::sleep_for(std::chrono::milliseconds(delay));
+		}
+	});
+
+	while (iter > *std::min_element(count.begin(), count.end()))
+	{
+		if (m.try_lock())
+		{
+			for (auto &camera : cameras) { camera.grab(); }
+
+			for (int c = 0; c < n_cameras; c++)
+			{
+				cameras[c].retrieve(img[c]);
+			}
+
+			ready = true;
+			m.unlock();
 		}
 		
-		for (int c = 0; c < n_cameras; c++) {
-			cv::imshow("Camera " + std::to_string(c), img[c]);
+		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);
+				}
+
+				calib.drawPoints(img_display[c], image_points[c].back());
+			}
+
+			m.unlock();
 		}
 
-		cv::waitKey(delay);
+		cv::namedWindow("Cameras", cv::WINDOW_KEEPRATIO | cv::WINDOW_NORMAL);
+		cv::imshow("Cameras", display);
+
+		cv::waitKey(10);
 	}
 
-	for (int c = 0; c < n_cameras; c++) {
+	cv::destroyAllWindows();
+
+	for (int c = 0; c < n_cameras; c++)
+	{
 		LOG(INFO) << "Calculating intrinsic paramters for camera " << std::to_string(c);
 		vector<Mat> rvecs, tvecs;
 		
@@ -148,7 +235,7 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 		LOG(INFO) << "";
 	}
 
-	saveIntrinsics(filename_intrinsics, camera_matrix, dist_coeffs);
+	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);
diff --git a/applications/calibration/src/stereo.cpp b/applications/calibration/src/stereo.cpp
index 0d2e1636996438ec9d241f227ff1aab003406f2a..964cf815300971cf31130e78fae94e1c21a172ad 100644
--- a/applications/calibration/src/stereo.cpp
+++ b/applications/calibration/src/stereo.cpp
@@ -93,11 +93,17 @@ void ftl::calibration::stereo(map<string, string> &opt) {
 	
 	vector<Mat> dist_coeffs(2);
 	vector<Mat> camera_matrices(2);
-
-	if (!loadIntrinsics(filename_intrinsics, camera_matrices, dist_coeffs)) {
+	Size intrinsic_resolution;
+	if (!loadIntrinsics(filename_intrinsics, camera_matrices, dist_coeffs, intrinsic_resolution))
+	{
 		LOG(FATAL) << "Failed to load intrinsic camera parameters from file.";
 	}
 	
+	if (intrinsic_resolution != image_size)
+	{
+		LOG(FATAL) << "Intrinsic resolution is not same as input resolution (TODO)";
+	}
+
 	Mat R, T, E, F, per_view_errors;
 	
 	// capture calibration patterns
diff --git a/components/rgbd-sources/src/calibrate.cpp b/components/rgbd-sources/src/calibrate.cpp
index 934ca1dcd221ed725c06cba5b6c921b279456d26..3940520d2374661449c376020cb98c5802e2c674 100644
--- a/components/rgbd-sources/src/calibrate.cpp
+++ b/components/rgbd-sources/src/calibrate.cpp
@@ -78,21 +78,24 @@ bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, s
 		return false;
 	}
 
+
+	cv::Size calib_size;
 	{
 		vector<Mat> K, D;
 		fs["K"] >> K;
 		fs["D"] >> D;
+		fs["resolution"] >> calib_size;
 
-		K[0].copyTo(M1_);
-		K[1].copyTo(M2_);
+		K[0].copyTo(K1_);
+		K[1].copyTo(K2_);
 		D[0].copyTo(D1_);
 		D[1].copyTo(D2_);
 	}
 
 	fs.release();
 
-	CHECK(M1_.size() == Size(3, 3));
-	CHECK(M2_.size() == Size(3, 3));
+	CHECK(K1_.size() == Size(3, 3));
+	CHECK(K2_.size() == Size(3, 3));
 	CHECK(D1_.size() == Size(5, 1));
 	CHECK(D2_.size() == Size(5, 1));
 
@@ -113,38 +116,49 @@ bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, s
 
 	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();
 
 	img_size_ = img_size;
 
-	// TODO: normalize calibration
-	double scale_x = ((double) img_size.width) / 1280.0;
-	double scale_y = ((double) img_size.height) / 720.0;
+	if (calib_size.empty())
+	{
+		LOG(WARNING) << "Calibration resolution missing!";
+	}
+	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;
+		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_;
+	}
 
-	M1_ = scale * M1_;
-	M2_ = scale * M2_;
-	P1_ = scale * P1_;
-	P2_ = scale * P2_;
+	double alpha = value("alpha", 0.0);
+	cv::stereoRectify(K1_, D1_, K2_, D2_, img_size_, R_, T_, R1_, R2_, P1_, P2_, Q_, 0, alpha);
 
+	/* 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(M1_, D1_, R1_, P1_, img_size_, CV_32FC1, map1.first, map2.first);
-	initUndistortRectifyMap(M2_, D2_, R2_, P2_, img_size_, CV_32FC1, map1.second, map2.second);
+	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);
 
 	return true;
 }
@@ -176,17 +190,19 @@ void Calibrate::_updateIntrinsics() {
 		// no rectification
 		R1 = Mat::eye(Size(3, 3), CV_64FC1);
 		R2 = R1;
-		P1 = M1_;
-		P2 = M2_;
+		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()
-	C_l_ = P1;
-	C_r_ = P2;
+	Kl_ = Mat(P1, cv::Rect(0, 0, 3, 3));
+	Kr_ = Mat(P1, cv::Rect(0, 0, 3, 3));
 
-	initUndistortRectifyMap(M1_, D1_, R1, P1, img_size_, CV_32FC1, map1_.first, map2_.first);
-	initUndistortRectifyMap(M2_, D2_, R2, P2, img_size_, CV_32FC1, map1_.second, map2_.second);
+	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);
diff --git a/components/rgbd-sources/src/calibrate.hpp b/components/rgbd-sources/src/calibrate.hpp
index 7f6b262c1e7afcd2852f65a6d62333b5389f7615..4561b90a79129dd5a0d46d9d54bd005147d766a7 100644
--- a/components/rgbd-sources/src/calibrate.hpp
+++ b/components/rgbd-sources/src/calibrate.hpp
@@ -54,8 +54,9 @@ class Calibrate : public ftl::Configurable {
 	 * a 3D point cloud.
 	 */
 	const cv::Mat &getQ() const { return Q_; }
-	const cv::Mat &getCameraMatrixLeft() { return C_l_; }
-	const cv::Mat &getCameraMatrixRight() { return C_r_; }
+
+	const cv::Mat &getCameraMatrixLeft() { return Kl_; }
+	const cv::Mat &getCameraMatrixRight() { return Kr_; }
 	const cv::Mat &getCameraMatrix() { return getCameraMatrixLeft(); }
 
 private:
@@ -70,12 +71,17 @@ private:
 	std::pair<cv::cuda::GpuMat, cv::cuda::GpuMat> map1_gpu_;
 	std::pair<cv::cuda::GpuMat, cv::cuda::GpuMat> map2_gpu_;
 
-	cv::Mat Q_;
+	// parameters for rectification, see cv::stereoRectify() documentation
 	cv::Mat R_, T_, R1_, P1_, R2_, P2_;
-	cv::Mat M1_, D1_, M2_, D2_;
 
-	cv::Mat C_l_;
-	cv::Mat C_r_;
+	// 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_;
 };
diff --git a/config/config_vision.jsonc b/config/config_vision.jsonc
new file mode 100644
index 0000000000000000000000000000000000000000..b73446cefe06aaaf3663b8fe2823822878b5a1a8
--- /dev/null
+++ b/config/config_vision.jsonc
@@ -0,0 +1,129 @@
+{
+	//"$id": "ftl://utu.fi",
+	"$schema": "",
+	"calibrations": {
+		"default": {
+			"use_intrinsics": true,
+			"use_extrinsics": true,
+			"alpha": 0.0
+		}
+	},
+	
+	"disparity": {
+		"libsgm": {
+			"algorithm": "libsgm",
+			"width": 1280,
+			"height": 720,
+			"use_cuda": true,
+			"minimum": 0,
+			"maximum": 256,
+			"tau": 0.0,
+			"gamma": 0.0,
+			"window_size": 5,
+			"sigma": 1.5,
+			"lambda": 8000.0,
+			"uniqueness":  0.65,
+			"use_filter": true,
+			"P1": 8,
+			"P2": 130,
+			"filter_radius": 11,
+			"filter_iter": 2,
+			"use_off": true,
+			"off_size": 24,
+			"off_threshold": 0.75,
+			"T": 60,
+			"T_add": 0,
+			"T_del": 25,
+			"T_x" : 3.0,
+			"alpha" : 0.6,
+			"beta" : 1.7,
+			"epsilon" : 15.0
+		},
+		
+		"rtcensus": {
+			"algorithm": "rtcensus",
+			"use_cuda": true,
+			"minimum": 0,
+			"maximum": 256,
+			"tau": 0.0,
+			"gamma": 0.0,
+			"window_size": 5,
+			"sigma": 1.5,
+			"lambda": 8000.0,
+			"use_filter": true,
+			"filter_radius": 3,
+			"filter_iter": 4	
+		}
+	},
+	
+	"sources": {
+		"stereocam": {
+			"uri": "device:video",
+			"feed": {
+				"flip": false,
+				"nostereo": false,
+				"scale": 1.0,
+				"flip_vert": false,
+				"max_fps": 500,
+				"width": 1280,
+				"height": 720,
+				"crosshair": false
+			},
+			"use_optflow" : true,
+			"calibration": { "$ref": "#calibrations/default" },
+			"disparity": { "$ref": "#disparity/libsgm" }
+		},
+		"stereovid": {},
+		"localhost": {}
+		
+	},
+	
+	"vision_default": {
+		"fps": 20,
+		"source": { "$ref": "#sources/stereocam" },
+		"middlebury": { "$ref": "#middlebury/none" },
+		"display": { "$ref": "#displays/none" },
+		"net": { "$ref": "#net/default_vision" },
+		"stream": { }
+	},
+	
+	// Listen to localhost
+	"net": {
+		"default_vision": {
+			"listen": "tcp://*:9001",
+			"peers": [],
+			"tcp_send_buffer": 100000 //204800
+		},
+		"default_reconstruct": {
+			"listen": "tcp://*:9002",
+			"peers": []
+		}
+	},
+	
+	"displays": {
+		"none": {
+			"flip_vert": false,
+			"disparity": false,
+			"points": false,
+			"depth": false,
+			"left": false,
+			"right": false
+		},
+		"left": {
+			"flip_vert": false,
+			"disparity": false,
+			"points": false,
+			"depth": false,
+			"left": true,
+			"right": false
+		}
+	},
+	
+	"middlebury": {
+		"none": {
+			"dataset": "",
+			"threshold": 10.0,
+			"scale": 0.25
+		}
+	}
+}