From a3b93a4c9275f691af3250f0dfb971fed0d91412 Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Mon, 27 Jan 2020 11:32:53 +0200
Subject: [PATCH] refactored calibartion and msgpack for cv::Mat

---
 applications/calibration-multi/src/main.cpp   | 162 ++++++++++++------
 .../calibration-multi/src/multicalibrate.cpp  |   6 +-
 applications/reconstruct/CMakeLists.txt       |   1 -
 .../reconstruct/include/ftl/registration.hpp  |  22 ---
 applications/reconstruct/src/main.cpp         |  33 +---
 applications/reconstruct/src/registration.cpp |  69 --------
 .../codecs/include/ftl/codecs/channels.hpp    |   2 +-
 .../codecs/include/ftl/codecs/codecs.hpp      |   2 +-
 .../codecs/include/ftl/codecs/packet.hpp      |   3 +-
 .../codecs/include/ftl/codecs/reader.hpp      |   2 +-
 .../codecs/include/ftl/codecs/writer.hpp      |   2 +-
 .../cpp/include/ftl/utility/msgpack.hpp       | 126 ++++++++++++++
 .../include/ftl/utility}/msgpack_optional.hpp |   0
 components/common/cpp/test/CMakeLists.txt     |  10 +-
 components/common/cpp/test/msgpack_unit.cpp   |  88 ++++++++++
 .../net/cpp/include/ftl/net/dispatcher.hpp    |   5 +-
 components/net/cpp/include/ftl/net/peer.hpp   |   2 +
 .../net/cpp/include/ftl/net/universe.hpp      |   4 +-
 .../net/cpp/include/ftl/net/ws_internal.hpp   |   2 +-
 components/net/cpp/include/ftl/uuid.hpp       |   3 +-
 .../rgbd-sources/include/ftl/rgbd/camera.hpp  |   2 +-
 .../src/sources/stereovideo/calibrate.cpp     |  94 +++++++---
 .../src/sources/stereovideo/calibrate.hpp     |  31 +++-
 .../src/sources/stereovideo/stereovideo.cpp   |  97 ++++++-----
 components/streams/src/parsers.cpp            |   8 +-
 25 files changed, 495 insertions(+), 281 deletions(-)
 delete mode 100644 applications/reconstruct/include/ftl/registration.hpp
 delete mode 100644 applications/reconstruct/src/registration.cpp
 create mode 100644 components/common/cpp/include/ftl/utility/msgpack.hpp
 rename components/{net/cpp/include => common/cpp/include/ftl/utility}/msgpack_optional.hpp (100%)
 create mode 100644 components/common/cpp/test/msgpack_unit.cpp

diff --git a/applications/calibration-multi/src/main.cpp b/applications/calibration-multi/src/main.cpp
index d8f20041f..0b84d6449 100644
--- a/applications/calibration-multi/src/main.cpp
+++ b/applications/calibration-multi/src/main.cpp
@@ -44,7 +44,10 @@ using ftl::rgbd::Source;
 using ftl::codecs::Channel;
 
 Mat createCameraMatrix(const ftl::rgbd::Camera &parameters) {
-	Mat m = (cv::Mat_<double>(3,3) << parameters.fx, 0.0, -parameters.cx, 0.0, parameters.fy, -parameters.cy, 0.0, 0.0, 1.0);
+	Mat m = (cv::Mat_<double>(3,3) <<
+				parameters.fx,	0.0,			-parameters.cx,
+				0.0, 			parameters.fy,	-parameters.cy,
+				0.0,			0.0,			 1.0);
 	return m;
 }
 
@@ -118,36 +121,28 @@ void visualizeCalibration(	MultiCameraCalibrationNew &calib, Mat &out,
 ////////////////////////////////////////////////////////////////////////////////
 // RPC
 ////////////////////////////////////////////////////////////////////////////////
+// Using Mat directly
+
+vector<Mat> getDistortionParametersRPC(ftl::net::Universe* net, ftl::stream::Net* nstream) {
+	return net->call<vector<Mat>>(nstream->getPeer(), "get_distortion");
+}
 
 bool setRectifyRPC(ftl::net::Universe* net, ftl::stream::Net* nstream, bool enabled) {
 	return net->call<bool>(nstream->getPeer(), "set_rectify", enabled);
 }
 
 bool setIntrinsicsRPC(ftl::net::Universe* net, ftl::stream::Net* nstream, Size size, vector<Mat> K, vector<Mat> D) {
-	Mat K0 = K[0].t();
-	Mat K1 = K[1].t();
-	return net->call<bool>(	nstream->getPeer(), "set_intrinsics",
-							vector<int>{size.width, size.height},
-							vector<double>(K0.begin<double>(), K0.end<double>()),
-							vector<double>(D[0].begin<double>(), D[0].end<double>()),
-							vector<double>(K1.begin<double>(), K1.end<double>()),
-							vector<double>(D[1].begin<double>(), D[1].end<double>())
-	);
+	return true;
+	return net->call<bool>(nstream->getPeer(), "set_intrinsics",
+							size, K[0], D[0], K[1], D[1] );
 }
 
 bool setExtrinsicsRPC(ftl::net::Universe* net, ftl::stream::Net* nstream, Mat R, Mat t) {
-	Mat rvec;
-	cv::Rodrigues(R, rvec);
-	return net->call<bool>(	nstream->getPeer(), "set_extrinsics",
-							vector<double>(rvec.begin<double>(), rvec.end<double>()),
-							vector<double>(t.begin<double>(), t.end<double>())
-	);
+	return net->call<bool>(nstream->getPeer(), "set_extrinsics", R, t);
 }
 
 bool setPoseRPC(ftl::net::Universe* net, ftl::stream::Net* nstream, Mat pose) {
-	Mat P = pose.t();
-	return net->call<bool>(	nstream->getPeer(), "set_pose", 
-							vector<double>(P.begin<double>(), P.end<double>()));
+	return net->call<bool>(nstream->getPeer(), "set_pose",  pose);
 }
 
 bool saveCalibrationRPC(ftl::net::Universe* net, ftl::stream::Net* nstream) {
@@ -199,26 +194,43 @@ void calibrateRPC(	ftl::net::Universe* net,
 		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);
 
+		R_c1c2 = R_c1c2.clone();
+		T_c1c2 = T_c1c2.clone();
+
 		// calculate extrinsics from rectified parameters
 		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();
 
+		LOG(INFO) << K1;
+		LOG(INFO) << K2;
+		LOG(INFO) << R_c1c2;
+		LOG(INFO) << T_c1c2;
+		LOG(INFO) << "R.isContinuous(): " << R_c1c2.isContinuous ();
+		LOG(INFO) << "R.step: " << R_c1c2.step;
+		LOG(INFO) << "R.cols: " << R_c1c2.cols;
+		LOG(INFO) << "T.isContinuous(): " << T_c1c2.isContinuous ();
+		LOG(INFO) << "T.step: " << T_c1c2.step;
+		LOG(INFO) << "T.cols: " << T_c1c2.cols;
+		LOG(INFO) << "--------------------------------------------------------";
+
 		auto *nstream = nstreams[c/2];
 		while(true) {
 			try {
-				setIntrinsicsRPC(net, nstream,
-					params.size,
-					vector<Mat>{calib.getCameraMat(c), calib.getCameraMat(c+1)},
-					vector<Mat>{calib.getDistCoeffs(c), calib.getDistCoeffs(c+1)}
-				);
+				if (params.optimize_intrinsic) {
+					setIntrinsicsRPC(net, nstream,
+						params.size,
+						vector<Mat>{calib.getCameraMat(c), calib.getCameraMat(c+1)},
+						vector<Mat>{calib.getDistCoeffs(c), calib.getDistCoeffs(c+1)}
+					);
+				}
 				setExtrinsicsRPC(net, nstream, R_c1c2, T_c1c2);
 				setPoseRPC(net, nstream, Rt_out[c]);
 				saveCalibrationRPC(net, nstream);
 				LOG(INFO) << "CALIBRATION SENT";
 				break;
 			}
-			catch (std::exception ex) {
+			catch (std::exception &ex) {
 				LOG(ERROR) << "RPC failed: " << ex.what();
 				sleep(1);
 			}
@@ -259,6 +271,7 @@ void runCameraCalibration(	ftl::Configurable* root,
 		nstream->set("uri", s);
 		nstreams.push_back(nstream);
 		stream->add(nstream);
+		
 		++count;
 	}
 	
@@ -278,23 +291,22 @@ void runCameraCalibration(	ftl::Configurable* root,
 		bool good = true;
 		try {
 			for (size_t i = 0; i < fs.frames.size(); i ++) {	
+				auto idx = stream->originStream(0, i);
+				if (idx < 0) { LOG(ERROR) << "negative index"; }
 				fs.frames[i].download(Channel::Left+Channel::Right);
 				
-				Mat left = fs.frames[i].get<Mat>(Channel::Left);
-				cv::cvtColor(left, rgb_new[2*i], cv::COLOR_BGRA2BGR);
-				Mat right = fs.frames[i].get<Mat>(Channel::Right);
-				cv::cvtColor(right, rgb_new[2*i+1], cv::COLOR_BGRA2BGR);
-
-				if (left.empty()) good = false;
-				if (right.empty()) good = false;
-				if (!good) break;
-
-				camera_parameters[2*i] = createCameraMatrix(fs.frames[i].getLeftCamera());
-				camera_parameters[2*i+1] = createCameraMatrix(fs.frames[i].getRightCamera());
-				if (res.empty()) res = rgb_new[2*i].size();
+				//fs.frames[i].get<Mat>(Channel::Left).copyTo(rgb_new[2*i]);
+				//fs.frames[i].get<Mat>(Channel::Right).copyTo(rgb_new[2*i+1]);
+				cv::cvtColor(fs.frames[i].get<Mat>(Channel::Left), rgb_new[2*idx], cv::COLOR_BGRA2BGR);
+				cv::cvtColor(fs.frames[i].get<Mat>(Channel::Right), rgb_new[2*idx+1], cv::COLOR_BGRA2BGR);
+				
+				camera_parameters[2*idx] = createCameraMatrix(fs.frames[i].getLeftCamera());
+				camera_parameters[2*idx+1] = createCameraMatrix(fs.frames[i].getRightCamera());
+				if (res.empty()) res = rgb_new[2*idx].size();
 			}
 		}
-		catch (...) {
+		catch (std::exception ex) {
+			LOG(ERROR) << ex.what();
 			good = false;
 		}
 		new_frames = good;
@@ -303,7 +315,8 @@ void runCameraCalibration(	ftl::Configurable* root,
 
 	stream->begin();
 	ftl::timer::start(false);
-
+	sleep(5);
+	
 	while(true) {
 		if (!res.empty()) {
 			params.size = res;
@@ -347,15 +360,25 @@ void runCameraCalibration(	ftl::Configurable* root,
 	sleep(3);
 
 	while(calib.getMinVisibility() < n_views) {
+		loop:
 		cv::waitKey(10);
 		
-		while (new_frames) {
-			UNIQUE_LOCK(mutex, LOCK);
-			if (new_frames) rgb.swap(rgb_new);
-			new_frames = false;
+		while (true) {
+			if (new_frames) {
+				UNIQUE_LOCK(mutex, LOCK);
+				rgb.swap(rgb_new);
+				new_frames = false;
+				break;
+			}
+			cv::waitKey(10);
 		}
 		
-		for (Mat &im : rgb) { CHECK(!im.empty()); }
+		for (Mat &im : rgb) {
+			if (im.empty()) {
+				LOG(ERROR) << "EMPTY";
+				goto loop;
+			}
+		}
 
 		visible.clear();
 		int n_found = findCorrespondingPoints(rgb, points, visible);
@@ -404,13 +427,21 @@ void runCameraCalibration(	ftl::Configurable* root,
 		cv::imshow("Cameras", show);
 	}
 	cv::destroyWindow("Cameras");
-
-	for (size_t i = 0; i < camera_parameters.size(); i++) {
-		//CHECK(params.size == Size(camera_r.width, camera_r.height));
-		//CHECK(params.size == Size(camera_l.width, camera_l.height));
-		
-		LOG(INFO) << "K[" << i << "] = \n" << camera_parameters[i];
-		calib.setCameraParameters(i, camera_parameters[i]);
+	
+	for (size_t i = 0; i < nstreams.size(); i++) {
+		while(true) {
+			try {
+				vector<Mat> D = getDistortionParametersRPC(net, nstreams[i]);
+				LOG(INFO) << "K[" << 2*i << "] = \n" << camera_parameters[2*i];
+				LOG(INFO) << "D[" << 2*i << "] = " << D[0];
+				LOG(INFO) << "K[" << 2*i+1 << "] = \n" << camera_parameters[2*i+1];
+				LOG(INFO) << "D[" << 2*i+1 << "] = " << D[1];
+				calib.setCameraParameters(2*i, camera_parameters[2*i], D[0]);
+				calib.setCameraParameters(2*i+1, camera_parameters[2*i+1], D[1]);
+				break;
+			}
+			catch (...) {}
+		}
 	}
 	
 	Mat out;
@@ -419,9 +450,16 @@ void runCameraCalibration(	ftl::Configurable* root,
 	vector<size_t> idx;
 	calibrateRPC(net, calib, params, nstreams, map1, map2, roi);
 
+	if (save_input) {
+		cv::FileStorage fs(path + filename, cv::FileStorage::WRITE);
+		calib.saveInput(fs);
+		fs.release();
+	}
+
+
 	// visualize
-	while(ftl::running) {
-		cv::waitKey(10);
+	while(cv::waitKey(10) != 27) {
+
 		while (!new_frames) {
 			if (cv::waitKey(50) != -1) { ftl::running = false; }
 		}
@@ -430,14 +468,26 @@ void runCameraCalibration(	ftl::Configurable* root,
 			UNIQUE_LOCK(mutex, LOCK)
 			rgb.swap(rgb_new);
 			new_frames = false;
-			mutex.unlock();
 		}
 
 		visualizeCalibration(calib, out, rgb, map1, map2, roi);
 		cv::namedWindow("Calibration", cv::WINDOW_KEEPRATIO | cv::WINDOW_NORMAL);
 		cv::imshow("Calibration", out);
 	}
-	
+
+	for (size_t i = 0; i < nstreams.size(); i++) {
+		while(true) {
+			try {
+				setRectifyRPC(net, nstreams[i], true);
+				break;
+			}
+			catch (...) {}
+		}
+	}
+
+	ftl::running = false;
+	ftl::timer::stop();
+	ftl::pool.stop(true);
 }
 
 int main(int argc, char **argv) {
@@ -478,7 +528,7 @@ int main(int argc, char **argv) {
 	
 	LOG(INFO)	<< "\n"
 				<< "\n"
-//				<< "\n                save_input: " << (int) save_input
+				<< "\n                save_input: " << (int) save_input
 //				<< "\n                load_input: " << (int) load_input
 //				<< "\n            save_extrinsic: " << (int) save_extrinsic
 //				<< "\n            save_intrinsic: " << (int) save_intrinsic
diff --git a/applications/calibration-multi/src/multicalibrate.cpp b/applications/calibration-multi/src/multicalibrate.cpp
index 1d22c8f0e..754c1aa83 100644
--- a/applications/calibration-multi/src/multicalibrate.cpp
+++ b/applications/calibration-multi/src/multicalibrate.cpp
@@ -91,7 +91,7 @@ MultiCameraCalibrationNew::MultiCameraCalibrationNew(
 	reference_camera_(reference_camera),
 	min_visible_points_(50),
 
-	fix_intrinsics_(fix_intrinsics == 1 ? 0 : 5),
+	fix_intrinsics_(fix_intrinsics == 1 ? 5 : 0),
 	resolution_(resolution),
 	K_(n_cameras),
 	dist_coeffs_(n_cameras),
@@ -612,6 +612,10 @@ double MultiCameraCalibrationNew::calibrateAll(int reference_camera) {
 		reference_camera_ = reference_camera; 
 	}
 
+	for (const auto &K : K_) {
+		LOG(INFO) << K;
+	}
+
 	reset(); // remove all old calibration results
 	map<pair<size_t, size_t>, pair<Mat, Mat>> transformations; 
 	
diff --git a/applications/reconstruct/CMakeLists.txt b/applications/reconstruct/CMakeLists.txt
index 61af68ef7..f7c34113a 100644
--- a/applications/reconstruct/CMakeLists.txt
+++ b/applications/reconstruct/CMakeLists.txt
@@ -8,7 +8,6 @@ set(REPSRC
 	#src/ray_cast_sdf.cu
 	src/camera_util.cu
 	#src/ray_cast_sdf.cpp
-	src/registration.cpp
 	#src/virtual_source.cpp
 	#src/splat_render.cpp
 	#src/dibr.cu
diff --git a/applications/reconstruct/include/ftl/registration.hpp b/applications/reconstruct/include/ftl/registration.hpp
deleted file mode 100644
index 1af4de8a5..000000000
--- a/applications/reconstruct/include/ftl/registration.hpp
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef _FTL_RECONSTRUCT_REGISTRATION_HPP_
-#define _FTL_RECONSTRUCT_REGISTRATION_HPP_
-
-#include <ftl/config.h>
-#include <ftl/configurable.hpp>
-#include <ftl/rgbd.hpp>
-#include <opencv2/opencv.hpp>
-
-
-namespace ftl {
-namespace registration {
-
-void to_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4d> &transformations);
-void from_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4d> &transformations);
-
-bool loadTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4d> &data);
-bool saveTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4d> &data);
-
-}
-}
-
-#endif  // _FTL_RECONSTRUCT_REGISTRATION_HPP_
\ No newline at end of file
diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index 1e2863fac..af7f5cac9 100644
--- a/applications/reconstruct/src/main.cpp
+++ b/applications/reconstruct/src/main.cpp
@@ -42,7 +42,6 @@
 #include <ftl/operators/clipping.hpp>
 
 #include <ftl/cuda/normals.hpp>
-#include <ftl/registration.hpp>
 
 #include <ftl/codecs/h264.hpp>
 #include <ftl/codecs/hevc.hpp>
@@ -71,40 +70,10 @@ using json = nlohmann::json;
 using std::this_thread::sleep_for;
 using std::chrono::milliseconds;
 
-using ftl::registration::loadTransformations;
-using ftl::registration::saveTransformations;
 
 /* Build a generator using a deprecated list of source objects. */
 static ftl::rgbd::Generator *createSourceGenerator(const std::vector<ftl::rgbd::Source*> &srcs) {
-	// Must find pose for each source...
-	if (srcs.size() > 1) {
-		std::map<std::string, Eigen::Matrix4d> transformations;
-
-		if (loadTransformations(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json", transformations)) {
-			LOG(INFO) << "Loaded camera trasformations from file";
-		}
-		else {
-			LOG(ERROR) << "Error loading camera transformations from file";
-		}
-
-		for (auto &input : srcs) {
-			string uri = input->getURI();
-
-			auto T = transformations.find(uri);
-			if (T == transformations.end()) {
-				LOG(WARNING) << "Camera pose for " + uri + " not found in transformations";
-				//LOG(WARNING) << "Using only first configured source";
-				// TODO: use target source if configured and found
-				//sources = { sources[0] };
-				//sources[0]->setPose(Eigen::Matrix4d::Identity());
-				//break;
-				input->setPose(input->getPose());
-				continue;
-			}
-			input->setPose(T->second);
-		}
-	}
-
+	
 	auto *grp = new ftl::rgbd::Group();
 	for (auto s : srcs) {
 		s->setChannel(Channel::Depth);
diff --git a/applications/reconstruct/src/registration.cpp b/applications/reconstruct/src/registration.cpp
deleted file mode 100644
index dab523544..000000000
--- a/applications/reconstruct/src/registration.cpp
+++ /dev/null
@@ -1,69 +0,0 @@
-#include <ftl/registration.hpp>
-#include <fstream>
-#define LOGURU_WITH_STREAMS 1
-#include <loguru.hpp>
-
-
-namespace ftl {
-namespace registration {
-
-using ftl::rgbd::Camera;
-using ftl::rgbd::Source;
-
-using std::string;
-using std::vector;
-using std::pair;
-using std::map;
-using std::optional;
-
-using cv::Mat;
-using Eigen::Matrix4f;
-using Eigen::Matrix4d;
-
-void from_json(nlohmann::json &json, map<string, Matrix4d> &transformations) {
-	for (auto it = json.begin(); it != json.end(); ++it) {
-		Eigen::Matrix4d m;
-		auto data = m.data();
-		for(size_t i = 0; i < 16; i++) { data[i] = it.value()[i]; }
-		transformations[it.key()] = m;
-	}
-}
-
-void to_json(nlohmann::json &json, map<string, Matrix4d> &transformations) {
-	for (auto &item : transformations) {
-		auto val = nlohmann::json::array();
-		for(size_t i = 0; i < 16; i++) { val.push_back((float) item.second.data()[i]); }
-		json[item.first] = val;
-	}
-}
-
-bool loadTransformations(const string &path, map<string, Matrix4d> &data) {
-	std::ifstream file(path);
-	if (!file.is_open()) {
-		LOG(ERROR) << "Error loading transformations from file " << path;
-		return false;
-	}
-	
-	nlohmann::json json_registration;
-	file >> json_registration;
-	from_json(json_registration, data);
-	return true;
-}
-
-bool saveTransformations(const string &path, map<string, Matrix4d> &data) {
-	nlohmann::json data_json;
-	to_json(data_json, data);
-	std::ofstream file(path);
-
-	if (!file.is_open()) {
-		LOG(ERROR) << "Error writing transformations to file " << path;
-		return false;
-	}
-
-	file << std::setw(4) << data_json;
-	return true;
-}
-
-
-} // namespace registration
-} // namespace ftl
diff --git a/components/codecs/include/ftl/codecs/channels.hpp b/components/codecs/include/ftl/codecs/channels.hpp
index 707702830..7a7ab95c3 100644
--- a/components/codecs/include/ftl/codecs/channels.hpp
+++ b/components/codecs/include/ftl/codecs/channels.hpp
@@ -2,7 +2,7 @@
 #define _FTL_RGBD_CHANNELS_HPP_
 
 #include <bitset>
-#include <msgpack.hpp>
+#include <ftl/utility/msgpack.hpp>
 
 namespace ftl {
 namespace codecs {
diff --git a/components/codecs/include/ftl/codecs/codecs.hpp b/components/codecs/include/ftl/codecs/codecs.hpp
index 4f643d406..8ff9705db 100644
--- a/components/codecs/include/ftl/codecs/codecs.hpp
+++ b/components/codecs/include/ftl/codecs/codecs.hpp
@@ -2,7 +2,7 @@
 #define _FTL_CODECS_BITRATES_HPP_
 
 #include <cstdint>
-#include <msgpack.hpp>
+#include <ftl/utility/msgpack.hpp>
 
 namespace ftl {
 namespace codecs {
diff --git a/components/codecs/include/ftl/codecs/packet.hpp b/components/codecs/include/ftl/codecs/packet.hpp
index f4089188b..e232bb31f 100644
--- a/components/codecs/include/ftl/codecs/packet.hpp
+++ b/components/codecs/include/ftl/codecs/packet.hpp
@@ -5,8 +5,7 @@
 #include <vector>
 #include <ftl/codecs/codecs.hpp>
 #include <ftl/codecs/channels.hpp>
-
-#include <msgpack.hpp>
+#include <ftl/utility/msgpack.hpp>
 
 namespace ftl {
 namespace codecs {
diff --git a/components/codecs/include/ftl/codecs/reader.hpp b/components/codecs/include/ftl/codecs/reader.hpp
index c3ea0adc6..498be4f76 100644
--- a/components/codecs/include/ftl/codecs/reader.hpp
+++ b/components/codecs/include/ftl/codecs/reader.hpp
@@ -2,10 +2,10 @@
 #define _FTL_CODECS_READER_HPP_
 
 #include <iostream>
-#include <msgpack.hpp>
 #include <inttypes.h>
 #include <functional>
 
+#include <ftl/utility/msgpack.hpp>
 #include <ftl/codecs/packet.hpp>
 #include <ftl/threads.hpp>
 
diff --git a/components/codecs/include/ftl/codecs/writer.hpp b/components/codecs/include/ftl/codecs/writer.hpp
index c1a0ba6c3..e2b30c8ae 100644
--- a/components/codecs/include/ftl/codecs/writer.hpp
+++ b/components/codecs/include/ftl/codecs/writer.hpp
@@ -2,7 +2,7 @@
 #define _FTL_CODECS_WRITER_HPP_
 
 #include <iostream>
-#include <msgpack.hpp>
+#include <ftl/utility/msgpack.hpp>
 //#include <Eigen/Eigen>
 
 #include <ftl/threads.hpp>
diff --git a/components/common/cpp/include/ftl/utility/msgpack.hpp b/components/common/cpp/include/ftl/utility/msgpack.hpp
new file mode 100644
index 000000000..993e068a8
--- /dev/null
+++ b/components/common/cpp/include/ftl/utility/msgpack.hpp
@@ -0,0 +1,126 @@
+#ifndef _FTL_MSGPACK_HPP_
+#define _FTL_MSGPACK_HPP_
+
+#ifdef _MSC_VER
+#include "msgpack_optional.hpp"
+#endif
+
+#include <msgpack.hpp>
+#include <opencv2/core/mat.hpp>
+
+namespace msgpack {
+MSGPACK_API_VERSION_NAMESPACE(MSGPACK_DEFAULT_API_NS) {
+namespace adaptor {
+
+////////////////////////////////////////////////////////////////////////////////
+// cv::Size
+
+template<>
+struct pack<cv::Size> {
+	template <typename Stream>
+	packer<Stream>& operator()(msgpack::packer<Stream>& o, cv::Size const& v) const {
+
+		o.pack_array(2);
+		o.pack(v.width);
+		o.pack(v.height);
+
+		return o;
+	}
+};
+
+template<>
+struct convert<cv::Size> {
+	msgpack::object const& operator()(msgpack::object const& o, cv::Size& v) const {
+		if (o.type != msgpack::type::ARRAY) { throw msgpack::type_error(); }
+		if (o.via.array.size != 2) { throw msgpack::type_error(); }
+		
+		int width = o.via.array.ptr[0].as<int>();
+		int height = o.via.array.ptr[1].as<int>();
+		v = cv::Size(width, height);
+		return o;
+	}
+};
+
+template <>
+struct object_with_zone<cv::Size> {
+	void operator()(msgpack::object::with_zone& o, cv::Size const& v) const {
+		o.type = type::ARRAY;
+		o.via.array.size = 2;
+		o.via.array.ptr = static_cast<msgpack::object*>(
+			o.zone.allocate_align(	sizeof(msgpack::object) * o.via.array.size,
+									MSGPACK_ZONE_ALIGNOF(msgpack::object)));
+
+		o.via.array.ptr[0] = msgpack::object(v.width, o.zone);
+		o.via.array.ptr[1] = msgpack::object(v.height, o.zone);
+	}
+};
+
+////////////////////////////////////////////////////////////////////////////////
+// cv::Mat
+
+template<>
+struct pack<cv::Mat> {
+	template <typename Stream>
+	packer<Stream>& operator()(msgpack::packer<Stream>& o, cv::Mat const& v) const {
+		// TODO: non continuous cv::Mat
+		if (!v.isContinuous()) { throw::msgpack::type_error(); }
+		o.pack_array(3);
+		o.pack(v.type());
+		o.pack(v.size());
+		
+		auto size = v.total() * v.elemSize();
+		o.pack(msgpack::type::raw_ref(reinterpret_cast<char*>(v.data), size));
+
+		return o;
+	}
+};
+
+template<>
+struct convert<cv::Mat> {
+	msgpack::object const& operator()(msgpack::object const& o, cv::Mat& v) const {
+		if (o.type != msgpack::type::ARRAY) { throw msgpack::type_error(); }
+		if (o.via.array.size != 3) { throw msgpack::type_error(); }
+
+		int type = o.via.array.ptr[0].as<int>();
+		cv::Size size = o.via.array.ptr[1].as<cv::Size>();
+		v.create(size, type);
+
+		if (o.via.array.ptr[2].via.bin.size != (v.total() * v.elemSize())) {
+			throw msgpack::type_error();
+		}
+	
+		memcpy(	v.data,
+				reinterpret_cast<const uchar*>(o.via.array.ptr[2].via.bin.ptr),
+				o.via.array.ptr[2].via.bin.size);
+		
+		return o;
+	}
+};
+
+template <>
+struct object_with_zone<cv::Mat> {
+	void operator()(msgpack::object::with_zone& o, cv::Mat const& v) const {
+		o.type = type::ARRAY;
+		o.via.array.size = 3;
+		o.via.array.ptr = static_cast<msgpack::object*>(
+			o.zone.allocate_align(	sizeof(msgpack::object) * o.via.array.size,
+									MSGPACK_ZONE_ALIGNOF(msgpack::object)));
+		
+		auto size = v.total() * v.elemSize();
+		o.via.array.ptr[0] = msgpack::object(v.type(), o.zone);
+		o.via.array.ptr[1] = msgpack::object(v.size(), o.zone);
+
+		// https://github.com/msgpack/msgpack-c/wiki/v2_0_cpp_object#conversion
+		// raw_ref not copied to zone (is this a problem?)
+		o.via.array.ptr[2] = msgpack::object(
+			msgpack::type::raw_ref(reinterpret_cast<char*>(v.data), size),
+			o.zone);
+	}
+};
+
+
+}
+}
+}
+
+#endif
diff --git a/components/net/cpp/include/msgpack_optional.hpp b/components/common/cpp/include/ftl/utility/msgpack_optional.hpp
similarity index 100%
rename from components/net/cpp/include/msgpack_optional.hpp
rename to components/common/cpp/include/ftl/utility/msgpack_optional.hpp
diff --git a/components/common/cpp/test/CMakeLists.txt b/components/common/cpp/test/CMakeLists.txt
index f9c1773b9..1b8dfc7ba 100644
--- a/components/common/cpp/test/CMakeLists.txt
+++ b/components/common/cpp/test/CMakeLists.txt
@@ -39,9 +39,15 @@ target_include_directories(timer_unit PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/../inc
 target_link_libraries(timer_unit
 	Threads::Threads ${OS_LIBS})
 
+### URI ########################################################################
+add_executable(msgpack_unit
+	./tests.cpp
+	../src/loguru.cpp
+	./msgpack_unit.cpp)
+target_include_directories(msgpack_unit PUBLIC ${OpenCV_INCLUDE_DIRS} "${CMAKE_CURRENT_SOURCE_DIR}/../include")
+target_link_libraries(msgpack_unit Threads::Threads ${OS_LIBS} ${OpenCV_LIBS})
 
 add_test(ConfigurableUnitTest configurable_unit)
 add_test(URIUnitTest uri_unit)
+add_test(MsgpackUnitTest msgpack_unit)
 # add_test(TimerUnitTest timer_unit) CI server can't achieve this
-
-
diff --git a/components/common/cpp/test/msgpack_unit.cpp b/components/common/cpp/test/msgpack_unit.cpp
new file mode 100644
index 000000000..a45ad3d1b
--- /dev/null
+++ b/components/common/cpp/test/msgpack_unit.cpp
@@ -0,0 +1,88 @@
+#include "catch.hpp"
+#define LOGURU_REPLACE_GLOG 1
+#include <loguru.hpp>
+
+#include <iostream>
+
+#include <opencv2/core.hpp>
+#include <ftl/utility/msgpack.hpp>
+
+using cv::Mat;
+using cv::Size;
+using cv::Rect;
+
+template <typename T>
+std::string msgpack_pack(T v) {
+	std::stringstream buffer;
+	msgpack::pack(buffer, v);
+	buffer.seekg(0);
+	return std::string(buffer.str());
+}
+
+Mat msgpack_unpack_mat(std::string str) {
+	msgpack::object_handle oh = msgpack::unpack(str.data(), str.size());
+	msgpack::object obj = oh.get();
+	Mat M;
+	return obj.convert<Mat>(M);
+}
+
+TEST_CASE( "msgpack cv::Mat" ) {
+	SECTION( "Mat::ones(Size(5, 5), CV_64FC1)" ) {
+		Mat A = Mat::ones(Size(5, 5), CV_64FC1);
+		Mat B = msgpack_unpack_mat(msgpack_pack(A));
+
+		REQUIRE(A.size() == B.size());
+		REQUIRE(A.type() == B.type());
+		REQUIRE(cv::countNonZero(A != B) == 0);
+	}
+
+	SECTION( "Mat::ones(Size(1, 5), CV_8UC3)" ) {
+		Mat A = Mat::ones(Size(1, 5), CV_8UC3);
+		Mat B = msgpack_unpack_mat(msgpack_pack(A));
+		
+		REQUIRE(A.size() == B.size());
+		REQUIRE(A.type() == B.type());
+		REQUIRE(cv::countNonZero(A != B) == 0);
+	}
+
+	SECTION ( "Mat 10x10 CV_64FC1 with random values [-1000, 1000]" ) {
+		Mat A(Size(10, 10), CV_64FC1);
+		cv::randu(A, -1000, 1000);
+		Mat B = msgpack_unpack_mat(msgpack_pack(A));
+		
+		REQUIRE(A.size() == B.size());
+		REQUIRE(A.type() == B.type());
+		REQUIRE(cv::countNonZero(A != B) == 0);
+	}
+
+	SECTION( "Test object_with_zone created from Mat with random values" ) {
+		Mat A(Size(10, 10), CV_64FC1);
+		cv::randu(A, -1000, 1000);
+
+		msgpack::zone z;
+		auto obj = msgpack::object(A, z);
+		
+		Mat B = msgpack_unpack_mat(msgpack_pack(obj));
+		
+		REQUIRE(A.size() == B.size());
+		REQUIRE(A.type() == B.type());
+		REQUIRE(cv::countNonZero(A != B) == 0);
+	}
+
+	SECTION( "Non-continuous Mat" ) {
+		try {
+			Mat A = Mat::ones(Size(10, 10), CV_8UC1);
+			A = A(Rect(2, 2, 3,3));
+			A.setTo(0);
+
+			Mat B = msgpack_unpack_mat(msgpack_pack(A));
+		
+			REQUIRE(A.size() == B.size());
+			REQUIRE(A.type() == B.type());
+			REQUIRE(cv::countNonZero(A != B) == 0);
+		}
+		catch (msgpack::type_error) {
+			// if not supported, throws exception
+		}
+	}
+}
diff --git a/components/net/cpp/include/ftl/net/dispatcher.hpp b/components/net/cpp/include/ftl/net/dispatcher.hpp
index 6f12e0e47..1666184c0 100644
--- a/components/net/cpp/include/ftl/net/dispatcher.hpp
+++ b/components/net/cpp/include/ftl/net/dispatcher.hpp
@@ -3,11 +3,8 @@
 
 #include <ftl/net/func_traits.hpp>
 
-#ifdef _MSC_VER
-#include <msgpack_optional.hpp>
-#endif
+#include <ftl/utility/msgpack.hpp>
 
-#include <msgpack.hpp>
 #include <memory>
 #include <tuple>
 #include <functional>
diff --git a/components/net/cpp/include/ftl/net/peer.hpp b/components/net/cpp/include/ftl/net/peer.hpp
index 4378a2578..457471747 100644
--- a/components/net/cpp/include/ftl/net/peer.hpp
+++ b/components/net/cpp/include/ftl/net/peer.hpp
@@ -5,6 +5,8 @@
 #define NOMINMAX
 #endif
 
+
+#include <ftl/utility/msgpack.hpp>
 #include <ftl/net/common.hpp>
 #include <ftl/exception.hpp>
 
diff --git a/components/net/cpp/include/ftl/net/universe.hpp b/components/net/cpp/include/ftl/net/universe.hpp
index 69d955739..aaf1c39d3 100644
--- a/components/net/cpp/include/ftl/net/universe.hpp
+++ b/components/net/cpp/include/ftl/net/universe.hpp
@@ -1,9 +1,7 @@
 #ifndef _FTL_NET_UNIVERSE_HPP_
 #define _FTL_NET_UNIVERSE_HPP_
 
-#ifdef _MSC_VER
-#include <msgpack_optional.hpp>
-#endif
+#include <ftl/utility/msgpack.hpp>
 
 #include <ftl/configurable.hpp>
 #include <ftl/net/peer.hpp>
diff --git a/components/net/cpp/include/ftl/net/ws_internal.hpp b/components/net/cpp/include/ftl/net/ws_internal.hpp
index 2e54aa01d..29fa3ff68 100644
--- a/components/net/cpp/include/ftl/net/ws_internal.hpp
+++ b/components/net/cpp/include/ftl/net/ws_internal.hpp
@@ -5,8 +5,8 @@
 #include <cstddef>
 #include <functional>
 #include <ftl/uri.hpp>
-#include <msgpack.hpp>
 
+#include <ftl/utility/msgpack.hpp>
 #include <ftl/net/common.hpp>
 
 using std::size_t;
diff --git a/components/net/cpp/include/ftl/uuid.hpp b/components/net/cpp/include/ftl/uuid.hpp
index 99f0626e6..afe5eb7de 100644
--- a/components/net/cpp/include/ftl/uuid.hpp
+++ b/components/net/cpp/include/ftl/uuid.hpp
@@ -10,7 +10,8 @@
 #include <memory>
 #include <string>
 #include <functional>
-#include <msgpack.hpp>
+
+#include <ftl/utility/msgpack.hpp>
 
 namespace ftl {
 	/**
diff --git a/components/rgbd-sources/include/ftl/rgbd/camera.hpp b/components/rgbd-sources/include/ftl/rgbd/camera.hpp
index 2d36b2f75..8108b738e 100644
--- a/components/rgbd-sources/include/ftl/rgbd/camera.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/camera.hpp
@@ -7,7 +7,7 @@
 #include <ftl/cuda_util.hpp>
 
 #ifndef __CUDACC__
-#include <msgpack.hpp>
+#include <ftl/utility/msgpack.hpp>
 #endif
 
 namespace ftl{
diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
index fd3b9ea91..14f7fb6c8 100644
--- a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
@@ -58,10 +58,10 @@ static bool isValidRotationMatrix(const Mat M) {
 	if (M.channels() != 1) 				{ return false; }
 	if (M.size() != Size(3, 3))			{ return false; }
 
-	if (abs(cv::determinant(M) - 1.0) > 0.00001)
-										{ return false; } 
+	double det = cv::determinant(M);
+	if (abs(abs(det)-1.0) > 0.00001)	{ return false; }
 
-	// accuracy problems 
+	// TODO: take floating point errors into account
 	// rotation matrix is orthogonal: M.T * M == M * M.T == I
 	//if (cv::countNonZero((M.t() * M) != Mat::eye(Size(3, 3), CV_64FC1)) != 0)
 	//									{ return false; }
@@ -71,15 +71,25 @@ static bool isValidRotationMatrix(const Mat M) {
 
 static bool isValidPose(const Mat M) {
 	if (M.size() != Size(4, 4))			{ return false; }
-	// check last row: 0 0 0 1
-	return isValidRotationMatrix(M(cv::Rect(0 , 0, 3, 3)));
+	if (!isValidRotationMatrix(M(cv::Rect(0 , 0, 3, 3))))
+										{ return false; }
+	if (!(	(M.at<double>(3, 0) == 0.0) && 
+			(M.at<double>(3, 1) == 0.0) && 
+			(M.at<double>(3, 2) == 0.0) && 
+			(M.at<double>(3, 3) == 1.0))) { return false; }
+
+	return true;
 }
 
 static bool isValidCamera(const Mat M) {
 	if (M.type() != CV_64F)				{ return false; }
 	if (M.channels() != 1)				{ return false; }
 	if (M.size() != Size(3, 3))			{ return false; }
-	// TODO: last row should be (0 0 0 1) ...
+	
+	if (!(	(M.at<double>(2, 0) == 0.0) && 
+			(M.at<double>(2, 1) == 0.0) && 
+			(M.at<double>(2, 2) == 1.0))) { return false; }
+	
 	return true;
 }
 
@@ -91,7 +101,7 @@ static Mat scaleCameraIntrinsics(Mat K, Size size_new, Size size_old) {
 	S.at<double>(0, 0) = scale_x;
 	S.at<double>(1, 1) = scale_y;
 	S.at<double>(2, 2) = 1.0;
-	return S * K;
+	return S*K;
 }
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -126,22 +136,32 @@ Mat Calibrate::_getK(size_t idx) {
 	return _getK(idx, img_size_);
 }
 
-cv::Mat Calibrate::getCameraMatrixLeft(const cv::Size res) {
+Mat Calibrate::getCameraMatrixLeft(const cv::Size res) {
 	if (rectify_) {
 		return Mat(P1_, cv::Rect(0, 0, 3, 3));
 	} else {
-		return scaleCameraIntrinsics(K_[0], res, img_size_);
+		return scaleCameraIntrinsics(K_[0], res, calib_size_);
 	}
 }
 
-cv::Mat Calibrate::getCameraMatrixRight(const cv::Size res) {
+Mat Calibrate::getCameraMatrixRight(const cv::Size res) {
 	if (rectify_) {
 		return Mat(P2_, cv::Rect(0, 0, 3, 3));
 	} else {
-		return scaleCameraIntrinsics(K_[1], res, img_size_);
+		return scaleCameraIntrinsics(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]; }
+}
+
 bool Calibrate::setRectify(bool enabled) {
 	if (t_.empty() || R_.empty()) { enabled = false; }
 	if (enabled) { 
@@ -153,17 +173,22 @@ bool Calibrate::setRectify(bool enabled) {
 	return rectify_;
 }
 
-bool Calibrate::setIntrinsics(const Size size, const vector<Mat> K, const vector<Mat> D) {
+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; }
-	if ((K.size() != 2) || (D.size() != 2)) { return false; }
 	for (const auto k : K) { if (!isValidCamera(k)) { return false; }}
-	for (const auto d : D) { if (d.size() != Size(5, 1)) { return false; }}
 
 	calib_size_ = size;
 	K[0].copyTo(K_[0]);
 	K[1].copyTo(K_[1]);
-	D[0].copyTo(D_[0]);
-	D[1].copyTo(D_[1]);
 	return true;
 }
 
@@ -206,25 +231,30 @@ bool Calibrate::loadCalibration(const string fname) {
 	fs["P"] >> pose;
 	fs.release();
 
+	bool retval = true;
 	if (calib_size.empty()) {
 		LOG(ERROR) << "calibration resolution missing in calibration file";
-		return false;
+		retval = false;
 	}
-	if (!setIntrinsics(calib_size, K, D)) {
+	if (!setIntrinsics(calib_size, K)) {
 		LOG(ERROR) << "invalid intrinsics in calibration file";
-		return false;
+		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";
-		return false;
+		retval = false;
 	}
 	if (!setPose(pose)) {
 		LOG(ERROR) << "invalid pose in calibration file";
-		return false; // TODO: allow missing pose? (config option)
+		retval = false;
 	}
 
 	LOG(INFO) << "calibration loaded from: " << fname;
-	return true;
+	return retval;
 }
 
 bool Calibrate::writeCalibration(	const string fname, const Size size,
@@ -267,18 +297,26 @@ bool Calibrate::calculateRectificationParameters() {
 		// 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);
+		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;
 	}
 
-	// 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);
-
 	return true;
 }
 
diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
index ed63f125e..161aab711 100644
--- a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
@@ -53,12 +53,24 @@ class Calibrate : public ftl::Configurable {
 	 */
 	const cv::Mat &getQ() const { return Q_; }
 
-	/* @brief	Get intrinsic paramters for rectified camera
-	 * @param	Camera resolution
+	/**
+	 * @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
 	 */
@@ -80,10 +92,15 @@ class Calibrate : public ftl::Configurable {
 	 * 
 	 * @param	size	calibration size
 	 * @param	K		2 camera matricies (3x3)
-	 * @param	D 		2 distortion parameters (5x1)
 	 * @returns	true if valid parameters
 	 */
-	bool setIntrinsics(const cv::Size size, const std::vector<cv::Mat> K, const std::vector<cv::Mat> D);
+	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.
@@ -143,7 +160,7 @@ class Calibrate : public ftl::Configurable {
 
 private:
 	// rectification enabled/disabled
-	bool rectify_;
+	volatile bool rectify_;
 
 	/**
 	 * @brief	Get intrinsic matrix saved in calibration.
@@ -153,9 +170,9 @@ private:
 	cv::Mat _getK(size_t idx, cv::Size size);
 	cv::Mat _getK(size_t idx);
 
-	// calibration resolution (loaded from file by _loadCalibration)
+	// calibration resolution (loaded from file by loadCalibration)
 	cv::Size calib_size_;
-	// camera resolution (set by _calculateRecitificationParameters)
+	// camera resolution
 	cv::Size img_size_;
 
 	// rectification maps
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
index b28fed8de..0a15e0bde 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -46,6 +46,25 @@ StereoVideoSource::~StereoVideoSource() {
 	delete lsrc_;
 }
 
+template<typename T>
+static std::pair<std::vector<int>, std::vector<T>> MatToVec(cv::Mat M) {
+	std::pair<std::vector<int>, std::vector<T>> res;
+	res.first = std::vector<int>(3);
+	res.first[0] = M.type();
+	res.first[1] = M.size().width;
+	res.first[2] = M.size().height;
+	res.second = std::vector<T>(M.begin<T>(), M.end<T>());
+	return res;
+}
+
+template<typename T>
+static cv::Mat VecToMat(std::pair<std::vector<int>, std::vector<T>> data) {
+	return cv::Mat(	data.first[1],
+					data.first[2],
+					data.first[0],
+					data.second.data());
+}
+
 void StereoVideoSource::init(const string &file) {
 	capabilities_ = kCapVideo | kCapStereo;
 
@@ -106,53 +125,35 @@ void StereoVideoSource::init(const string &file) {
 	// Tries to follow interface of ftl::Calibrate
 	
 	host_->getNet()->bind("set_pose",
-		[this](std::vector<double> data){
-			if (data.size() != 16) {
-				LOG(ERROR) << "invalid pose received (wrong size)";
-				return false;
-			}
-
-			cv::Mat M = cv::Mat(data).reshape(1, 4).t();
-			if (!calib_->setPose(M)) {
+		[this](cv::Mat pose){
+			if (!calib_->setPose(pose)) {
 				LOG(ERROR) << "invalid pose received (bad value)";
 				return false;
 			}
 
 			return true;
 	});
-
+	
 	host_->getNet()->bind("set_intrinsics",
-		[this](	std::vector<int> size,
-				std::vector<double> camera_l, std::vector<double> dist_l,
-				std::vector<double> camera_r, std::vector<double> dist_r) {
-		if ((size.size() != 2) || (camera_l.size() != 9) || (camera_r.size() != 9)) {
-			LOG(ERROR) << "bad intrinsic parameters (wrong size)";
-			return false;
-		}
-		cv::Size calib_size(size[0], size[1]);
-		cv::Mat K_l = cv::Mat(camera_l).reshape(1, 3).t();
-		cv::Mat K_r = cv::Mat(camera_r).reshape(1, 3).t();
-		cv::Mat D_l = cv::Mat(dist_l);
-		cv::Mat D_r = cv::Mat(dist_r);
-		
-		if (!calib_->setIntrinsics(calib_size, {K_l, K_r}, {D_l, D_r})) {
-			LOG(ERROR) << "bad intrinsic parameters (bad values)";
-			return false;
-		}
-		return true;
-	});
-
-	host_->getNet()->bind("set_extrinsics",
-		[this](std::vector<double> data_rvec, std::vector<double> data_tvec){
-			if ((data_rvec.size() != 3) || (data_tvec.size() != 3)) {
-				LOG(ERROR) << "invalid extrinsic parameters received (wrong size)";
+		[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;
 			}
 
-			cv::Mat R;
-			cv::Rodrigues(data_rvec, R);
-			cv::Mat t(data_tvec);
-			
+			if (!D_l.empty() && !D_r.empty()) {
+				if (!calib_->setDistortion({D_l, D_r})) {
+					LOG(ERROR) << "bad distortion parameters (bad values)";
+					return false;
+				}
+			}
+
+			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;
@@ -167,11 +168,17 @@ void StereoVideoSource::init(const string &file) {
 
 	host_->getNet()->bind("set_rectify", 
 		[this](bool enable){
-			bool retval = enable && calib_->setRectify(enable);
+			bool retval = calib_->setRectify(enable);
 			updateParameters();
 			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
@@ -201,6 +208,8 @@ void StereoVideoSource::updateParameters() {
 	// same for left and right
 	double baseline = 1.0 / calib_->getQ().at<double>(3,2);
 	double doff =  -calib_->getQ().at<double>(3,3) * baseline;
+	double min_depth = this->host_->getConfig().value<double>("min_depth", 0.0);
+	double max_depth = this->host_->getConfig().value<double>("max_depth", 15.0);
 
 	// left
 
@@ -212,9 +221,9 @@ void StereoVideoSource::updateParameters() {
 		-K.at<double>(1,2),	// Cy
 		(unsigned int) color_size_.width,
 		(unsigned int) color_size_.height,
-		0.0f,	// 0m min
-		15.0f,	// 15m max
-		baseline, // Baseline
+		min_depth,
+		max_depth,
+		baseline,
 		doff
 	};
 	
@@ -234,9 +243,9 @@ void StereoVideoSource::updateParameters() {
 		-K.at<double>(1,2),	// Cy
 		(unsigned int) color_size_.width,
 		(unsigned int) color_size_.height,
-		0.0f,	// 0m min
-		15.0f,	// 15m max
-		baseline, // Baseline
+		min_depth,
+		max_depth,
+		baseline,
 		doff
 	};
 }
diff --git a/components/streams/src/parsers.cpp b/components/streams/src/parsers.cpp
index f6dee9cce..9a5f1ee29 100644
--- a/components/streams/src/parsers.cpp
+++ b/components/streams/src/parsers.cpp
@@ -15,22 +15,24 @@ ftl::rgbd::Camera ftl::stream::parseCalibration(const ftl::codecs::Packet &pkt)
 			  << ", fx: " << std::get<0>(params).fx
 			  << ", cx: " << std::get<0>(params).cx
 			  << ", cy: " << std::get<0>(params).cy;
+	
 	return std::get<0>(params);
 }
 
 Eigen::Matrix4d ftl::stream::parsePose(const ftl::codecs::Packet &pkt) {
-    Eigen::Matrix4d p;
+	Eigen::Matrix4d p;
 
 	if (pkt.codec == ftl::codecs::codec_t::POSE) {
 		p = Eigen::Map<Eigen::Matrix4d>((double*)pkt.data.data());
 	} else if (pkt.codec == ftl::codecs::codec_t::MSGPACK) {
+
 		auto unpacked = msgpack::unpack((const char*)pkt.data.data(), pkt.data.size());
 		std::vector<double> posevec;
 		unpacked.get().convert(posevec);
 		p = Eigen::Matrix4d(posevec.data());
+		
 	}
-
-    return p;
+	return p;
 }
 
 std::string ftl::stream::parseConfig(const ftl::codecs::Packet &pkt) {
-- 
GitLab