applications/calibration-multi/src/multicalibrate.cpp

-#include "multicalibrate.hpp"
-
-#include "calibration_data.hpp"
-#include "calibration.hpp"
-
-#include <ftl/calibration/optimize.hpp>
-
-#include <opencv2/core.hpp>
-#include <opencv2/calib3d.hpp>
-
-#include <loguru.hpp>
-
-#include <map>
-
-using cv::Mat;
-using cv::Size;
-using cv::Point2d;
-using cv::Point3d;
-using cv::Vec4d;
-using cv::Scalar;
-
-using std::string;
-using std::vector;
-using std::map;
-using std::pair;
-using std::make_pair;
-
-double CalibrationTarget::estimateScale(vector<Point3d> points) {
-	
-	// 1. calculate statistics 
-	// 2. reject possible outliers 
-	// 3. calculate scale factor
-
-	double f = 0.0;
-	double S = 0.0;
-	double m = 0.0;
-	
-	vector<double> d(points.size() / 2, 0.0);
-
-	for (size_t i = 0; i < points.size(); i += 2) {
-		const Point3d &p1 = points[i];
-		const Point3d &p2 = points[i + 1];
-
-		Point3d p = p1 - p2;
-
-		double x = sqrt(p.x * p.x + p.y * p.y + p.z * p.z);
-		double prev_mean = m;
-		d[i/2] = x;
-
-		f = f + 1.0;
-		m = m + (x - m) / f;
-		S = S + (x - m) * (x - prev_mean);
-
-	}
-
-	double stddev = sqrt(S / f);
-	f = 0.0;
-
-	int outliers = 0;
-	double scale = 0.0;
-
-	for (double l : d) {
-		// TODO:	* Parameterize how large deviation allowed
-		//			* Validate this actually improves quality
-
-		if (abs(l - m) > 3.0 * stddev) {
-			outliers++;
-		}
-		else {
-			f += 1.0;
-			scale += 1.0 / l;
-		}
-		DCHECK(scale != INFINITY);
-	}
-
-	if (outliers != 0) {
-		LOG(WARNING) << "Outliers (large std. deviation in scale): " << outliers;
-	}
-
-	LOG(INFO) << "calibration target std. dev. " <<  stddev << " (" << (int) f << " samples), scale: " << scale * calibration_bar_length_ / f;
-
-	return scale * calibration_bar_length_ / f;
-
-	// TODO:	LM-optimization for scale.
-}
-
-MultiCameraCalibrationNew::MultiCameraCalibrationNew(
-			size_t n_cameras, size_t reference_camera, Size resolution,
-			CalibrationTarget target, int fix_intrinsics) :
-		
-	target_(target),
-	visibility_graph_(n_cameras),
-	is_calibrated_(false),
-	n_cameras_(n_cameras),
-	reference_camera_(reference_camera),
-	min_visible_points_(50),
-
-	fix_intrinsics_(fix_intrinsics == 1 ? 5 : 0),
-	resolution_(resolution),
-	K_(n_cameras),
-	dist_coeffs_(n_cameras),
-	R_(n_cameras),
-	t_(n_cameras),
-
-	points3d_optimized_(n_cameras),
-	points3d_(n_cameras),
-	points2d_(n_cameras),
-	visible_(n_cameras),
-
-	fm_method_(cv::FM_8POINT), // RANSAC/LMEDS results need validation (does not work)
-	fm_ransac_threshold_(0.95),
-	fm_confidence_(0.90)
-{
-	for (auto &K : K_) { K = Mat::eye(Size(3, 3), CV_64FC1); }
-	for (auto &d : dist_coeffs_) { d = Mat(Size(5, 1), CV_64FC1, Scalar(0.0)); }
-}
-
-Mat MultiCameraCalibrationNew::getCameraMat(size_t idx) {
-	DCHECK(idx < n_cameras_);
-	Mat K;
-	K_[idx].copyTo(K);
-	return K;
-}
-
-
-Mat MultiCameraCalibrationNew::getCameraMatNormalized(size_t idx, double scale_x, double scale_y)
-{
-	Mat K = getCameraMat(idx);
-	CHECK((scale_x != 0.0 && scale_y != 0.0) || ((scale_x == 0.0) && scale_y == 0.0));
-
-	scale_x = scale_x / (double) resolution_.width;
-	scale_y = scale_y / (double) resolution_.height;
-
-	Mat scale(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 * K);
-}
-
-Mat MultiCameraCalibrationNew::getDistCoeffs(size_t idx) {
-	DCHECK(idx < n_cameras_);
-	Mat D;
-	dist_coeffs_[idx].copyTo(D);
-	return D;
-}
-
-void MultiCameraCalibrationNew::setCameraParameters(size_t idx, const Mat &K, const Mat &distCoeffs) {
-	CHECK(idx < n_cameras_);
-	CHECK(K.size() == Size(3, 3));
-	CHECK(distCoeffs.total() == 5);
-	K.convertTo(K_[idx], CV_64FC1);
-	distCoeffs.convertTo(dist_coeffs_[idx], CV_64FC1);
-}
-
-void MultiCameraCalibrationNew::setCameraParameters(size_t idx, const Mat &K) {
-	DCHECK(idx < n_cameras_);
-	setCameraParameters(idx, K, dist_coeffs_[idx]);
-}
-
-void MultiCameraCalibrationNew::addPoints(vector<vector<Point2d>> points, vector<int> visible) {
-	DCHECK(points.size() == visible.size());
-	DCHECK(visible.size() == n_cameras_);
-
-	for (size_t i = 0; i < n_cameras_; i++) {
-		visible_[i].insert(visible_[i].end(), points[i].size(), visible[i]);
-		points2d_[i].insert(points2d_[i].end(), points[i].begin(), points[i].end());
-	}
-	visibility_graph_.update(visible);
-}
-
-void MultiCameraCalibrationNew::reset() {
-	is_calibrated_ = false;
-	weights_ = vector(n_cameras_, vector(points2d_[0].size(), 0.0));
-	inlier_ = vector(n_cameras_, vector(points2d_[0].size(), 0));
-	points3d_ = vector(n_cameras_, vector(points2d_[0].size(), Point3d()));
-	points3d_optimized_ = vector(points2d_[0].size(), Point3d());
-	R_ = vector<Mat>(n_cameras_, Mat::eye(Size(3, 3), CV_64FC1));
-	t_ = vector<Mat>(n_cameras_, Mat(Size(1, 3), CV_64FC1, Scalar(0.0)));
-}
-
-void MultiCameraCalibrationNew::saveInput(const string &filename) {
-	cv::FileStorage fs(filename, cv::FileStorage::WRITE);
-	saveInput(fs);
-	fs.release();
-}
-
-void MultiCameraCalibrationNew::saveInput(cv::FileStorage &fs) {
-	fs << "resolution" << resolution_;
-	fs << "K" << K_;
-	fs << "D" << dist_coeffs_;
-	fs << "points2d" << points2d_;
-	fs << "visible" << visible_;
-}
-
-void MultiCameraCalibrationNew::loadInput(const std::string &filename, const vector<size_t> &cameras_in) {
-	points2d_.clear();
-	points3d_.clear();
-	points3d_optimized_.clear();
-	visible_.clear();
-	inlier_.clear();
-	K_.clear();
-	dist_coeffs_.clear();
-	cv::FileStorage fs(filename, cv::FileStorage::READ);
-	vector<Mat> K;
-	vector<vector<Point2d>> points2d;
-	vector<vector<int>> visible;
-	fs["K"] >> K;
-	fs["D"] >> dist_coeffs_;
-	fs["points2d"] >> points2d;
-	fs["visible"] >> visible;
-	fs["resolution"] >> resolution_;
-	fs.release();
-	
-	vector<size_t> cameras;
-	if (cameras_in.size() == 0) {
-		cameras.resize(K.size());
-		size_t i = 0;
-		for (auto &c : cameras) { c = i++; }
-	} 
-	else {
-		cameras.reserve(cameras_in.size());
-		for (auto &c : cameras_in) { cameras.push_back(c); }
-	}
-	
-	n_cameras_ = cameras.size();
-
-	points2d_.resize(n_cameras_);
-	points3d_.resize(n_cameras_);
-	visible_.resize(n_cameras_);
-
-	for (auto const &c : cameras) {
-		K_.push_back(K[c]);
-		LOG(INFO) << K[c];
-	}
-	for (size_t c = 0; c < n_cameras_; c++) {
-		points2d_[c].reserve(visible[0].size());
-		points3d_[c].reserve(visible[0].size());
-		visible_[c].reserve(visible[0].size());
-		points3d_optimized_.reserve(visible[0].size());
-	}
-
-	visibility_graph_ = Visibility(n_cameras_);
-	dist_coeffs_.resize(n_cameras_);
-
-	vector<vector<Point2d>> points2d_add(n_cameras_, vector<Point2d>());
-	vector<int> visible_add(n_cameras_);
-	for (size_t i = 0; i < visible[0].size(); i += target_.n_points) {
-		int count = 0;
-		for (size_t c = 0; c < n_cameras_; c++) {
-			count += visible[c][i];
-			points2d_add[c].clear();
-			points2d_add[c].insert(
-								points2d_add[c].begin(),
-								points2d[cameras[c]].begin() + i,
-								points2d[cameras[c]].begin() + i + target_.n_points);
-			visible_add[c] = visible[cameras[c]][i];
-		}
-		if (count >= 2) {
-			addPoints(points2d_add, visible_add);
-		}
-	}
-	reset();
-	
-	DCHECK(points2d_.size() == n_cameras_);
-	DCHECK(points2d_.size() == visible_.size());
-	size_t len = visible_[0].size();
-	for (size_t i = 0; i < n_cameras_; i++) {
-		DCHECK(visible_[i].size() == len);
-		DCHECK(points2d_[i].size() == visible_[i].size());
-	}
-}
-
-size_t MultiCameraCalibrationNew::getViewsCount() {
-	return points2d_[0].size() / target_.n_points;
-}
-
-size_t MultiCameraCalibrationNew::getOptimalReferenceCamera() {
-	return (size_t) visibility_graph_.getOptimalCamera();
-}
-
-bool MultiCameraCalibrationNew::isVisible(size_t camera, size_t idx) {
-	return visible_[camera][idx] == 1;
-}
-
-bool MultiCameraCalibrationNew::isValid(size_t camera, size_t idx) {
-	return inlier_[camera][idx] >= 0;
-}
-
-bool MultiCameraCalibrationNew::isValid(size_t idx) {
-	for (auto camera : inlier_) {
-		if (camera[idx] > 0) return true;
-	}
-	return false;
-}
-
-vector<Point2d> MultiCameraCalibrationNew::getPoints(size_t camera, size_t idx) {
-	return vector<Point2d> (points2d_[camera].begin() + idx * (target_.n_points), 
-							points2d_[camera].begin() + idx * (target_.n_points + 1));
-}
-
-
-void MultiCameraCalibrationNew::updatePoints3D(size_t camera, Point3d new_point,
-		size_t idx, const Mat &R, const Mat &t) {
-	
-	int &f = inlier_[camera][idx];
-	Point3d &point = points3d_[camera][idx];
-	new_point = transformPoint(new_point, R, t);
-
-	if (f == -1) return;
-
-	if (f > 0) {
-		// TODO:	remove parameter (10.0 cm - 1.0m); over 0.25m difference
-		//			would most likely suggest very bad triangulation (sync? wrong match?)
-		// 			instead store all triangulations and handle outliers
-		//			(perhaps inverse variance weighted mean?)
-		
-		if (euclideanDistance(point, new_point) > 10.0) {
-			LOG(ERROR) << "bad value (skipping) " << "(" << point << " vs " << new_point << ")";
-			f = -1;
-		}
-		else {
-			point = (point * f + new_point) / (double) (f + 1);
-			f++;
-		}
-	}
-	else {
-		point = new_point;
-		f = 1;
-	}
-}
-
-void MultiCameraCalibrationNew::updatePoints3D(size_t camera, vector<Point3d> points,
-		vector<size_t> idx, const Mat &R, const Mat &t) {
-	
-	for (size_t i = 0; i < idx.size(); i++) {
-		updatePoints3D(camera, points[i], idx[i], R, t);
-	}
-}
-
-void MultiCameraCalibrationNew::getVisiblePoints(
-		vector<size_t> cameras, vector<vector<Point2d>> &points, vector<size_t> &idx) {
-	
-	size_t n_points_total = points2d_[0].size();
-	DCHECK(cameras.size() <= n_cameras_);
-	DCHECK(n_points_total % target_.n_points == 0);
-	
-	idx.clear();
-	idx.reserve(n_points_total);
-	points.clear();
-	points.resize(cameras.size(), {});
-	
-	for (size_t i = 0; i < n_points_total; i += target_.n_points) {
-		bool visible_all = true;
-
-		for (auto c : cameras) {
-			for (size_t j = 0; j < target_.n_points; j++) {
-				visible_all &= isVisible(c, i + j);
-			}
-		}
-		
-		if (!visible_all) { continue; }
-
-		for (size_t j = 0; j < target_.n_points; j++) {
-			idx.push_back(i + j);
-		}
-
-		for (size_t c = 0; c < cameras.size(); c++) {
-			points[c].insert(points[c].end(),
-							 points2d_[cameras[c]].begin() + i,
-							 points2d_[cameras[c]].begin() + i + target_.n_points
-			);
-		}
-	}
-
-	for (auto p : points) {	DCHECK(idx.size() == p.size()); }
-}
-
-double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camera_to, Mat &rmat, Mat &tvec) {
-	
-
-	vector<size_t> idx;
-	vector<Point2d> points1, points2;
-	{
-		vector<vector<Point2d>> points2d;
-		getVisiblePoints({camera_from, camera_to}, points2d, idx);
-
-		points1 = points2d[0];
-		points2 = points2d[1];
-	}
-	DCHECK(points1.size() % target_.n_points == 0);
-	DCHECK(points1.size() == points2.size());
-
-	// cameras possibly lack line of sight?
-	DCHECK(points1.size() > 8);
-	
-	Mat &K1 = K_[camera_from];
-	Mat &K2 = K_[camera_to];
-	/*
-	vector<uchar> inliers;
-	Mat F, E;
-	F = cv::findFundamentalMat(points1, points2, fm_method_, fm_ransac_threshold_, fm_confidence_, inliers);
-
-	if (F.empty())
-	{
-		LOG(ERROR) << "Fundamental matrix estimation failed. Possibly degenerate configuration?";
-		return INFINITY;
-	}
-
-	E = K2.t() * F * K1;
-
-	// Only include inliers
-	if (fm_method_ == cv::FM_LMEDS || fm_method_ == cv::FM_RANSAC) {
-		vector<Point2d> inliers1, inliers2;
-		vector<size_t> inliers_idx;
-
-		inliers1.reserve(points1.size());
-		inliers2.reserve(points1.size());
-		inliers_idx.reserve(points1.size());
-
-		for (size_t i = 0; i < inliers.size(); i += target_.n_points) {
-			bool inlier = true;
-			
-			for (size_t j = 0; j < target_.n_points; j++) {
-				inlier &= inliers[i+j];
-			}
-
-			if (inlier) {
-				inliers1.insert(inliers1.end(), points1.begin() + i, points1.begin() + i + target_.n_points);
-				inliers2.insert(inliers2.end(), points2.begin() + i, points2.begin() + i + target_.n_points);
-				inliers_idx.insert(inliers_idx.end(), idx.begin() + i, idx.begin() + i + target_.n_points);
-			}
-		}
-		
-		LOG(INFO) << "Total points: " << points1.size() << ", inliers: " << inliers1.size();
-		double ratio_good_points = (double) inliers1.size() / (double) points1.size();
-		if (ratio_good_points < 0.66) {
-			// TODO: ... 
-			LOG(WARNING) << "Over 1/3 of points rejected!";
-			if (ratio_good_points < 0.33) { LOG(FATAL) << "Over 2/3 points rejected!"; }
-		}
-		
-		DCHECK(inliers1.size() == inliers_idx.size());
-		DCHECK(inliers2.size() == inliers_idx.size());
-
-		std::swap(inliers1, points1);
-		std::swap(inliers2, points2);
-		std::swap(inliers_idx, idx);
-	}
-	
-	// Estimate initial rotation matrix and translation vector and triangulate
-	// points (in camera 1 coordinate system).
-
-	Mat R1, R2, t1, t2;
-	R1 = Mat::eye(Size(3, 3), CV_64FC1);
-	t1 = Mat(Size(1, 3), CV_64FC1, Scalar(0.0));
-
-	vector<Point3d> points3d;
-	// Convert homogeneous coordinates 
-	{
-		Mat points3dh;
-		recoverPose(E, points1, points2, K1, K2, R2, t2, 1000.0, points3dh);
-		points3d.reserve(points3dh.cols);
-
-		for (int col = 0; col < points3dh.cols; col++) {
-			Point3d p = Point3d(points3dh.at<double>(0, col),
-								points3dh.at<double>(1, col),
-								points3dh.at<double>(2, col))
-								/ points3dh.at<double>(3, col);
-			points3d.push_back(p);
-		}
-	}
-	DCHECK(points3d.size() == points1.size());
-
-	// Estimate and apply scale factor
-	{
-		double scale = ftl::calibration::optimizeScale(object_points_, points3d);
-		t1 = t1 * scale;
-		t2 = t2 * scale;
-	}
-
-	// Reprojection error before BA
-	{
-		// SBA should report squared mean error
-		const double err1 = reprojectionError(points3d, points1, K1, R1, t1);
-		const double err2 = reprojectionError(points3d, points2, K2, R2, t2);
-		
-		if (abs(err1 - err2) > 2.0) {
-			LOG(INFO) << "Initial reprojection error (camera " << camera_from << "): " << err1;
-			LOG(INFO) << "Initial reprojection error (camera " << camera_to << "): " << err2;
-		}
-		LOG(INFO)	<< "Initial reprojection error (" << camera_from << ", " << camera_to << "): "
-					<< sqrt(err1 * err1 + err2 * err2);
-		
-	}
-	
-	// Bundle Adjustment
-	
-	double err = INFINITY;
-	{
-		auto cameras = vector<ftl::calibration::Camera> {
-			ftl::calibration::Camera(K1, dist_coeffs_[camera_from], R1, t1),
-			ftl::calibration::Camera(K2, dist_coeffs_[camera_to], R2, t2)
-		};
-
-		ftl::calibration::BundleAdjustment ba;
-		ba.addCameras(cameras);
-		for (size_t i = 0; i < points3d.size(); i++) {
-			ba.addPoint({points1[i], points2[i]}, points3d[i]);
-		}
-
-		ftl::calibration::BundleAdjustment::Options options;
-		options.fix_camera_extrinsic = {0};
-		options.optimize_intrinsic = false;
-
-		ba.run(options);
-		// TODO: ... 
-		err = sqrt(ba.reprojectionError(0) * ba.reprojectionError(1));
-
-		R2 = cameras[1].rmat();
-		t2 = Mat(cameras[1].tvec());
-	}
-
-	calculateTransform(R2, t2, R1, t1, rmat, tvec);
-	*/
-	
-	Mat R1, R2, t1, t2;
-	R1 = Mat::eye(Size(3, 3), CV_64FC1);
-	t1 = Mat(Size(1, 3), CV_64FC1, Scalar(0.0));
-	
-	vector<Point3d> points3d;
-	
-	double err = ftl::calibration::computeExtrinsicParameters(K1, dist_coeffs_[camera_from], K2, dist_coeffs_[camera_to], points1, points2, object_points_, R2, t2, points3d);
-	calculateTransform(R2, t2, R1, t1, rmat, tvec);
-	
-
-	// Store and average 3D points for both cameras (skip garbage)
-	if (err < 10.0) {
-		Mat rmat1, tvec1;
-		updatePoints3D(camera_from, points3d, idx, R1, t1);
-		updatePoints3D(camera_to, points3d, idx, R2, t2);
-	}
-	else {
-		LOG(ERROR)	<< "Large RMS error ("
-					<< reprojectionError(points3d, points2, K2, rmat, tvec)
-					<< "), not updating points!";
-	}
-
-	//LOG(INFO) << reprojectionError(points3d, points1, K1, R1, t1);
-	//LOG(INFO) << reprojectionError(points3d, points2, K2, R2, t2);
-
-	return err;
-}
-
-Point3d MultiCameraCalibrationNew::getPoint3D(size_t camera, size_t idx) {
-	return points3d_[camera][idx];
-}
-
-void MultiCameraCalibrationNew::calculateMissingPoints3D() {
-	points3d_optimized_.clear();
-	points3d_optimized_.resize(points3d_[reference_camera_].size());
-
-	for (size_t i = 0; i < points3d_optimized_.size(); i++) {
-		if (inlier_[reference_camera_][i] > 0) {
-			points3d_optimized_[i] = points3d_[reference_camera_][i];
-			continue;
-		}
-
-		if (!isValid(i)) continue;
-
-		double f = 0.0;
-		Point3d point(0.0, 0.0, 0.0);
-		for (size_t c = 0; c < n_cameras_; c++) {
-			if (inlier_[c][i] <= 0) { continue; }
-			point += transformPoint(getPoint3D(c, i), R_[c], t_[c]);
-			f += 1.0;
-		}
-
-		DCHECK(f != 0.0);
-
-		points3d_optimized_[i] = point / f;
-	}
-}
-
-double MultiCameraCalibrationNew::getReprojectionError(size_t c_from, size_t c_to, const Mat &K, const Mat &R, const Mat &t) {
-
-	vector<Point2d> points2d;
-	vector<Point3d> points3d;
-
-	for (size_t i = 0; i < points2d_[c_from].size(); i++) {
-		if (!isValid(i) || !isVisible(c_from, i) || !isVisible(c_to, i)) continue;
-		points2d.push_back(points2d_[c_from][i]);
-		points3d.push_back(points3d_[c_to][i]);
-	}
-
-	return reprojectionError(points3d, points2d, K, R, t);
-}
-
-double MultiCameraCalibrationNew::getReprojectionErrorOptimized(size_t c_from, const Mat &K, const Mat &R, const Mat &t) {
-	
-	vector<Point2d> points2d;
-	vector<Point3d> points3d;
-
-	for (size_t i = 0; i < points2d_[c_from].size(); i++) {
-		if (!isValid(i) || !isVisible(c_from, i)) continue;
-		points2d.push_back(points2d_[c_from][i]);
-		points3d.push_back(points3d_optimized_[i]);
-	}
-
-	return reprojectionError(points3d, points2d, K, R, t);
-}
-
-
-double MultiCameraCalibrationNew::calibrateAll(int reference_camera) {
-	if (reference_camera != -1) {
-		DCHECK(reference_camera >= 0 && reference_camera < static_cast<int>(n_cameras_));
-		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; 
-	
-	// All cameras should be calibrated pairwise; otherwise all possible 3D
-	// points are not necessarily triangulated
-
-	auto paths = visibility_graph_.findShortestPaths(reference_camera_);
-	
-	for (size_t c1 = 0; c1 < n_cameras_; c1++) {
-	for (size_t c2 = c1; c2 < n_cameras_; c2++) {
-		if (c1 == c2) {
-			transformations[make_pair(c1, c2)] = 
-				make_pair(Mat::eye(Size(3, 3), CV_64FC1),
-				Mat(Size(1, 3), CV_64FC1, Scalar(0.0))
-			);
-			continue;
-		}
-
-		size_t n_visible = getVisiblePointsCount({c1, c2});
-
-		if (n_visible < min_visible_points_) {
-			LOG(INFO)	<< "Not enough (" << min_visible_points_ << ") points  between "
-						<< "cameras " << c1 << " and " << c2 << " (" << n_visible << " points), "
-						<< "skipping";
-			continue;
-		}
-		LOG(INFO)	<< "Running pairwise calibration for cameras "
-					<< c1 << " and " << c2 << "(" << n_visible << " points)";
-
-		if (transformations.find(make_pair(c2, c1)) != transformations.end()) {
-			continue;
-		}
-		Mat R, t, R_i, t_i;
-
-		// TODO: threshold parameter, 16.0 possibly too high
-
-		if (calibratePair(c1, c2, R, t) > 16.0) {
-			LOG(ERROR)	<< "Pairwise calibration failed, skipping cameras "
-						<< c1 << " and " << c2;
-			visibility_graph_.deleteEdge(c1, c2);
-			continue;
-		}
-
-		calculateInverse(R, t, R_i, t_i);
-
-		transformations[make_pair(c2, c1)] = make_pair(R, t);
-		transformations[make_pair(c1, c2)] = make_pair(R_i, t_i);
-	}}
-
-	for (size_t c = 0; c < paths.size(); c++) {
-		Mat R_chain = Mat::eye(Size(3, 3), CV_64FC1);
-		Mat t_chain = Mat(Size(1, 3), CV_64FC1, Scalar(0.0));
-		LOG(INFO) << "Chain for camera " << c;
-		for (auto e: paths[c]) {
-			CHECK(transformations.find(e) != transformations.end()) << "chain not calculated; pairwise calibration possibly failed earlier?";
-			LOG(INFO) << e.first << " -> " << e.second;
-			Mat R = transformations[e].first;
-			Mat t = transformations[e].second;
-			R_chain = R * R_chain;
-			t_chain = t + R * t_chain;
-		}
-
-		R_[c] = R_chain;
-		t_[c] = t_chain;
-		/*R_[c] = transformations[make_pair(reference_camera_, c)].first;
-		t_[c] = transformations[make_pair(reference_camera_, c)].second;
-		DCHECK(R_[c].size() == Size(3, 3));
-		DCHECK(t_[c].size() == Size(1, 3));*/
-	}
-	
-	calculateMissingPoints3D();
-	
-	for (size_t c_from = 0; c_from < n_cameras_; c_from++) {
-		if (c_from == reference_camera_) continue;
-		Mat R, t;
-		calculateInverse(R_[c_from], t_[c_from], R, t);
-		LOG(INFO)	<< "Error before BA, cameras " << reference_camera_ << " and " << c_from << ": "
-					<< getReprojectionErrorOptimized(c_from, K_[c_from], R, t);
-	}
-
-	double err;
-	{
-		auto cameras = vector<ftl::calibration::Camera>();
-		
-		for (size_t i = 0; i < n_cameras_; i++) {
-			calculateInverse(R_[i], t_[i], R_[i], t_[i]);
-			cameras.push_back(ftl::calibration::Camera(K_[i], dist_coeffs_[i], R_[i], t_[i]));
-		}
-
-		ftl::calibration::BundleAdjustment ba;
-		ba.addCameras(cameras);
-
-		for (size_t i = 0; i < points3d_optimized_.size(); i++) {
-			
-			auto &p = points3d_optimized_[i];
-			DCHECK(!isnanl(p.x) && !isnanl(p.y) && !isnanl(p.z));
-
-			int count = 0;
-			for (size_t c = 0; c < n_cameras_; c++) {
-				if (isVisible(c, i) && isValid(c, i)) { count++; }
-			}
-			
-			if (count < 2) continue;
-			
-			vector<bool> visible(n_cameras_);
-			vector<Point2d> points2d(n_cameras_);
-
-			for (size_t c = 0; c < n_cameras_; c++) {
-				bool good = isVisible(c, i) && isValid(c, i);
-				visible[c] = good;
-				points2d[c] = points2d_[c][i];
-			}
-
-			ba.addPoint(visible, points2d, p);
-		}
-
-		ba.addObject(object_points_);
-
-		ftl::calibration::BundleAdjustment::Options options;
-		options.loss = ftl::calibration::BundleAdjustment::Options::Loss::CAUCHY;
-		options.optimize_intrinsic = !fix_intrinsics_;
-		options.fix_distortion = true;
-		options.max_iter = 50;
-		options.fix_camera_extrinsic = {reference_camera};
-		options.verbose = true;
-		options.max_iter = 500;
-		
-		err = ba.reprojectionError();
-		ba.run(options);
-
-		for (size_t i = 0; i < n_cameras_; i++) {
-			R_[i] = cameras[i].rmat();
-			t_[i] = cameras[i].tvec();
-			K_[i] = cameras[i].intrinsicMatrix();
-			//dist_coeffs_[i] = D; // not updated
-			calculateInverse(R_[i], t_[i], R_[i], t_[i]);
-		}
-	}
-
-	for (size_t c_from = 0; c_from < n_cameras_; c_from++) {
-		if (c_from == reference_camera_) continue;
-		Mat R, t;
-		calculateInverse(R_[c_from], t_[c_from], R, t);
-		LOG(INFO)	<< "Error (RMS) after BA, cameras " << reference_camera_ << " and " << c_from << ": "
-					<< getReprojectionErrorOptimized(c_from, K_[c_from], R, t);
-	
-	}
-
-	is_calibrated_ = true;
-	return err;
-}
-
-void MultiCameraCalibrationNew::projectPointsOriginal(size_t camera_src, size_t camera_dst, size_t idx, vector<Point2d> &points) {
-	
-}
-
-void MultiCameraCalibrationNew::projectPointsOptimized(size_t camera_dst, size_t idx, vector<Point2d> &points) {
-	// TODO:	indexing does not match input (points may be skipped in loadInput())
-
-	points.clear();
-	size_t i = target_.n_points * idx;
-	
-	if (!isValid(i)) return;
-
-	Point3d p1(points3d_optimized_[i]);
-	Point3d p2(points3d_optimized_[i + 1]);
-
-	if (!std::isfinite(p1.x) || !std::isfinite(p2.x)) {
-		// DEBUG: should not happen
-		LOG(ERROR) << "Bad point! (no valid triangulation)";
-		return; 
-	}
-	
-	Mat R, tvec, rvec;
-	calculateTransform(R_[reference_camera_], t_[reference_camera_], R_[camera_dst], t_[camera_dst], R, tvec);
-	
-	cv::Rodrigues(R, rvec);
-	cv::projectPoints(	vector<Point3d> { p1, p2 },
-						rvec, tvec, K_[camera_dst], dist_coeffs_[camera_dst], points);
-}
-
-void MultiCameraCalibrationNew::getCalibration(vector<Mat> &R, vector<Mat> &t) {
-	DCHECK(is_calibrated_);
-	R.resize(n_cameras_);
-	t.resize(n_cameras_);
-
-	for (size_t i = 0; i < n_cameras_; i++) {
-		R_[i].copyTo(R[i]);
-		t_[i].copyTo(t[i]);
-	}
-}
\ No newline at end of file

applications/calibration-multi/src/multicalibrate.hpp

-#pragma once
-
-#include <opencv2/core.hpp>
-
-#include "visibility.hpp"
-#include "util.hpp"
-
-using cv::Mat;
-using cv::Size;
-using cv::Point2d;
-using cv::Point3d;
-using cv::Vec4d;
-using cv::Scalar;
-
-using std::vector;
-using std::pair;
-
-class CalibrationTarget {
-public:
-	explicit CalibrationTarget(double length):
-		n_points(2),
-		calibration_bar_length_(length)
-	{}
-	
-	/* @brief	Estimate scale factor.
-	 * @param	3D points (can pass n views)
-	 */
-	double estimateScale(vector<Point3d> points3d);
-	size_t n_points;
-
-private:
-	double calibration_bar_length_;
-};
-
-class MultiCameraCalibrationNew {
-public:
-	MultiCameraCalibrationNew(	size_t n_cameras, size_t reference_camera,
-								Size resolution, CalibrationTarget target,
-								int fix_intrinsics=1);
-	
-	void setCameraParameters(size_t idx, const Mat &K, const Mat &distCoeffs);
-	void setCameraParameters(size_t idx, const Mat &K);
-
-	void addPoints(vector<vector<Point2d>> points2d, vector<int> visibility);
-
-	size_t getViewsCount();
-	size_t getCamerasCount() { return n_cameras_; }
-	size_t getOptimalReferenceCamera();
-
-	size_t getMinVisibility() { return visibility_graph_.getMinVisibility(); }
-	size_t getViewsCount(size_t camera) { return visibility_graph_.getViewsCount(camera); }
-
-	void setFixIntrinsic(int value) { fix_intrinsics_ = (value == 1 ? 5 : 0); }
-
-	void loadInput(const std::string &filename, const vector<size_t> &cameras = {});
-
-	void saveInput(cv::FileStorage &fs);
-	void saveInput(const std::string &filename);
-
-	Mat getCameraMat(size_t idx);
-	Mat getCameraMatNormalized(size_t idx, double scale_x = 1.0, double scale_y = 1.0);
-
-	Mat getDistCoeffs(size_t idx);
-
-	double calibrateAll(int reference_camera = -1);
-	double getReprojectionError();
-	void getCalibration(vector<Mat> &R, vector<Mat> &t);
-
-	void projectPointsOriginal(size_t camera_src, size_t camera_dst, size_t idx, vector<Point2d> &points);
-	void projectPointsOptimized(size_t camera_dst, size_t idx, vector<Point2d> &points);
-
-	std::vector<cv::Point3d> object_points_;
-
-protected:
-	bool isVisible(size_t camera, size_t idx);
-	bool isValid(size_t camera, size_t idx);
-	bool isValid(size_t idx);
-
-	Point3d getPoint3D(size_t camera, size_t i);
-
-	vector<Point2d> getPoints(size_t camera, size_t idx);
-	vector<vector<Point2d>> getAllPoints(size_t camera, vector<size_t> idx);
-
-	void getVisiblePoints(	vector<size_t> cameras,
-							vector<vector<Point2d>> &points,
-							vector<size_t> &idx);
-
-	size_t getVisiblePointsCount(vector<size_t> cameras) {
-		// TODO: for pairs can use visibility graph adjacency matrix
-		vector<vector<Point2d>> points2d;
-		vector<size_t> idx;
-		getVisiblePoints(cameras, points2d, idx);
-		return idx.size();
-	}
-
-	size_t getTotalPointsCount() {
-		return points2d_[0].size();
-	}
-
-	vector<Point3d> getPoints3D(size_t idx);
-
-	/* @brief	Find points which are visible on all cameras. Returns
-	 * 			corresponding indices in idx vector.
-	 */
-	void getVisiblePoints3D(vector<size_t> cameras,
-							vector<vector<Point3d>> &points,
-							vector<size_t> &idx);
-
-	/* @brief	Update 3D points with new values. If no earlier data, new data
-	 *			is used as is, otherwise calculates average.
-	 */
-	void updatePoints3D(size_t camera, Point3d new_point, size_t idx, const Mat &R, const Mat &t);
-	void updatePoints3D(size_t camera, vector<Point3d> new_points, vector<size_t> idx, const Mat &R, const Mat &t);
-
-	/* @brief	Calculates 3D points that are not visible in reference camera
-	 *			from transformations in visible cameras.
-	 */
-	void calculateMissingPoints3D();
-
-	void getTransformation(size_t camera_from, size_t camera_to, Mat &R, Mat &T);
-	double calibratePair(size_t camera_from, size_t camera_to, Mat &R, Mat &T);
-
-	/* @brief	Calculate reprojection error of visible points (triangulation) */
-	double getReprojectionError(size_t c_from, size_t c_to, const Mat &K, const Mat &R, const Mat &T);
-
-	/* @brief	Calculate reprojection error of visible points (optimized/averaged points) */
-	double getReprojectionErrorOptimized(size_t c_from, const Mat &K, const Mat &R, const Mat &T);
-
-	/* @brief	Remove old calibration data calculated by calibrateAll */
-	void reset();
-
-	
-private:
-	CalibrationTarget target_;
-	Visibility visibility_graph_; 
-
-	bool is_calibrated_;
-	size_t n_cameras_;
-	size_t reference_camera_;
-	size_t min_visible_points_;
-	int fix_intrinsics_;
-
-	Size resolution_;
-	vector<Mat> K_;
-	vector<Mat> dist_coeffs_;
-	vector<Mat> R_;
-	vector<Mat> t_;
-
-	vector<Point3d> points3d_optimized_;
-	vector<vector<Point3d>> points3d_;
-	vector<vector<Point2d>> points2d_;
-	vector<vector<int>> visible_;
-	vector<vector<int>> inlier_; // "inlier"
-	vector<vector<double>> weights_;
-
-	int fm_method_;
-	double fm_ransac_threshold_;
-	double fm_confidence_;
-};

applications/calibration-multi/src/util.cpp

-#include "util.hpp"
-
-#include <loguru.hpp>
-
-#include <opencv2/core.hpp>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/imgproc.hpp>
-#include <opencv2/aruco.hpp>
-
-using std::vector;
-
-using cv::Mat;
-using cv::Point2i;
-using cv::Point2d;
-using cv::Point3d;
-using cv::Size;
-using cv::Scalar;
-
-/* @brief	Visualize epipolar lines for given points in the other image.
- * @param	Points in image
- * @param	Corresponding image where to draw the lines
- * @param	Fundamental matrix
- * @param	Line color
- * @param	Which image (1 or 2), see OpenCV's computeCorrespondEpilines()
- */
-void drawEpipolarLines(vector<Point2d> const &points, Mat &img, Mat const &F, Scalar color, int image) {
-	Mat lines;
-	cv::computeCorrespondEpilines(points, image, F, lines);
-
-	for (int i = 0; i < lines.rows; i++) {
-		cv::Vec3f l = lines.at<cv::Vec3f>(i);
-		float a = l[0];
-		float b = l[1];
-		float c = l[2];
-		float x0, y0, x1, y1;
-		x0 = 0;
-		y0 = (-c -a * x0) / b;
-		x1 = img.cols;
-		y1 = (-c -a * x1) / b;
-		cv::line(img, cv::Point(x0, y0), cv::Point(x1,y1), color, 1);
-	}
-}
-
-/* @breif	Find calibration points. AruCo markers, two per image.
- *			visible parameter input/ouput
- */
-int findCorrespondingPoints(vector<Mat> imgs, vector<vector<Point2d>> &points,
-							vector<int> &visible) {
-	using namespace cv;
-	int count = 0;
-
-	visible.resize(imgs.size(), 1);
-
-	points.clear();
-	points.resize(imgs.size(), vector<Point2d>(2, Point2d(0.0, 0.0)));
-
-	auto dictionary = aruco::getPredefinedDictionary(aruco::DICT_5X5_50);
-	vector<vector<Point2f>> corners;
-	vector<int> ids;
-	
-	for (size_t i = 0; i < imgs.size(); i++) {
-		if (visible[i] == 0) continue;
-
-		aruco::detectMarkers(imgs[i], dictionary, corners, ids);
-		if (corners.size() == 2) {
-			Point2d center0((corners[0][0] + corners[0][1] + corners[0][2] + corners[0][3]) / 4.0);
-			Point2d center1((corners[1][0] + corners[1][1] + corners[1][2] + corners[1][3]) / 4.0);
-			if (ids[0] != 0) { std::swap(center0, center1); }
-
-			points[i][0] = center0; points[i][1] = center1;
-			visible[i] = 1;
-
-			count++;
-		}
-		else {
-			visible[i] = 0;
-		}
-	}
-
-	return count;
-}
-
-/* @brief	Find AruCo marker centers.
- * @param	(input) image
- * @param	(output) found centers
- * @param	(output) marker IDs
- */
-void findMarkerCenters(Mat &img, vector<Point2d> &points, vector<int> &ids, int dict) {
-	using namespace cv;
-
-	points.clear();
-
-	auto dictionary = aruco::getPredefinedDictionary(dict);
-	vector<vector<Point2f>> corners;
-
-	aruco::detectMarkers(img, dictionary, corners, ids);
-	for (size_t j = 0; j < corners.size(); j++) {
-		Point2f center((corners[j][0] + corners[j][1] + corners[j][2] + corners[j][3]) / 4.0);
-		points.push_back(center);
-	}
-}
-
-/* OpenCV's recoverPose() expects both cameras to have identical intrinsic
- * parameters.
- */
-int recoverPose(Mat &E, vector<Point2d> &_points1, vector<Point2d> &_points2,
-				Mat &_cameraMatrix1, Mat &_cameraMatrix2,
-				Mat &_R, Mat &_t, double distanceThresh,
-				Mat &triangulatedPoints) {
-
-	Mat points1, points2, cameraMatrix1, cameraMatrix2, cameraMatrix;
-	
-	Mat(_points1.size(), 2, CV_64FC1, _points1.data()).convertTo(points1, CV_64F);
-	Mat(_points2.size(), 2, CV_64FC1, _points2.data()).convertTo(points2, CV_64F);
-	_cameraMatrix1.convertTo(cameraMatrix1, CV_64F);
-	_cameraMatrix2.convertTo(cameraMatrix2, CV_64F);
-	cameraMatrix = Mat::eye(Size(3, 3), CV_64FC1);
-
-	double fx1 = cameraMatrix1.at<double>(0,0);
-	double fy1 = cameraMatrix1.at<double>(1,1);
-	double cx1 = cameraMatrix1.at<double>(0,2);
-	double cy1 = cameraMatrix1.at<double>(1,2);
-
-	double fx2 = cameraMatrix2.at<double>(0,0);
-	double fy2 = cameraMatrix2.at<double>(1,1);
-	double cx2 = cameraMatrix2.at<double>(0,2);
-	double cy2 = cameraMatrix2.at<double>(1,2);
-
-	points1.col(0) = (points1.col(0) - cx1) / fx1;
-	points1.col(1) = (points1.col(1) - cy1) / fy1;
-
-	points2.col(0) = (points2.col(0) - cx2) / fx2;
-	points2.col(1) = (points2.col(1) - cy2) / fy2;
-
-	// TODO mask
-	// cameraMatrix = I (for details, see OpenCV's recoverPose() source code)
-	// modules/calib3d/src/five-point.cpp (461)
-	//
-	// https://github.com/opencv/opencv/blob/371bba8f54560b374fbcd47e7e02f015ac4969ad/modules/calib3d/src/five-point.cpp#L461
-
-	return cv::recoverPose(E, points1, points2, cameraMatrix, _R, _t, distanceThresh, cv::noArray(), triangulatedPoints);
-}
-
-/* @brief	Calculate RMS reprojection error
- * @param	3D points
- * @param	Expected 2D points
- * @param	Camera matrix
- * @param	Rotation matrix/vector
- * @param	Translation vector
- */
-double reprojectionError(	const vector<Point3d> &points3d, const vector<Point2d> &points2d,
-							const Mat &K, const Mat &rvec, const Mat &tvec) {
-	
-	DCHECK(points3d.size() == points2d.size());
-	
-	Mat _rvec;
-	if (rvec.size() == Size(3, 3)) { cv::Rodrigues(rvec, _rvec); }
-	else { _rvec = rvec; }
-
-	DCHECK(_rvec.size() == Size(1, 3) || _rvec.size() == Size(3, 1));
-
-	vector<Point2d> points_reprojected;
-	cv::projectPoints(points3d, _rvec, tvec, K, cv::noArray(), points_reprojected);
-	
-	int n_points = points2d.size();
-	double err = 0.0;
-
-	for (int i = 0; i < n_points; i++) {
-		Point2d a = points2d[i] - points_reprojected[i];
-		err += a.x * a.x + a.y * a.y;
-	}
-
-	return sqrt(err / n_points);
-}
\ No newline at end of file

applications/calibration-multi/src/util.hpp

-#pragma once
-
-#include <loguru.hpp>
-
-#include <opencv2/core.hpp>
-#include <opencv2/aruco.hpp>
-
-using std::vector;
-
-using cv::Mat;
-using cv::Point2i;
-using cv::Point2d;
-using cv::Point3d;
-using cv::Size;
-using cv::Scalar;
-
-/* @brief	Visualize epipolar lines for given points in the other image.
- * @param	Points in image
- * @param	Corresponding image where to draw the lines
- * @param	Fundamental matrix
- * @param	Line color
- * @param	Which image (1 or 2), see OpenCV's computeCorrespondEpilines()
- */
-void drawEpipolarLines(vector<Point2d> const &points, Mat &img, Mat const &F, Scalar color, int image=1);
-
-
-/* @breif	Find calibration points. AruCo markers, two per image.
- */
-int findCorrespondingPoints(vector<Mat> imgs, vector<vector<Point2d>> &points,
-							vector<int> &visible);
-
-/* @brief	Find AruCo marker centers.
- * @param	(input) image
- * @param	(output) found centers
- * @param	(output) marker IDs
- */
-void findMarkerCenters(Mat &img, vector<Point2d> &points, vector<int> &ids, int dict=cv::aruco::DICT_4X4_50);
-
-/* OpenCV's recoverPose() expects both cameras to have identical intrinsic
- * parameters.
- * 
- * https://github.com/opencv/opencv/blob/371bba8f54560b374fbcd47e7e02f015ac4969ad/modules/calib3d/src/five-point.cpp#L461
- */
-int recoverPose(Mat &E, vector<Point2d> &_points1, vector<Point2d> &_points2,
-				Mat &_cameraMatrix1, Mat &_cameraMatrix2,
-				Mat &_R, Mat &_t, double distanceThresh,
-				Mat &triangulatedPoints);
-
-/* @brief	Calculate RMS reprojection error
- * @param	3D points
- * @param	Expected 2D points
- * @param	Camera matrix
- * @param	Rotation matrix/vector
- * @param	Translation vector
- */
-double reprojectionError(	const vector<Point3d> &points3d, const vector<Point2d> &points2d,
-							const Mat &K, const Mat &rvec, const Mat &tvec);
-
-inline double euclideanDistance(Point3d a, Point3d b) {
-	Point3d c = a - b;
-	return sqrt(c.x*c.x + c.y*c.y + c.z*c.z);
-}
-
-inline Point3d transformPoint(Point3d p, Mat R, Mat t) {
-	DCHECK(R.size() == Size(3, 3));
-	DCHECK(t.size() == Size(1, 3));
-	return Point3d(Mat(R * Mat(p) + t));
-}
-
-inline Point3d inverseTransformPoint(Point3d p, Mat R, Mat t) {
-	DCHECK(R.size() == Size(3, 3));
-	DCHECK(t.size() == Size(1, 3));
-	return Point3d(Mat(R.t() * (Mat(p) - t)));
-}
-
-inline Mat getMat4x4(const Mat &R, const Mat &t) {
-	DCHECK(R.size() == Size(3, 3));
-	DCHECK(t.size() == Size(1, 3));
-	Mat M = Mat::eye(Size(4, 4), CV_64FC1);
-	R.copyTo(M(cv::Rect(0, 0, 3, 3)));
-	t.copyTo(M(cv::Rect(3, 0, 1, 3)));
-	return M;
-}
-
-inline void getRT(const Mat RT, Mat &R, Mat &t) {
-	R = RT(cv::Rect(0, 0, 3, 3));
-	t = RT(cv::Rect(3, 0, 1, 3));
-}
-
-// calculate transforms from (R1, t1) to (R2, t2), where parameters
-// (R1, t1) and (R2, t2) map to same (target) coordinate system
-
-inline void calculateTransform(const Mat &R1, const Mat &T1, const Mat &R2, const Mat &T2, Mat &R, Mat &tvec, Mat &M) {
-	Mat M_src = getMat4x4(R1, T1);
-	Mat M_dst = getMat4x4(R2, T2);
-	M = M_dst.inv() * M_src;	
-	R = M(cv::Rect(0, 0, 3, 3));
-	tvec = M(cv::Rect(3, 0, 1, 3));
-}
-
-inline void calculateTransform(const Mat &R1, const Mat &T1, const Mat &R2, const Mat &T2,Mat &R, Mat &tvec) {
-	Mat M;
-	calculateTransform(R1, T1, R2, T2, R, tvec, M);
-}
-
-inline void calculateInverse(const Mat &R2, const Mat &T2, Mat &R, Mat &T) {
-	Mat R1 = Mat::eye(Size(3, 3), CV_64FC1);
-	Mat T1(Size(1, 3), CV_64FC1, Scalar(0.0));
-	calculateTransform(R1, T1, R2, T2, R, T);
-}
\ No newline at end of file

applications/calibration-multi/src/visibility.cpp

-#include <numeric>
-#include <loguru.hpp>
-#include <queue>
-
-#include "visibility.hpp"
-
-using cv::Mat;
-using cv::Scalar;
-using cv::Size;
-using std::vector;
-using std::pair;
-using std::make_pair;
-
-Visibility::Visibility(int n_cameras) : n_cameras_(n_cameras) {
-	visibility_ = Mat(Size(n_cameras, n_cameras), CV_32SC1, Scalar(0));
-	count_ = vector(n_cameras, 0);
-}
-
-void Visibility::update(vector<int> &visible) {
-	DCHECK(visible.size() == (size_t) n_cameras_);
-
-	for (int i = 0; i < n_cameras_; i++) {
-		if (visible[i] == 0) continue;
-		count_[i]++;
-
-		for (int j = 0; j < n_cameras_; j++) {
-			if (i == j) continue;
-			if (visible[j] == 1) visibility_.at<int>(i, j)++;
-		}
-	}
-}
-
-int Visibility::getOptimalCamera() {
-	// most visible on average
-	int best_i = 0;
-	double best_score = -INFINITY;
-	for (int i = 0; i < visibility_.rows; i++) {
-		double score = 0.0;
-		for (int x = 0; x < visibility_.cols; x++) {
-			score += visibility_.at<int>(i, x);
-		}
-		score = score / (double) visibility_.cols;
-		if (score > best_score) {
-			best_i = i;
-			best_score = score;
-		}
-	}
-	
-	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_count = INT_MAX;
-
-	for (int i = 0; i < n_cameras_; i++) {
-		if (count_[i] < min_count) {
-			min_count = count_[i];
-		}
-	}
-	
-	return min_count;
-}
-
-int Visibility::getViewsCount(int camera) {
-	return count_[camera];
-}
-
-vector<vector<pair<int, int>>> Visibility::findShortestPaths(int reference) {
-	DCHECK(reference < n_cameras_);
-
-	vector<vector<pair<int, int>>> res(n_cameras_);
-	for (int i = 0; i < n_cameras_; i++) {
-		res[i] = findShortestPath(i, reference);
-	}
-	
-	return res;
-}
-
-vector<pair<int, int>> Visibility::findShortestPath(int from, int to) {
-	if (from == to) return vector<pair<int, int>>();
-
-	vector<bool> visited(n_cameras_, false);
-	vector<double> distances(n_cameras_, INFINITY);
-	vector<int> previous(n_cameras_, -1);
-	
-	distances[from] = 0.0;
-
-	auto cmp = [](pair<int, double> u, pair<int, double> v) { return u.second > v.second; };
-	std::priority_queue<pair<int, double>, vector<pair<int, double>>, decltype(cmp)> pq(cmp);
-
-	pq.push(make_pair(from, distances[from]));
-
-	while(!pq.empty()) {
-		pair<int, double> current = pq.top();
-		pq.pop();
-
-		int current_id = current.first;
-		double current_distance = distances[current_id];
-
-		visited[current_id] = true;
-
-		for (int i = 0; i < n_cameras_; i++) {
-			int count = visibility_.at<int>(current_id, i);
-			if (count == 0) continue; // not connected
-
-			double distance = 1.0 / (double) count;
-			double new_distance = current_distance + distance;
-			
-			if (distances[i] > new_distance) {
-				distances[i] = new_distance;
-				previous[i] = current_id;
-
-				pq.push(make_pair(i, distances[i]));
-			}
-		}
-	}
-
-	vector<pair<int, int>> res;
-	int prev = previous[to];
-	int current = to;
-
-	do {
-		res.push_back(make_pair(current, prev));
-		current = prev;
-		prev = previous[prev];
-	}
-	while(prev != -1);
-
-	std::reverse(res.begin(), res.end());
-	return res;
-}
-
-vector<int> Visibility::getClosestCameras(int c) {
-
-	// initialize original index locations
-	vector<int> idx(n_cameras_);
-	iota(idx.begin(), idx.end(), 0);
-	int* views = visibility_.ptr<int>(c);
-	
-	// sort indexes based on comparing values in v
-	sort(idx.begin(), idx.end(),
-		[views](size_t i1, size_t i2) {return views[i1] < views[i2];});
-
-	return idx;
-}
\ No newline at end of file

applications/calibration-multi/src/visibility.hpp

-#pragma once
-
-#include <opencv2/core.hpp>
-
-using cv::Mat;
-using std::vector;
-using std::pair;
-
-class Visibility {
-public:
-	explicit Visibility(int n_cameras);
-
-	/**
-	 * @breif	Update visibility graph.
-	 * @param	Which cameras see the feature(s) in this iteration
-	 */
-	void update(vector<int> &visible);
-
-	/**
-	 * @brief	For all cameras, find shortest (optimal) paths to reference
-	 * 			camera
-	 * @param	Id of reference camera
-	 * 
-	 * Calculates shortest path in weighted graph using Dijstra's
-	 * algorithm. Weights are inverse of views between cameras (nodes)
-	 * 
-	 * @todo	Add constant weight for each edge (prefer less edges)
-	 */
-	vector<vector<pair<int, int>>> findShortestPaths(int reference);
-
-	vector<int> getClosestCameras(int c);
-	void deleteEdge(int camera1, int camera2);
-
-	int getOptimalCamera();
-	
-	/** @brief Returns the smallest visibility count (any camera)
-	 */
-	int getMinVisibility();
-	
-	/** @brief Returns the visibility camera's value
-	 */
-	int getViewsCount(int camera);
-
-protected:
-	/**
-	 * @brief	Find shortest path between nodes
-	 * @param	Source node id
-	 * @param	Destination node id
-	 */
-	vector<pair<int, int>> findShortestPath(int from, int to);
-
-private:
-	int n_cameras_;		// @brief number of cameras
-	Mat visibility_;	// @brief adjacency matrix
-	vector<int> count_;
-};

applications/calibration/CMakeLists.txt

-set(CALIBSRC
-	src/main.cpp
-	src/lens.cpp
-	src/stereo.cpp
-	src/align.cpp
-	src/common.cpp
-)
-
-add_executable(ftl-calibrate ${CALIBSRC})
-
-target_include_directories(ftl-calibrate PRIVATE src)
-
-target_link_libraries(ftl-calibrate ftlcalibration ftlcommon Threads::Threads ${OpenCV_LIBS})
-
-

applications/calibration/src/align.cpp

-#include "align.hpp"
-
-#include <loguru.hpp>
-
-#include <opencv2/core.hpp>
-#include <opencv2/core/utility.hpp>
-#include <opencv2/imgproc.hpp>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/imgcodecs.hpp>
-#include <opencv2/videoio.hpp>
-#include <opencv2/highgui.hpp>
-
-using std::map;
-using std::string;
-using cv::Mat;
-using std::vector;
-using cv::Point2f;
-using cv::Size;
-
-struct Rec4f {
-	float left;
-	float right;
-	float top;
-	float bottom;
-};
-
-bool loadIntrinsicsMap(const std::string &ifile, const cv::Size &imageSize, Mat &map1, Mat &map2, Mat &cameraMatrix, float scale) {
-    using namespace cv;
-
-    FileStorage fs;
-
-    // reading intrinsic parameters
-        fs.open((ifile).c_str(), FileStorage::READ);
-        if (!fs.isOpened()) {
-            LOG(WARNING) << "Could not open intrinsics file : " << ifile;
-            return false;
-        }
-        
-        LOG(INFO) << "Intrinsics from: " << ifile;
-
-
-    Mat D1;
-    fs["M"] >> cameraMatrix;
-    fs["D"] >> D1;
-
-    //cameraMatrix *= scale;
-
-	initUndistortRectifyMap(cameraMatrix, D1, Mat::eye(3,3, CV_64F), cameraMatrix, imageSize, CV_16SC2,
-    		map1, map2);
-
-    return true;
-}
-
-inline bool hasOption(const map<string, string> &options, const std::string &opt) {
-    return options.find(opt) != options.end();
-}
-
-inline std::string getOption(map<string, string> &options, const std::string &opt) {
-    auto str = options[opt];
-    return str.substr(1,str.size()-2);
-}
-
-static const float kPI = 3.14159f;
-
-/*static float calculateZRotation(const vector<Point2f> &points, Size &boardSize) {
-    Point2f tl = points[boardSize.width * (boardSize.height / 2)];
-    Point2f tr = points[boardSize.width * (boardSize.height / 2) + boardSize.width-1];
-
-    float dx = tr.x - tl.x;
-    float dy = tr.y - tl.y;
-    float angle = atan2(dy,  dx) * (180.0f / kPI);
-    return angle;
-}
-
-static Point2f parallaxDistortion(const vector<Point2f> &points, Size &boardSize) {
-    Point2f tl = points[0];
-    Point2f tr = points[boardSize.width-1];
-    Point2f bl = points[(boardSize.height-1)*boardSize.width];
-    Point2f br = points[points.size()-1];
-
-    float dx1 = tr.x - tl.x;
-    float dx2 = br.x - bl.x;
-    float ddx = dx1 - dx2;
-
-    float dy1 = bl.y - tl.y;
-    float dy2 = br.y - tr.y;
-    float ddy = dy1 - dy2;
-
-    return Point2f(ddx, ddy);
-}*/
-
-static float distanceTop(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
-    Point2f tl = points[0];
-    Point2f tr = points[boardSize.width-1];
-
-    float pixSize = tr.x - tl.x;
-    float mmSize = boardSize.width * squareSize;
-    float focal = camMatrix.at<double>(0,0);
-
-    return ((mmSize / pixSize) * focal) / 1000.0f;
-}
-
-static float distanceBottom(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
-    Point2f bl = points[(boardSize.height-1)*boardSize.width];
-    Point2f br = points[points.size()-1];
-
-    float pixSize = br.x - bl.x;
-    float mmSize = boardSize.width * squareSize;
-    float focal = camMatrix.at<double>(0,0);
-
-    return ((mmSize / pixSize) * focal) / 1000.0f;
-}
-
-static float distanceLeft(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
-    Point2f bl = points[(boardSize.height-1)*boardSize.width];
-    Point2f tl = points[0];
-
-    float pixSize = bl.y - tl.y;
-    float mmSize = boardSize.height * squareSize;
-    float focal = camMatrix.at<double>(0,0);
-
-    return ((mmSize / pixSize) * focal) / 1000.0f;
-}
-
-static float distanceRight(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
-    Point2f tr = points[boardSize.width-1];
-    Point2f br = points[points.size()-1];
-
-    float pixSize = br.y - tr.y;
-    float mmSize = boardSize.height * squareSize;
-    float focal = camMatrix.at<double>(0,0);
-
-    return ((mmSize / pixSize) * focal) / 1000.0f;
-}
-
-static Rec4f distances(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
-	return {
-		-distanceLeft(camMatrix, points, boardSize, squareSize),
-		-distanceRight(camMatrix, points, boardSize, squareSize),
-		-distanceTop(camMatrix, points, boardSize, squareSize),
-		-distanceBottom(camMatrix, points, boardSize, squareSize)
-	};
-}
-
-/*static float distance(const Mat &camMatrix, const vector<Point2f> &points, Size &boardSize, float squareSize) {
-    Point2f tl = points[boardSize.width * (boardSize.height / 2)];
-    Point2f tr = points[boardSize.width * (boardSize.height / 2) + boardSize.width-1];
-
-    float pixSize = tr.x - tl.x;
-    float mmSize = boardSize.width * squareSize;
-    float focal = camMatrix.at<double>(0,0);
-
-    return ((mmSize / pixSize) * focal) / 1000.0f;
-}
-
-static Point2f diffY(const vector<Point2f> &pointsA, const vector<Point2f> &pointsB, Size &boardSize) {
-    Point2f tlA = pointsA[boardSize.width * (boardSize.height / 2)];
-    Point2f trA = pointsA[boardSize.width * (boardSize.height / 2) + boardSize.width-1];
-
-    Point2f tlB = pointsB[boardSize.width * (boardSize.height / 2)];
-    Point2f trB = pointsB[boardSize.width * (boardSize.height / 2) + boardSize.width-1];
-
-    float d1 = tlA.y - tlB.y;
-    float d2 = trA.y - trB.y;
-
-    return Point2f(d1,d2);
-}*/
-
-static const float kDistanceThreshold = 0.005f;
-
-
-static void showAnaglyph(const Mat &frame_l, const Mat &frame_r, Mat &img3d) {
-    using namespace cv;
-
-    float data[] = {0.299f, 0.587f, 0.114f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.299, 0.587, 0.114};
-    Mat m(2, 9, CV_32FC1, data);
-
-    //Mat img3d;
-
-    img3d = Mat(frame_l.size(), CV_8UC3);
-  
-    for (int y=0; y<img3d.rows; y++) {
-        unsigned char *row3d = img3d.ptr(y);
-        const unsigned char *rowL = frame_l.ptr(y);
-        const unsigned char *rowR = frame_r.ptr(y);
-
-        for (int x=0; x<img3d.cols*3; x+=3) {
-            const uchar lb = rowL[x+0];
-            const uchar lg = rowL[x+1];
-            const uchar lr = rowL[x+2];
-            const uchar rb = rowR[x+0];
-            const uchar rg = rowR[x+1];
-            const uchar rr = rowR[x+2];
-
-            row3d[x+0] = lb*m.at<float>(0,6) + lg*m.at<float>(0,7) + lr*m.at<float>(0,8) + rb*m.at<float>(1,6) + rg*m.at<float>(1,7) + rr*m.at<float>(1,8);
-            row3d[x+1] = lb*m.at<float>(0,3) + lg*m.at<float>(0,4) + lr*m.at<float>(0,5) + rb*m.at<float>(1,3) + rg*m.at<float>(1,4) + rr*m.at<float>(1,5);
-            row3d[x+2] = lb*m.at<float>(0,0) + lg*m.at<float>(0,1) + lr*m.at<float>(0,2) + rb*m.at<float>(1,0) + rg*m.at<float>(1,1) + rr*m.at<float>(1,2);
-        }
-    }
-
-    //imshow("Anaglyph", img3d);
-    //return img3d;
-}
-
-void ftl::calibration::align(map<string, string> &opt) {
-    using namespace cv;
-
-    float squareSize = 36.0f;
-
-    VideoCapture camA(0);
-    VideoCapture camB(1);
-
-    if (!camA.isOpened() || !camB.isOpened()) {
-        LOG(ERROR) << "Could not open a camera device";
-        return;
-    }
-
-    camA.set(cv::CAP_PROP_FRAME_WIDTH, 1280);  // TODO Use settings
-	camA.set(cv::CAP_PROP_FRAME_HEIGHT, 720);
-    camB.set(cv::CAP_PROP_FRAME_WIDTH, 1280);
-	camB.set(cv::CAP_PROP_FRAME_HEIGHT, 720);
-
-    Mat map1, map2, cameraMatrix;
-    Size imgSize(1280,720);
-    loadIntrinsicsMap((hasOption(opt, "profile")) ? getOption(opt,"profile") : "./panasonic.yml", imgSize, map1, map2, cameraMatrix, 1.0f);
-
-    Size boardSize(9,6);
-
-#if CV_VERSION_MAJOR >= 4
-	int chessBoardFlags = CALIB_CB_NORMALIZE_IMAGE;
-#else
-	int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
-
-	if (!settings.useFisheye) {
-		// fast check erroneously fails with high distortions like fisheye
-		chessBoardFlags |= CALIB_CB_FAST_CHECK;
-	}
-#endif
-
-    bool anaglyph = true;
-
-    while (true) {
-        Mat frameA, fA;
-        Mat frameB, fB;
-
-        camA.grab();
-        camB.grab();
-        camA.retrieve(frameA);
-        camB.retrieve(frameB);
-
-        remap(frameA, fA, map1, map2, INTER_LINEAR);
-        remap(frameB, fB, map1, map2, INTER_LINEAR);
-
-        // Get the chessboard
-        vector<Point2f> pointBufA;
-        vector<Point2f> pointBufB;
-		bool foundA, foundB;
-        foundA = findChessboardCornersSB(fA, boardSize,
-				pointBufA, chessBoardFlags);
-        foundB = findChessboardCornersSB(fB, boardSize,
-				pointBufB, chessBoardFlags);
-
-    // Step 1: Position cameras correctly with respect to chessboard
-    //      - print distance estimate etc
-
-    // Step 2: Show individual camera tilt degrees with left / right indicators
-
-    // Show also up down tilt perspective error
-
-    // Step 3: Display current baseline in mm
-
-        if (foundA) {
-            // Draw the corners.
-			//drawChessboardCorners(fA, boardSize,
-			//		Mat(pointBufA), foundA);
-        }
-
-        if (foundB) {
-            // Draw the corners.
-			//drawChessboardCorners(fB, boardSize,
-			//		Mat(pointBufB), foundB);
-        }
-
-        Mat anag;
-        showAnaglyph(fA, fB, anag);
-
-        if (foundA) {
-			Rec4f dists = distances(cameraMatrix, pointBufA, boardSize, squareSize);
-			//Rec4f angs = angles(pointBufA, boardSize);
-
-			// TODO Check angles also...
-			bool lrValid = std::abs(dists.left-dists.right) <= kDistanceThreshold;
-			bool tbValid = std::abs(dists.top-dists.bottom) <= kDistanceThreshold;
-			bool tiltUp = dists.top < dists.bottom && !tbValid;
-			bool tiltDown = dists.top > dists.bottom && !tbValid;
-			bool rotLeft = dists.left > dists.right && !lrValid;
-			bool rotRight = dists.left < dists.right && !lrValid;
-
-			// TODO Draw lines
-            Point2f bl = pointBufA[(boardSize.height-1)*boardSize.width];
-            Point2f tl = pointBufA[0];
-            Point2f tr = pointBufA[boardSize.width-1];
-            Point2f br = pointBufA[pointBufA.size()-1];
-
-
-            line(anag, tl, tr, (!lrValid && tiltUp) ? Scalar(0,0,255) : Scalar(0,255,0));
-            line(anag, bl, br, (!lrValid && tiltDown) ? Scalar(0,0,255) : Scalar(0,255,0));
-            line(anag, tl, bl, (!tbValid && rotLeft) ? Scalar(0,0,255) : Scalar(0,255,0));
-            line(anag, tr, br, (!tbValid && rotRight) ? Scalar(0,0,255) : Scalar(0,255,0));
-
-            //fA = fA(Rect(tl.x - 100, tl.y- 100, br.x-tl.x + 200, br.y-tl.y + 200));
-
-			// Show distance error between cameras
-
-			// Show estimated baseline
-
-            //if (step == 0) {
-            //    Point2f pd = parallaxDistortion(pointBufA, boardSize);
-            //    putText(fA, string("Distort: ") + std::to_string(pd.x) + string(",") + std::to_string(pd.y), Point(10,50), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
-            //} else if (step == 1) {
-                //float d = distance(cameraMatrix, pointBufA, boardSize, squareSize);
-                //putText(anag, string("Distance: ") + std::to_string(-d) + string("m"), Point(10,50), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
-           // } else if (step == 2) {
-                //float angle = calculateZRotation(pointBufA, boardSize) - 180.0f;
-                //putText(anag, string("Angle: ") + std::to_string(angle), Point(10,150), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
-            //} else if (step == 3) {
-            //    Point2f vd = diffY(pointBufA, pointBufB, boardSize);
-            //    putText(fA, string("Vertical: ") + std::to_string(vd.x) + string(",") + std::to_string(vd.y), Point(10,200), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
-           // }
-
-            if (foundB) {
-                //if (step == 0) {
-                    //Point2f pd = parallaxDistortion(pointBufB, boardSize);
-                    //putText(fB, string("Distort: ") + std::to_string(pd.x) + string(",") + std::to_string(pd.y), Point(10,50), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
-                //} else if (step == 1) {
-                    //float d = distance(cameraMatrix, pointBufB, boardSize, squareSize);
-                    //putText(fB, string("Distance: ") + std::to_string(-d) + string("m"), Point(10,100), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
-                //} else if (step == 2) {
-                    //float angle = calculateZRotation(pointBufB, boardSize) - 180.0f;
-                    //putText(fB, string("Angle: ") + std::to_string(angle), Point(10,150), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,0,255), 3);
-                //}
-
-                Rec4f dists = distances(cameraMatrix, pointBufB, boardSize, squareSize);
-                //Rec4f angs = angles(pointBufA, boardSize);
-
-                // TODO Check angles also...
-                bool lrValid = std::abs(dists.left-dists.right) <= kDistanceThreshold;
-                bool tbValid = std::abs(dists.top-dists.bottom) <= kDistanceThreshold;
-                bool tiltUp = dists.top < dists.bottom && !tbValid;
-                bool tiltDown = dists.top > dists.bottom && !tbValid;
-                bool rotLeft = dists.left > dists.right && !lrValid;
-                bool rotRight = dists.left < dists.right && !lrValid;
-
-                // TODO Draw lines
-                Point2f bbl = pointBufB[(boardSize.height-1)*boardSize.width];
-                Point2f btl = pointBufB[0];
-                Point2f btr = pointBufB[boardSize.width-1];
-                Point2f bbr = pointBufB[pointBufB.size()-1];
-
-
-                line(anag, btl, btr, (!lrValid && tiltUp) ? Scalar(0,0,255) : Scalar(0,255,0));
-                line(anag, bbl, bbr, (!lrValid && tiltDown) ? Scalar(0,0,255) : Scalar(0,255,0));
-                line(anag, btl, bbl, (!tbValid && rotLeft) ? Scalar(0,0,255) : Scalar(0,255,0));
-                line(anag, btr, bbr, (!tbValid && rotRight) ? Scalar(0,0,255) : Scalar(0,255,0));
-
-                float baseline1 = std::abs(tl.x - btl.x);
-                float baseline2 = std::abs(tr.x - btr.x);
-                float baseline3 = std::abs(bl.x - bbl.x);
-                float baseline4 = std::abs(br.x - bbr.x);
-                float boardWidth = (std::abs(tl.x-tr.x) + std::abs(btl.x - btr.x)) / 2.0f;
-                float baseline = ((baseline1 + baseline2 + baseline3 + baseline4) / 4.0f) / boardWidth * (boardSize.width*squareSize);
-
-                putText(anag, string("Baseline: ") + std::to_string(baseline) + string("mm"), Point(10,150), FONT_HERSHEY_PLAIN, 2.0, Scalar(0,255,0), 2);
-            }
-
-        }
-
-        /*if (anaglyph) {
-            showAnaglyph(fA,fB);
-        } else {
-            imshow("Left", fA);
-            imshow("Right", fB);
-        }*/
-        imshow("Anaglyph", anag);
-
-		char key = static_cast<char>(waitKey(20));
-		if (key  == 27)
-			break;
-        if (key == 32) anaglyph = !anaglyph;
-    }
-}
\ No newline at end of file

applications/calibration/src/align.hpp

-#ifndef _FTL_CALIBRATION_ALIGN_HPP_
-#define _FTL_CALIBRATION_ALIGN_HPP_
-
-#include <map>
-#include <string>
-
-namespace ftl {
-namespace calibration {
-
-void align(std::map<std::string, std::string> &opt);
-
-}
-}
-
-#endif  // _FTL_CALIBRATION_ALIGN_HPP_

applications/calibration/src/common.cpp

-#include <loguru.hpp>
-#include <ftl/config.h>
-
-#include <opencv2/calib3d.hpp>
-#include <opencv2/imgproc.hpp>
-#include <opencv2/videoio.hpp>
-#include <opencv2/highgui.hpp>
-
-#include "common.hpp"
-
-using std::vector;
-using std::map;
-using std::string;
-
-using cv::Mat;
-using cv::Vec2f, cv::Vec3f;
-using cv::Size;
-
-using cv::stereoCalibrate;
-
-namespace ftl {
-namespace calibration {
-
-// Options
-string getOption(const map<string, string> &options, const string &opt) {
-	const string str = options.at(opt);
-	return str.substr(1, str.size() - 2);
-}
-
-bool hasOption(const map<string, string> &options, const string &opt) {
-	return options.find(opt) != options.end();
-}
-
-int getOptionInt(const map<string, string> &options, const string &opt, int default_value) {
-	if (!hasOption(options, opt)) return default_value;
-	return std::stoi(options.at(opt));
-}
-
-double getOptionDouble(const map<string, string> &options, const string &opt, double default_value) {
-	if (!hasOption(options, opt)) return default_value;
-	return std::stod(options.at(opt));
-}
-
-string getOptionString(const map<string, string> &options, const string &opt, string default_value) {
-	if (!hasOption(options, opt)) return default_value;
-	return getOption(options, opt);
-}
-
-// Save/load files
-
-bool saveExtrinsics(const string &ofile, Mat &R, Mat &T, Mat &R1, Mat &R2, Mat &P1, Mat &P2, Mat &Q) {
-	cv::FileStorage fs;
-	fs.open(ofile, cv::FileStorage::WRITE);
-	if (fs.isOpened()) {
-		fs << "R" << R << "T" << T << "R1" << R1 << "R2" << R2 << "P1"
-			<< P1 << "P2" << P2 << "Q" << Q;
-		fs.release();
-		return true;
-	} else {
-		LOG(ERROR) << "Error: can not save the extrinsic parameters";
-	}
-	return false;
-}
-
-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())
-	{
-		fs << "resolution" << size;
-		fs << "K" << M << "D" << D;
-		fs.release();
-		return true;
-	}
-	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, Size &size) {
-	using namespace cv;
-
-	FileStorage fs;
-
-	// reading intrinsic parameters
-	fs.open((ifile).c_str(), FileStorage::READ);
-	if (!fs.isOpened()) {
-		LOG(WARNING) << "Could not open intrinsics file : " << ifile;
-		return false;
-	}
-	
-	LOG(INFO) << "Intrinsics from: " << ifile;
-
-	fs["resolution"] >> size;
-	fs["K"] >> K1;
-	fs["D"] >> D1;
-	
-	return true;
-}
-
-Grid::Grid(int rows, int cols, int width, int height, 
-		   int offset_x, int offset_y) {
-	rows_ = rows;
-	cols_ = cols;
-	width_ = width;
-	height_ = height;
-	offset_x_ = offset_x;
-	offset_y_ = offset_y;
-	cell_width_ = width_ / cols_;
-	cell_height_ = height_ / rows_;
-	reset();
-
-	corners_ = vector<std::pair<cv::Point, cv::Point>>();
-
-	for (int r = 0; r < rows_; r++) {
-	for (int c = 0; c < cols_; c++) {
-		int x1 = offset_x_ + c * cell_width_;
-		int y1 = offset_y_ + r * cell_height_;
-		int x2 = offset_x_ + (c + 1) * cell_width_ - 1;
-		int y2 = offset_y_ + (r + 1) * cell_height_ - 1;
-		corners_.push_back(std::pair(cv::Point(x1, y1), cv::Point(x2, y2)));
-	}}
-}
-
-void Grid::drawGrid(Mat &rgb) {
-	for (int i = 0; i < rows_ * cols_; ++i) {	
-		bool visited = visited_[i];
-		cv::Scalar color = visited ? cv::Scalar(24, 255, 24) : cv::Scalar(24, 24, 255);
-		cv::rectangle(rgb, corners_[i].first, corners_[i].second, color, 2);
-	}
-}
-
-int Grid::checkGrid(cv::Point p1, cv::Point p2) {
-	// TODO calculate directly
-
-	for (int i = 0; i < rows_ * cols_; ++i) {
-		auto &corners = corners_[i];
-		if (p1.x >= corners.first.x &&
-			p1.x <= corners.second.x &&
-			p1.y >= corners.first.y &&
-			p1.y <= corners.second.y && 
-			p2.x >= corners.first.x &&
-			p2.x <= corners.second.x &&
-			p2.y >= corners.first.y &&
-			p2.y <= corners.second.y) {
-			return i;
-		}
-	}
-
-	return -1;
-}
-
-void Grid::updateGrid(int i) {
-	if (i >= 0 && i < static_cast<int>(visited_.size()) && !visited_[i]) {
-		visited_[i] = true;
-		visited_count_ += 1;
-	}
-}
-
-bool Grid::isVisited(int i) {
-	if (i >= 0 && i < static_cast<int>(visited_.size())) {
-		return visited_[i];
-	}
-	return false;
-}
-
-bool Grid::isComplete() {
-	return visited_count_ == static_cast<int>(visited_.size());
-}
-
-void Grid::reset() {
-	visited_count_ = 0;
-	visited_ = vector<bool>(rows_ * cols_, false);
-	// reset visited
-}
-
-// Calibration classes for different patterns
-
-CalibrationChessboard::CalibrationChessboard(const map<string, string> &opt) {
-	pattern_size_ = Size(	getOptionInt(opt, "cols", 9),
-							getOptionInt(opt, "rows", 6));
-	image_size_ = Size(	getOptionInt(opt, "width", 1280),
-						getOptionInt(opt, "height", 720));
-	pattern_square_size_ = getOptionDouble(opt, "square_size", 0.036);
-
-	LOG(INFO) << "Chessboard calibration parameters";
-	LOG(INFO) << "         rows: " << pattern_size_.height;
-	LOG(INFO) << "         cols: " << pattern_size_.width;
-	LOG(INFO) << "        width: " << image_size_.width;
-	LOG(INFO) << "       height: " << image_size_.height;
-	LOG(INFO) << "  square_size: " << pattern_square_size_;
-	LOG(INFO) << "-----------------------------------";
-
-	// From OpenCV (4.1.0) Documentation
-	//
-	// CALIB_CB_NORMALIZE_IMAGE	Normalize the image gamma with equalizeHist before detection.
-	// CALIB_CB_EXHAUSTIVE		Run an exhaustive search to improve detection rate.
-	// CALIB_CB_ACCURACY		Up sample input image to improve sub-pixel accuracy due to
-	//							aliasing effects. This should be used if an accurate camera
-	//							calibration is required.
-
-	chessboard_flags_ = cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_ACCURACY;
-}
-
-void CalibrationChessboard::objectPoints(vector<Vec3f> &out) {
-	out.reserve(pattern_size_.width * pattern_size_.height);
-	for (int row = 0; row < pattern_size_.height; ++row) {
-	for (int col = 0; col < pattern_size_.width; ++col) {
-		out.push_back(Vec3f(col * pattern_square_size_, row * pattern_square_size_, 0));
-	}}
-}
-
-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);
-}
-
-}
-}

applications/calibration/src/common.hpp

-#ifndef _FTL_CALIBRATION_COMMON_HPP_
-#define _FTL_CALIBRATION_COMMON_HPP_
-
-#include <map>
-#include <string>
-
-#include <opencv2/core.hpp>
-
-namespace ftl {
-namespace calibration {
-
-std::string getOption(const std::map<std::string, std::string> &options, const std::string &opt);
-bool hasOption(const std::map<std::string, std::string> &options, const std::string &opt);
-int getOptionInt(const std::map<std::string, std::string> &options, const std::string &opt, int default_value);
-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, 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);
-
-class Grid {
-private:
-	int rows_;
-	int cols_;
-	int width_;
-	int height_;
-	int cell_width_;
-	int cell_height_;
-	int offset_x_;
-	int offset_y_;
-	int visited_count_;
-
-	std::vector<std::pair<cv::Point, cv::Point>> corners_;
-	std::vector<bool> visited_;
-
-public:
-	Grid(int rows, int cols, int width, int height, int offset_x, int offset_y);
-	void drawGrid(cv::Mat &rgb);
-	int checkGrid(cv::Point p1, cv::Point p2);
-	void updateGrid(int i);
-	bool isVisited(int i);
-	bool isComplete();
-	void reset(); 
-};
-
-/**
- * @brief	Wrapper for OpenCV's calibration methods. Paramters depend on
- * 			implementation (different types of patterns).
- *
- * Calibration objects may store state; eg. from previous views of calibration
- * images.
- */
-class Calibration {
-public:
-	/**
-	 * @brief	Calculate reference points for given pattern
-	 * @param	Output parameter
-	 */
-	void objectPoints(std::vector<cv::Vec3f> &out);
-
-	/**
-	 * @brief	Try to find calibration pattern in input image
-	 * @param	Input image
-	 * @param	Output parameter for found point image coordinates
-	 * @returns	true if pattern found, otherwise false
-	 */
-	bool findPoints(cv::Mat &in, std::vector<cv::Vec2f> &out);
-
-	/**
-	 * @brief	Draw points to image
-	 * @param	Image to draw to
-	 * @param	Pattern points (in image coordinates)
-	 */
-	void drawPoints(cv::Mat &img, const std::vector<cv::Vec2f> &points);
-};
-
-/**
- * @brief	Chessboard calibration pattern. Uses OpenCV's
- * 			findChessboardCornersSB function.
- * @todo	Parameters hardcoded in constructor
- *
- * All parameters (command line parameters):
- * 	- rows, cols: pattern size (inner corners)
- * 	- square_size: millimeters (TODO: meters)
- * 	- width, height: image size, pixels
- * 	- flags: see ChessboardCornersSB documentation (TODO: not implemented)
- */
-class CalibrationChessboard : Calibration {
-public:
-	explicit CalibrationChessboard(const std::map<std::string, std::string> &opt);
-	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;
-	float pattern_square_size_;
-	cv::Size pattern_size_;
-	cv::Size image_size_;
-};
-
-// TODO other patterns, circles ...
-
-}
-}
-
-#endif // _FTL_CALIBRATION_COMMON_HPP_

applications/calibration/src/lens.cpp

-#include "common.hpp"
-#include "lens.hpp"
-
-#include <ftl/config.h>
-#include <ftl/calibration/parameters.hpp>
-
-#include <loguru.hpp>
-
-#include <opencv2/core.hpp>
-#include <opencv2/core/utility.hpp>
-#include <opencv2/imgproc.hpp>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/imgcodecs.hpp>
-#include <opencv2/videoio.hpp>
-#include <opencv2/highgui.hpp>
-
-#include <vector>
-#include <atomic>
-#include <thread>
-
-using std::map;
-using std::string;
-using std::vector;
-
-using cv::Mat;
-using cv::Vec2f;
-using cv::Vec3f;
-using cv::Size;
-
-using namespace ftl::calibration;
-
-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", 1920),
-							getOptionInt(opt, "height", 1080));
-	const int n_cameras = getOptionInt(opt, "n_cameras", 2);
-	const int iter = getOptionInt(opt, "iter", 20);
-	const int delay = getOptionInt(opt, "delay", 1000);
-	const double aperture_width = getOptionDouble(opt, "aperture_width", 6.2);
-	const double aperture_height = getOptionDouble(opt, "aperture_height", 4.6);
-	const string filename_intrinsics = getOptionString(opt, "profile", FTL_LOCAL_CONFIG_ROOT "/calibration.yml");
-	CalibrationChessboard calib(opt);
-	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 << "\n";
-	LOG(WARNING) << "WARNING: This application overwrites existing files and does not previous values!";
-	//LOG(INFO) << "  use_guess_distortion: " << use_guess_distortion;
-
-	LOG(INFO) << "-----------------------------------";
-
-	// assume no tangential and thin prism distortion and only estimate first
-	// three radial distortion coefficients
-	
-	int calibrate_flags = 	cv::CALIB_FIX_K4 | cv::CALIB_FIX_K5 | cv::CALIB_FIX_K6 |
-							cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_S1_S2_S3_S4 | cv::CALIB_FIX_ASPECT_RATIO;
-
-	vector<Mat> camera_matrix(n_cameras), dist_coeffs(n_cameras);
-
-	for (Mat &d : dist_coeffs) {
-		d = Mat(Size(8, 1), CV_64FC1, cv::Scalar(0.0));
-	}
-
-	if (use_guess) {
-		camera_matrix.clear();
-		vector<Mat> tmp;
-		Size tmp_size;
-		
-		loadIntrinsics(filename_intrinsics, camera_matrix, tmp, tmp_size);
-		CHECK(camera_matrix.size() == static_cast<unsigned int>(n_cameras));
-
-		if ((tmp_size != image_size) && (!tmp_size.empty())) {
-			for (Mat &K : camera_matrix) {
-				K = ftl::calibration::scaleCameraMatrix(K, image_size, tmp_size);
-			}
-		}
-
-		if (tmp_size.empty()) {
-			LOG(ERROR) << "No valid calibration found.";
-		}
-		else {
-			calibrate_flags |= cv::CALIB_USE_INTRINSIC_GUESS;
-		}
-	}
-
-	vector<cv::VideoCapture> cameras;
-	cameras.reserve(n_cameras);
-
-	for (int c = 0; c < n_cameras; c++) { cameras.emplace_back(c); }
-	for (auto &camera : cameras)
-	{
-		if (!camera.isOpened()) {
-			LOG(ERROR) << "Could not open camera device";
-			return;
-		}
-		camera.set(cv::CAP_PROP_FRAME_WIDTH, image_size.width); 
-		camera.set(cv::CAP_PROP_FRAME_HEIGHT, image_size.height);
-	}
-
-	vector<vector<vector<Vec2f>>> image_points(n_cameras);
-	vector<vector<vector<Vec3f>>> object_points(n_cameras);
-	
-	vector<Mat> img(n_cameras);
-	vector<Mat> img_display(n_cameras);
-	vector<int> count(n_cameras, 0);
-	Mat display(Size(image_size.width * n_cameras, image_size.height), CV_8UC3);
-
-	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));
-	}
-
-	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;
-			}
-
-			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);
-			}
-
-			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++) {
-			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::namedWindow("Cameras", cv::WINDOW_KEEPRATIO | cv::WINDOW_NORMAL);
-		cv::imshow("Cameras", display);
-
-		cv::waitKey(10);
-	}
-
-	cv::destroyAllWindows();
-	
-	//bool calib_ok = true;
-
-	for (int c = 0; c < n_cameras; c++) {
-		LOG(INFO) << "Calculating intrinsic paramters for camera " << std::to_string(c);
-		vector<Mat> rvecs, tvecs;
-		
-		double rms = cv::calibrateCamera(
-							object_points[c], image_points[c],
-							image_size, camera_matrix[c], dist_coeffs[c],
-							rvecs, tvecs, calibrate_flags
-		);
-
-		LOG(INFO) << "final reprojection RMS error: " << rms;
-
-		if (!ftl::calibration::validate::distortionCoefficients(dist_coeffs[c], image_size)) {
-			LOG(ERROR)	<< "Calibration failed: invalid distortion coefficients:\n" 
-						<< dist_coeffs[c];
-			
-			LOG(WARNING) << "Estimating only intrinsic parameters for camera " << std::to_string(c);
-
-			dist_coeffs[c] = Mat(Size(8, 1), CV_64FC1, cv::Scalar(0.0));
-			calibrate_flags |=	cv::CALIB_FIX_K1 | cv::CALIB_FIX_K2 | cv::CALIB_FIX_K3;
-			c--;
-			continue;
-		}
-
-		//calib_ok = true;
-		calibrate_flags &=	~cv::CALIB_FIX_K1 & ~cv::CALIB_FIX_K2 & ~cv::CALIB_FIX_K3;
-
-		double fovx, fovy, focal_length, aspect_ratio;
-		cv::Point2d principal_point;
-
-		// TODO: check for valid aperture width/height; do not run if not valid values
-		cv::calibrationMatrixValues(camera_matrix[c], image_size, aperture_width, aperture_height,
-									fovx, fovy, focal_length, principal_point, aspect_ratio);
-		
-		LOG(INFO) << "";
-		LOG(INFO) << "            fovx (deg): " << fovx;
-		LOG(INFO) << "            fovy (deg): " << fovy;
-		LOG(INFO) << "     focal length (mm): " << focal_length;
-		LOG(INFO) << "  principal point (mm): " << principal_point;
-		LOG(INFO) << "          aspect ratio: " << aspect_ratio;
-		LOG(INFO) << "";
-		LOG(INFO) << "Camera matrix:\n" << camera_matrix[c];
-		LOG(INFO) << "Distortion coefficients:\n" << dist_coeffs[c];
-		LOG(INFO) << "";
-	}
-
-	saveIntrinsics(filename_intrinsics, camera_matrix, dist_coeffs, image_size);
-	LOG(INFO) << "intrinsic paramaters saved to: " << filename_intrinsics;
-	
-	vector<Mat> map1(n_cameras), map2(n_cameras);
-	for (int c = 0; c < n_cameras; c++) {
-		cv::initUndistortRectifyMap(camera_matrix[c], dist_coeffs[c], Mat::eye(3,3, CV_64F), camera_matrix[c],
-									image_size, CV_16SC2, map1[c], map2[c]);
-	}
-	
-	while (cv::waitKey(25) != 27) {
-		for (auto &camera : cameras ) {	camera.grab(); }
-		for (int c = 0; c < n_cameras; c++) {
-			cameras[c].retrieve(img[c]);
-			cv::remap(img[c], img[c], map1[c], map2[c], cv::INTER_CUBIC);
-			cv::imshow("Camera " + std::to_string(c), img[c]);
-		}
-	}
-}

applications/calibration/src/lens.hpp

-#ifndef _FTL_CALIBRATION_LENS_HPP_
-#define _FTL_CALIBRATION_LENS_HPP_
-
-#include <map>
-#include <string>
-
-namespace ftl {
-namespace calibration {
-
-void intrinsic(std::map<std::string, std::string> &opt);
-
-}
-}
-
-#endif  // _FTL_CALIBRATION_LENS_HPP_

applications/calibration/src/main.cpp

-#include <loguru.hpp>
-#include <ftl/configuration.hpp>
-
-#include "lens.hpp"
-#include "stereo.hpp"
-#include "align.hpp"
-
-int main(int argc, char **argv) {
-	loguru::g_preamble_date = false;
-	loguru::g_preamble_uptime = false;
-	loguru::g_preamble_thread = false;
-	loguru::init(argc, argv, "--verbosity");
-	argc--;
-	argv++;
-
-	// Process Arguments
-	auto options = ftl::config::read_options(&argv, &argc);
-
-	ftl::calibration::intrinsic(options);
-
-	return 0;
-}

applications/calibration/src/stereo.cpp

-#include <loguru.hpp>
-#include <ftl/config.h>
-
-#include <opencv2/imgproc.hpp>
-#include <opencv2/videoio.hpp>
-#include <opencv2/highgui.hpp>
-
-#include "common.hpp"
-#include "stereo.hpp"
-
-using std::vector;
-using std::map;
-using std::string;
-
-using cv::Mat;
-using cv::Vec2f, cv::Vec3f;
-using cv::Size;
-
-using cv::stereoCalibrate;
-
-using namespace ftl::calibration;
-
-void ftl::calibration::stereo(map<string, string> &opt) {
-	LOG(INFO) << "Begin stereo calibration";
-
-	// TODO PARAMETERS TO CONFIG FILE
-	// image size, also used by CalibrationChessboard
-	Size image_size = Size(	getOptionInt(opt, "width", 1280),
-							getOptionInt(opt, "height", 720));
-	// iterations
-	int iter = getOptionInt(opt, "iter", 50);
-	// delay between images
-	double delay = getOptionInt(opt, "delay", 250);
-	// max_error for a single image; if error larger image discarded
-	double max_error = getOptionDouble(opt, "max_error", 1.0);
-	// scaling/cropping (see OpenCV stereoRectify())
-	float alpha = getOptionDouble(opt, "alpha", 0);
-	// intrinsics filename
-	string filename_intrinsics = getOptionString(opt, "profile", "./panasonic.yml");
-
-	bool use_grid = (bool) getOptionInt(opt, "use_grid", 0);
-
-	LOG(INFO) << "Stereo calibration parameters";
-	LOG(INFO) << "     profile: " << filename_intrinsics;
-	LOG(INFO) << "       width: " << image_size.width;
-	LOG(INFO) << "      height: " << image_size.height;
-	LOG(INFO) << "        iter: " << iter;
-	LOG(INFO) << "       delay: " << delay;
-	LOG(INFO) << "   max_error: " << max_error;
-	LOG(INFO) << "       alpha: " << alpha;
-	LOG(INFO) << "    use_grid: " << use_grid;
-	LOG(INFO) << "-----------------------------------";
-
-	CalibrationChessboard calib(opt);
-	vector<Grid> grids;
-	int grid_i = 0;
-
-	// grid parameters, 3x3 grid; one small grid and one large grid. Grids are cycled until
-	// iter reaches zero
-	grids.push_back(Grid(3, 3,
-						(3.0f/4.0f) * image_size.width, (3.0f/4.0f) * image_size.height,
-						((1.0f/4.0f) * image_size.width) / 2, ((1.0f/4.0f) * image_size.height) / 2));
-
-	grids.push_back(Grid(3, 3, image_size.width, image_size.height, 0, 0));
-	Grid grid = grids[grid_i];
-
-	// PARAMETERS
-
-	int stereocalibrate_flags =
-		cv::CALIB_FIX_INTRINSIC | cv::CALIB_FIX_PRINCIPAL_POINT | cv::CALIB_FIX_ASPECT_RATIO |
-		cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_SAME_FOCAL_LENGTH | 
-		cv::CALIB_FIX_K3 | cv::CALIB_FIX_K4 | cv::CALIB_FIX_K5;
-
-	vector<cv::VideoCapture> cameras { cv::VideoCapture(0), cv::VideoCapture(1) };
-
-	for (auto &camera : cameras ) {
-		if (!camera.isOpened()) {
-			LOG(ERROR) << "Could not open camera device";
-			return;
-		}
-		camera.set(cv::CAP_PROP_FRAME_WIDTH, image_size.width); 
-		camera.set(cv::CAP_PROP_FRAME_HEIGHT, image_size.height);
-	}
-
-	// image points to calculate the parameters after all input data is captured
-	vector<vector<vector<Vec2f>>> image_points(2);
-	vector<vector<Vec3f>> object_points;
-
-	// image points for each grid, updated to image_points and object_points
-	// after is grid complete
-	vector<vector<vector<Vec2f>>> image_points_grid(9, vector<vector<Vec2f>>(2));
-	vector<vector<Vec3f>> object_points_grid(9);
-	
-	vector<Mat> dist_coeffs(2);
-	vector<Mat> camera_matrices(2);
-	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
-	while (iter > 0) {
-		int res = 0;
-		int grid_pos = -1;
-
-		vector<Mat> new_img(2);
-		vector<vector<Vec2f>> new_points(2);
-
-		int delay_remaining = delay;
-		for (; delay_remaining > 50; delay_remaining -= 50) {
-			cv::waitKey(50);
-
-			for (size_t i = 0; i < 2; i++) {
-				auto &camera = cameras[i];
-				auto &img = new_img[i];
-
-				camera.grab();
-				camera.retrieve(img);
-
-				if (use_grid && i == 0) grid.drawGrid(img);
-				cv::imshow("Camera " + std::to_string(i), img);
-			}
-		}
-
-		for (size_t i = 0; i < 2; i++) {
-			auto &img = new_img[i];
-			auto &points = new_points[i];
-
-			// TODO move to "findPoints"-thread
-			if (calib.findPoints(img, points)) {
-				calib.drawPoints(img, points);
-				res++;
-			}
-
-			cv::imshow("Camera " + std::to_string(i), img);
-		}
-
-		if (res != 2) { LOG(WARNING) << "Input not detected on all inputs"; continue; }
-		
-		if (use_grid) {
-			// top left and bottom right corners; not perfect but good enough
-			grid_pos = grid.checkGrid(
-				cv::Point(new_points[0][0]),
-				cv::Point(new_points[0][new_points[0].size()-1])
-			);
-
-			if (grid_pos == -1) { LOG(WARNING) << "Captured pattern not inside grid cell"; continue; }
-		}
-
-		vector<Vec3f> points_ref;
-		calib.objectPoints(points_ref);
-		
-		/* doesn't seem to be very helpful (error almost always low enough)
-		// calculate reprojection error with single pair of images
-		// reject it if RMS reprojection error too high
-		int flags = stereocalibrate_flags;
-		
-		double rms_iter = stereoCalibrate(
-					vector<vector<Vec3f>> { points_ref }, 
-					vector<vector<Vec2f>> { new_points[0] },
-					vector<vector<Vec2f>> { new_points[1] },
-					camera_matrices[0], dist_coeffs[0],
-					camera_matrices[1], dist_coeffs[1],
-					image_size, R, T, E, F, per_view_errors,
-					flags);
-		
-		LOG(INFO) << "rms for pattern: " << rms_iter;
-		if (rms_iter > max_error) {
-			LOG(WARNING) << "RMS reprojection error too high, maximum allowed error: " << max_error;
-			continue;
-		}*/
-		
-		if (use_grid) {
-			// store results in result grid
-			object_points_grid[grid_pos] = points_ref;
-			for (size_t i = 0; i < 2; i++) { image_points_grid[grid_pos][i] = new_points[i]; }
-			
-			grid.updateGrid(grid_pos);
-
-			if (grid.isComplete()) {
-				LOG(INFO) << "Grid complete";
-				grid.reset();
-				grid_i = (grid_i + 1) % grids.size();
-				grid = grids[grid_i];
-
-				// copy results
-				object_points.insert(object_points.end(), object_points_grid.begin(), object_points_grid.end());
-				for (size_t i = 0; i < image_points_grid.size(); i++) {
-					for (size_t j = 0; j < 2; j++) { image_points[j].push_back(image_points_grid[i][j]); }
-				}
-				iter--;
-			}
-		}
-		else {
-			object_points.push_back(points_ref);
-			for (size_t i = 0; i < 2; i++) { image_points[i].push_back(new_points[i]); }
-			iter--;
-		}
-	}
-
-	// calculate stereoCalibration using all input images (which have low enough
-	// RMS error in previous step)
-
-	LOG(INFO) << "Calculating extrinsic stereo parameters using " << object_points.size() << " samples.";
-
-	CHECK(object_points.size() == image_points[0].size());
-	CHECK(object_points.size() == image_points[1].size());
-
-	double rms = stereoCalibrate(object_points,
-		image_points[0], image_points[1],
-		camera_matrices[0], dist_coeffs[0],
-		camera_matrices[1], dist_coeffs[1],
-		image_size, R, T, E, F, per_view_errors,
-		stereocalibrate_flags,
-		cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 120, 1e-6)
-	);
-
-	LOG(INFO) << "Final extrinsic calibration RMS (reprojection error): " << rms;
-	for (int i = 0; i < per_view_errors.rows * per_view_errors.cols; i++) {
-		LOG(4) << "error for sample " << i << ": " << ((double*) per_view_errors.data)[i];
-	}
-
-	Mat R1, R2, P1, P2, Q;
-	cv::Rect validRoi[2];
-
-	stereoRectify(
-		camera_matrices[0], dist_coeffs[0],
-		camera_matrices[1], dist_coeffs[1],
-		image_size, R, T, R1, R2, P1, P2, Q,
-		0, alpha, image_size,
-		&validRoi[0], &validRoi[1]
-	);
-
-	saveExtrinsics(FTL_LOCAL_CONFIG_ROOT "/extrinsics.yml", R, T, R1, R2, P1, P2, Q);
-	LOG(INFO) << "Stereo camera extrinsics saved to: " << FTL_LOCAL_CONFIG_ROOT "/extrinsics.yml";
-
-	for (size_t i = 0; i < 2; i++) { cv::destroyWindow("Camera " + std::to_string(i)); }
-
-	// Visualize results
-	vector<Mat> map1(2), map2(2);
-	cv::initUndistortRectifyMap(camera_matrices[0], dist_coeffs[0], R1, P1, image_size, CV_16SC2, map1[0], map2[0]);
-	cv::initUndistortRectifyMap(camera_matrices[1], dist_coeffs[1], R2, P2, image_size, CV_16SC2, map1[1], map2[1]);
-
-	vector<Mat> in(2);
-	vector<Mat> out(2);
-	// vector<Mat> out_gray(2);
-	// Mat diff, diff_color;
-
-	while(cv::waitKey(25) == -1) {
-		for(size_t i = 0; i < 2; i++) {
-			auto &camera = cameras[i];
-			camera.grab();
-			camera.retrieve(in[i]);
-	
-			auto p = cv::Point2i(camera_matrices[i].at<double>(0, 2), camera_matrices[i].at<double>(1, 2));
-			cv::drawMarker(in[i], p, cv::Scalar(51, 204, 51), cv::MARKER_CROSS, 40, 1);
-			cv::drawMarker(in[i], p, cv::Scalar(51, 204, 51), cv::MARKER_SQUARE, 25);
-
-			cv::remap(in[i], out[i], map1[i], map2[i], cv::INTER_CUBIC);
-
-			// draw lines
-			for (int r = 50; r < image_size.height; r = r+50) {
-				cv::line(out[i], cv::Point(0, r), cv::Point(image_size.width-1, r), cv::Scalar(0,0,255), 1);
-			}
-
-			if (i == 0) { // left camera
-				auto p_r = cv::Point2i(-Q.at<double>(0, 3), -Q.at<double>(1, 3));
-				cv::drawMarker(out[i], p_r, cv::Scalar(0, 0, 204), cv::MARKER_CROSS, 30);
-				cv::drawMarker(out[i], p_r, cv::Scalar(0, 0, 204), cv::MARKER_SQUARE);
-			}
-			
-			cv::imshow("Camera " + std::to_string(i) + " (unrectified)", in[i]);
-			cv::imshow("Camera " + std::to_string(i) + " (rectified)", out[i]);
-		}
-		
-		/* not useful
-		cv::absdiff(out_gray[0], out_gray[1], diff);
-		cv::applyColorMap(diff, diff_color, cv::COLORMAP_JET);
-		cv::imshow("Difference", diff_color);
-		*/
-	}
-
-	cv::destroyAllWindows();
-}

applications/calibration/src/stereo.hpp

-#ifndef _FTL_CALIBRATION_STEREO_HPP_
-#define _FTL_CALIBRATION_STEREO_HPP_
-
-#include <map>
-#include <string>
-
-#include <opencv2/core.hpp>
-#include <opencv2/calib3d.hpp>
-
-namespace ftl {
-namespace calibration {
-
-void stereo(std::map<std::string, std::string> &opt);
-
-}
-}
-
-#endif  // _FTL_CALIBRATION_STEREO_HPP_

applications/ftl2mkv/src/main.cpp

@@ -5,6 +5,7 @@
 #include <ftl/rgbd/camera.hpp>
 #include <ftl/codecs/hevc.hpp>
 #include <ftl/codecs/h264.hpp>
+#include <ftl/codecs/decoder.hpp>
 
 #include <fstream>
 
@@ -17,7 +18,7 @@ extern "C" {
 using ftl::codecs::codec_t;
 using ftl::codecs::Channel;
 
-static AVStream *add_video_stream(AVFormatContext *oc, const ftl::codecs::Packet &pkt)
+static AVStream *add_video_stream(AVFormatContext *oc, const ftl::codecs::Packet &pkt, const ftl::rgbd::Camera &cam)
 {
     //AVCodecContext *c;
     AVStream *st;
@@ -47,13 +48,13 @@ static AVStream *add_video_stream(AVFormatContext *oc, const ftl::codecs::Packet
 	//st->nb_frames = 0;
 	st->codecpar->codec_id = codec_id;
 	st->codecpar->codec_type = AVMEDIA_TYPE_VIDEO;
-	st->codecpar->width = ftl::codecs::getWidth(pkt.definition);
+	st->codecpar->width = cam.width; //ftl::codecs::getWidth(pkt.definition);
 	//if (pkt.flags & ftl::codecs::kFlagStereo) st->codecpar->width *= 2;
-	st->codecpar->height = ftl::codecs::getHeight(pkt.definition);
+	st->codecpar->height = cam.height; //ftl::codecs::getHeight(pkt.definition);
 	st->codecpar->format = AV_PIX_FMT_NV12;
 	st->codecpar->bit_rate = 4000000;
 
-	if (pkt.flags & ftl::codecs::kFlagStereo) av_dict_set(&st->metadata, "stereo_mode", "left_right", 0);
+	//if (pkt.flags & ftl::codecs::kFlagStereo) av_dict_set(&st->metadata, "stereo_mode", "left_right", 0);
 	//if (pkt.flags & ftl::codecs::kFlagStereo) av_dict_set(&oc->metadata, "stereo_mode", "1", 0);
 	//if (pkt.flags & ftl::codecs::kFlagStereo) av_dict_set_int(&st->metadata, "StereoMode", 1, 0);
 
@@ -78,6 +79,50 @@ static AVStream *add_video_stream(AVFormatContext *oc, const ftl::codecs::Packet
     return st;
 }
 
+static AVStream *add_audio_stream(AVFormatContext *oc, const ftl::codecs::Packet &pkt)
+{
+    //AVCodecContext *c;
+    AVStream *st;
+
+    st = avformat_new_stream(oc, 0);
+    if (!st) {
+        fprintf(stderr, "Could not alloc stream\n");
+        exit(1);
+    }
+
+	AVCodecID codec_id = AV_CODEC_ID_OPUS;
+	st->codecpar->codec_id = codec_id;
+	st->codecpar->codec_type = AVMEDIA_TYPE_AUDIO;
+	st->codecpar->channel_layout = 0;
+	st->codecpar->channels = 2;
+	st->codecpar->sample_rate = 48000;
+	st->codecpar->frame_size = 960;
+    return st;
+}
+
+static uint32_t make_id(const ftl::codecs::StreamPacket &spkt) {
+	return (((spkt.streamID << 8) + spkt.frame_number) << 8) + int(spkt.channel);
+}
+
+
+struct StreamState {
+	int64_t first_ts = 100000000000000000ll;
+	std::list<std::pair<ftl::codecs::StreamPacket, ftl::codecs::Packet>> packets;
+	bool seen_key = false;
+	AVStream *stream = nullptr;
+	int64_t last_ts = 0;
+
+	void insert(const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
+		for (auto i = packets.begin(); i != packets.end(); ++i) {
+			if (i->first.timestamp > spkt.timestamp) {
+				packets.insert(i, std::make_pair(spkt, pkt));
+				return;
+			}
+		}
+		packets.push_back(std::make_pair(spkt, pkt));
+	}
+};
+
 
 int main(int argc, char **argv) {
     std::string filename;
@@ -109,8 +154,12 @@ int main(int argc, char **argv) {
 
 	AVOutputFormat *fmt;
 	AVFormatContext *oc;
-	AVStream *video_st[10][2] = {nullptr};
+	StreamState video_st[10];
+
+	int stream_count = 0;
+	std::unordered_map<uint32_t, int> mapping;
 
+	// TODO: Remove in newer versions
 	av_register_all();
 
 	fmt = av_guess_format(NULL, outputfile.c_str(), NULL);
@@ -127,7 +176,11 @@ int main(int argc, char **argv) {
         return -1;
     }
     oc->oformat = fmt;
+
+	// TODO: Use URL in newer versions
 	snprintf(oc->filename, sizeof(oc->filename), "%s", outputfile.c_str());
+	//oc->url = (char*)av_malloc(outputfile.size()+1);
+	//snprintf(oc->url, outputfile.size()+1, "%s", outputfile.c_str());
 
 	/* open the output file, if needed */
     if (!(fmt->flags & AVFMT_NOFILE)) {
@@ -144,26 +197,63 @@ int main(int argc, char **argv) {
 
 	//bool stream_added[10] = {false};
 
-	int64_t first_ts = 10000000000000ll;
-
 	// TODO: In future, find a better way to discover number of streams...
 	// Read entire file to find all streams before reading again to write data
-	bool res = r.read(90000000000000, [&first_ts,&current_stream,&current_channel,&r,&video_st,oc](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
-        if (spkt.channel != static_cast<ftl::codecs::Channel>(current_channel) && current_channel != -1) return;
-        if (spkt.frame_number == current_stream || current_stream == 255) {
+	bool res = r.read(90000000000000, [&current_stream,&current_channel,&r,&video_st,oc,&mapping,&stream_count,root](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
+		if (spkt.channel != Channel::Audio && current_stream != 255 && spkt.streamID != current_stream) return;
+		if (spkt.channel != Channel::Colour && spkt.channel != Channel::Right && spkt.channel != Channel::Audio) return;
+
+        //if (spkt.channel != static_cast<ftl::codecs::Channel>(current_channel) && current_channel != -1) return;
+        //if (spkt.frame_number == current_stream || current_stream == 255) {
 
-            if (pkt.codec != codec_t::HEVC && pkt.codec != codec_t::H264) {
+            if (pkt.codec != codec_t::HEVC && pkt.codec != codec_t::H264 && pkt.codec != codec_t::OPUS) {
                 return;
             }
 
 			if (spkt.frame_number >= 10) return;  // TODO: Allow for more than 10
 
-			if (spkt.timestamp < first_ts) first_ts = spkt.timestamp;
+			//if (video_st[spkt.frame_number][(spkt.channel == Channel::Left) ? 0 : 1] == nullptr) {
+			if ((pkt.codec == codec_t::HEVC && ftl::codecs::hevc::isIFrame(pkt.data.data(), pkt.data.size())) ||
+					(pkt.codec == codec_t::H264 && ftl::codecs::h264::isIFrame(pkt.data.data(), pkt.data.size()))) {
+				if (mapping.count(make_id(spkt)) == 0) {
+					int id = stream_count++;
+
+					if (id >= 10) return;				
+					
+					auto *dec = ftl::codecs::allocateDecoder(pkt);
+					if (!dec) return;
+
+					if (spkt.timestamp < video_st[id].first_ts) video_st[id].first_ts = spkt.timestamp;
+
+					cv::cuda::GpuMat m;
+					dec->decode(pkt, m);
+
+					ftl::rgbd::Camera cam;
+					cam.width = m.cols;
+					cam.height = m.rows;
+					// Use decoder to get frame size...
+					video_st[id].stream = add_video_stream(oc, pkt, cam);
+
+					ftl::codecs::free(dec);
+
+					mapping[make_id(spkt)] = id;
+				}
+			} else if (pkt.codec == codec_t::OPUS) {
+				if (mapping.count(make_id(spkt)) == 0) {
+					int id = stream_count++;
+
+					if (id >= 10) return;				
+
+					if (spkt.timestamp < video_st[id].first_ts) video_st[id].first_ts = spkt.timestamp;
 
-			if (video_st[spkt.frame_number][(spkt.channel == Channel::Left) ? 0 : 1] == nullptr) {
-				video_st[spkt.frame_number][(spkt.channel == Channel::Left) ? 0 : 1] = add_video_stream(oc, pkt);
+					video_st[id].stream = add_audio_stream(oc, pkt);
+					video_st[id].seen_key = true;
+					video_st[id].last_ts = root->value("audio_delay", 1000);  // second delay?
+
+					mapping[make_id(spkt)] = id;
 				}
 			}
+		//}
 	});
 
 	r.end();
@@ -182,64 +272,175 @@ int main(int argc, char **argv) {
 		LOG(ERROR) << "Failed to write stream header";
 	}
 
-	bool seen_key[10] = {false};
-
-    res = r.read(90000000000000, [first_ts,&current_stream,&current_channel,&r,&video_st,oc,&seen_key](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
-        if (spkt.channel != static_cast<ftl::codecs::Channel>(current_channel) && current_channel != -1) return;
-        if (spkt.frame_number == current_stream || current_stream == 255) {
+    res = r.read(90000000000000, [&current_stream,&current_channel,&r,&video_st,oc,&mapping](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
+        //if (spkt.channel != static_cast<ftl::codecs::Channel>(current_channel) && current_channel != -1) return;
+        //if (spkt.frame_number == current_stream || current_stream == 255) {
 
-            if (pkt.codec != codec_t::HEVC && pkt.codec != codec_t::H264) {
+            if (pkt.codec != codec_t::HEVC && pkt.codec != codec_t::H264 && pkt.codec != codec_t::OPUS) {
                 return;
             }
 
             //LOG(INFO) << "Reading packet: (" << (int)spkt.streamID << "," << (int)spkt.channel << ") " << (int)pkt.codec << ", " << (int)pkt.definition;
 
-			if (spkt.frame_number >= 10) return;  // TODO: Allow for more than 10
+			auto i = mapping.find(make_id(spkt));
+			if (i == mapping.end()) return;
+			int id = i->second;
+
+			if (!video_st[id].stream) return;
 
 			bool keyframe = false;
 			if (pkt.codec == codec_t::HEVC) {
 				if (ftl::codecs::hevc::isIFrame(pkt.data.data(), pkt.data.size())) {
-					seen_key[spkt.frame_number] = true;
+					video_st[id].seen_key = true;
 					keyframe = true;
 				}
 			} else if (pkt.codec == codec_t::H264) {
 				if (ftl::codecs::h264::isIFrame(pkt.data.data(), pkt.data.size())) {
-					seen_key[spkt.frame_number] = true;
+					video_st[id].seen_key = true;
 					keyframe = true;
 				}
 			}
-			if (!seen_key[spkt.frame_number]) return;
+			if (!video_st[id].seen_key) return;
 
 			//if (spkt.timestamp > last_ts) framecount++;
 			//last_ts = spkt.timestamp;
 
+			video_st[id].insert(spkt, pkt);
+
+			if (video_st[id].packets.size() > 5) {
+				auto &spkt = video_st[id].packets.front().first;
+				auto &pkt = video_st[id].packets.front().second;
+
+				if (pkt.codec == codec_t::OPUS) {
+					// Must unpack the audio
+
+					const unsigned char *inptr = pkt.data.data();
+					int count = 0;
+					int frames = 0;
+
+					for (size_t i=0; i<pkt.data.size(); ) {
+						AVPacket avpkt;
+						av_init_packet(&avpkt);
+						avpkt.stream_index= video_st[id].stream->index;
+
+						const short *len = (const short*)inptr;
+						if (*len == 0) break;
+						if (frames == 10) break;
+
+						inptr += 2;
+						i += (*len)+2;
+
+						avpkt.pts = video_st[id].last_ts;
+						avpkt.dts = avpkt.pts;
+						avpkt.data= const_cast<uint8_t*>(inptr);
+						avpkt.size= *len;
+						avpkt.duration = 20;
+						video_st[id].last_ts += avpkt.duration;
+
+						/* write the compressed frame in the media file */
+						auto ret = av_write_frame(oc, &avpkt);
+						if (ret != 0) {
+							LOG(ERROR) << "Error writing audio frame: " << ret;
+						}
+
+						inptr += *len;
+						++frames;
+					}
+				} else {
 					AVPacket avpkt;
 					av_init_packet(&avpkt);
+					avpkt.stream_index= video_st[id].stream->index;
 					if (keyframe) avpkt.flags |= AV_PKT_FLAG_KEY;
-			//avpkt.pts = framecount*50; //spkt.timestamp - r.getStartTime();
-			avpkt.pts = spkt.timestamp - first_ts;
+					avpkt.pts = spkt.timestamp - video_st[id].first_ts;
 					avpkt.dts = avpkt.pts;
-			avpkt.stream_index= video_st[spkt.frame_number][(spkt.channel == Channel::Left) ? 0 : 1]->index;
 					avpkt.data= const_cast<uint8_t*>(pkt.data.data());
 					avpkt.size= pkt.data.size();
-			avpkt.duration = 1;
+					avpkt.duration = 0;
+
+					/* write the compressed frame in the media file */
+					auto ret = av_write_frame(oc, &avpkt);
+					if (ret != 0) {
+						LOG(ERROR) << "Error writing video frame: " << ret;
+					}
+				}
 
 				//LOG(INFO) << "write frame: " << avpkt.pts << "," << avpkt.stream_index << "," << avpkt.size;
 
+				
+
+				video_st[id].packets.pop_front();
+			}
+        //}
+    });
+
+	for (int i=0; i<10; ++i) {
+		while (video_st[i].packets.size() > 0) {
+			int id = i;
+			auto &spkt = video_st[i].packets.front().first;
+			auto &pkt = video_st[i].packets.front().second;
+
+			if (pkt.codec == codec_t::OPUS) {
+				// Must unpack the audio
+
+				const unsigned char *inptr = pkt.data.data();
+				int count = 0;
+				int frames = 0;
+
+				for (size_t i=0; i<pkt.data.size(); ) {
+					AVPacket avpkt;
+					av_init_packet(&avpkt);
+					avpkt.stream_index= video_st[id].stream->index;
+
+					const short *len = (const short*)inptr;
+					if (*len == 0) break;
+					if (frames == 10) break;
+
+					inptr += 2;
+					i += (*len)+2;
+
+					avpkt.pts = video_st[id].last_ts;
+					avpkt.dts = avpkt.pts;
+					avpkt.data= const_cast<uint8_t*>(inptr);
+					avpkt.size= *len;
+					avpkt.duration = 20;
+					video_st[id].last_ts += avpkt.duration;
+
 					/* write the compressed frame in the media file */
 					auto ret = av_write_frame(oc, &avpkt);
 					if (ret != 0) {
-				LOG(ERROR) << "Error writing frame: " << ret;
+						LOG(ERROR) << "Error writing audio frame: " << ret;
+					}
+
+					inptr += *len;
+					++frames;
+				}
+			} else {
+				AVPacket avpkt;
+				av_init_packet(&avpkt);
+				avpkt.stream_index= video_st[id].stream->index;
+				//if (keyframe) avpkt.flags |= AV_PKT_FLAG_KEY;
+				avpkt.pts = spkt.timestamp - video_st[id].first_ts;
+				avpkt.dts = avpkt.pts;
+				avpkt.data= const_cast<uint8_t*>(pkt.data.data());
+				avpkt.size= pkt.data.size();
+				avpkt.duration = 0;
+
+				/* write the compressed frame in the media file */
+				auto ret = av_write_frame(oc, &avpkt);
+				if (ret != 0) {
+					LOG(ERROR) << "Error writing video frame: " << ret;
+				}
+			}
+
+			video_st[i].packets.pop_front();	
 		}
 	}
-    });
 
 	av_write_trailer(oc);
 	//avcodec_close(video_st->codec);
 
 	for (int i=0; i<10; ++i) {
-		if (video_st[i][0]) av_free(video_st[i][0]);
-		if (video_st[i][1]) av_free(video_st[i][1]);
+		if (video_st[i].stream) av_free(video_st[i].stream);
 	}
 
 	if (!(fmt->flags & AVFMT_NOFILE)) {

applications/gui/src/camera.cpp

-#include "camera.hpp"
-#include "pose_window.hpp"
-#include "screen.hpp"
-#include <nanogui/glutil.h>
-
-#include <ftl/profiler.hpp>
-
-#include <opencv2/imgproc.hpp>
-#include <opencv2/imgcodecs.hpp>
-#include <opencv2/cudaarithm.hpp>
-
-#include <ftl/operators/antialiasing.hpp>
-#include <ftl/cuda/normals.hpp>
-#include <ftl/render/colouriser.hpp>
-#include <ftl/cuda/transform.hpp>
-#include <ftl/operators/gt_analysis.hpp>
-#include <ftl/operators/poser.hpp>
-#include <ftl/cuda/colour_cuda.hpp>
-#include <ftl/streams/parsers.hpp>
-
-#include <ftl/render/overlay.hpp>
-#include "statsimage.hpp"
-
-#define LOGURU_REPLACE_GLOG 1
-#include <loguru.hpp>
-
-#include <fstream>
-
-#ifdef HAVE_OPENVR
-#include "vr.hpp"
-#endif
-
-using ftl::rgbd::isValidDepth;
-using ftl::gui::GLTexture;
-using ftl::gui::PoseWindow;
-using ftl::codecs::Channel;
-using ftl::codecs::Channels;
-using cv::cuda::GpuMat;
-
-
-static int vcamcount = 0;
-
-static Eigen::Affine3d create_rotation_matrix(float ax, float ay, float az) {
-	Eigen::Affine3d rx =
-		Eigen::Affine3d(Eigen::AngleAxisd(ax, Eigen::Vector3d(1, 0, 0)));
-	Eigen::Affine3d ry =
-		Eigen::Affine3d(Eigen::AngleAxisd(ay, Eigen::Vector3d(0, 1, 0)));
-	Eigen::Affine3d rz =
-		Eigen::Affine3d(Eigen::AngleAxisd(az, Eigen::Vector3d(0, 0, 1)));
-	return rz * rx * ry;
-}
-
-ftl::gui::Camera::Camera(ftl::gui::Screen *screen, int fsmask, int fid, ftl::codecs::Channel c)
-		: screen_(screen), fsmask_(fsmask), fid_(fid), texture1_(GLTexture::Type::BGRA), texture2_(GLTexture::Type::BGRA), depth1_(GLTexture::Type::Float), channel_(c),channels_(0u) {
-
-	eye_ = Eigen::Vector3d::Zero();
-	neye_ = Eigen::Vector4d::Zero();
-	rotmat_.setIdentity();
-
-	//up_ = Eigen::Vector3f(0,1.0f,0);
-	lerpSpeed_ = 0.999f;
-	sdepth_ = false;
-	ftime_ = (float)glfwGetTime();
-	pause_ = false;
-
-#ifdef HAVE_OPENVR
-	vr_mode_ = false;
-#endif
-
-	//channel_ = Channel::Left;
-
-	channels_ += c;
-	//channels_ += Channel::Depth;
-	width_ = 0;
-	height_ = 0;
-
-	// Create pose window...
-	//posewin_ = new PoseWindow(screen, src_->getURI());
-	//posewin_->setTheme(screen->windowtheme);
-	//posewin_->setVisible(false);
-	posewin_ = nullptr;
-	renderer_ = nullptr;
-	renderer2_ = nullptr;
-	post_pipe_ = nullptr;
-	record_stream_ = nullptr;
-	transform_ix_ = -1;
-	stereo_ = false;
-	rx_ = 0;
-	ry_ = 0;
-	framesets_ = nullptr;
-
-	colouriser_ = ftl::create<ftl::render::Colouriser>(screen->root(), "colouriser");
-	overlayer_ = ftl::create<ftl::overlay::Overlay>(screen->root(), "overlay");
-
-	// Is virtual camera?
-	if (fid == 255) {
-		renderer_ = ftl::create<ftl::render::CUDARender>(screen_->root(), std::string("vcam")+std::to_string(vcamcount++));
-		// Allow mask to be changed
-		fsmask_ = renderer_->value("fsmask", fsmask_);
-		renderer_->on("fsmask", [this](const ftl::config::Event &e) {
-			fsmask_ = renderer_->value("fsmask", fsmask_);
-		});
-
-		// Allow Pose origin to be changed
-		pose_source_ = renderer_->value("pose_source", pose_source_);
-		renderer_->on("pose_source", [this](const ftl::config::Event &e) {
-			pose_source_ = renderer_->value("pose_source", pose_source_);
-		});
-
-		intrinsics_ = ftl::create<ftl::Configurable>(renderer_, "intrinsics");
-
-		state_.getLeft() = ftl::rgbd::Camera::from(intrinsics_);
-		state_.getRight() = state_.getLeft();
-
-		intrinsics_->on("width", [this](const ftl::config::Event &e) {
-			state_.getLeft() = ftl::rgbd::Camera::from(intrinsics_);
-			state_.getRight() = state_.getLeft();
-		});
-
-		intrinsics_->on("focal", [this](const ftl::config::Event &e) {
-			state_.getLeft() = ftl::rgbd::Camera::from(intrinsics_);
-			state_.getRight() = state_.getLeft();
-		});
-
-		{
-			Eigen::Matrix4d pose;
-			pose.setIdentity();
-			state_.setPose(pose);
-
-			for (auto &t : transforms_) {
-				t.setIdentity();
-			}
-		}
-		{
-			double camera_initial_x = intrinsics_->value("camera_x", 0.0);
-			double camera_initial_y = intrinsics_->value("camera_y", -1.75);
-			double camera_initial_z = intrinsics_->value("camera_z", 0.0);
-
-			double lookat_initial_x = intrinsics_->value("lookat_x", 1.0);
-			double lookat_initial_y = intrinsics_->value("lookat_y", 0.0);
-			double lookat_initial_z = intrinsics_->value("lookat_z", 0.0);
-
-			Eigen::Vector3f head(camera_initial_x, camera_initial_y, camera_initial_z);
-			Eigen::Vector3f lookat(lookat_initial_x, lookat_initial_y, lookat_initial_z);
-			// TODO up vector
-			Eigen::Matrix4f pose = nanogui::lookAt(head, head+lookat, Eigen::Vector3f(0.0f, 1.0f, 0.0f));
-
-			eye_ = Eigen::Vector3d(camera_initial_x, camera_initial_y, camera_initial_z);
-			neye_ = Eigen::Vector4d(eye_(0), eye_(1), eye_(2), 0.0);
-			rotmat_ = pose.cast<double>();
-			rotmat_.block(0, 3, 3, 1).setZero();
-		}
-	}
-}
-
-ftl::gui::Camera::~Camera() {
-	//delete writer_;
-	//delete fileout_;
-}
-
-void ftl::gui::Camera::drawUpdated(std::vector<ftl::rgbd::FrameSet*> &fss) {
-	// Only draw if frameset updated.
-	if (!stale_frame_.test_and_set()) {
-		draw(fss);
-	}
-}
-
-void ftl::gui::Camera::draw(std::vector<ftl::rgbd::FrameSet*> &fss) {
-	if (fid_ != 255) {
-		for (auto *fs : fss) {
-			if (!usesFrameset(fs->id)) continue;
-			UNIQUE_LOCK(fs->mtx, lk);
-
-			ftl::rgbd::Frame *frame = nullptr;
-
-			if ((size_t)fid_ >= fs->frames.size()) return;
-			if (!fs->hasFrame(fid_)) return;
-
-			frame = &fs->frames[fid_];
-			if (!frame->hasChannel(channel_)) return;
-
-			auto &buf = colouriser_->colourise(*frame, channel_, 0);
-			auto &buf2 = frame->getTexture<uchar4>(Channel::Colour);
-			ftl::cuda::compositeInverse(buf2, buf, 0);
-
-			// For non-virtual cameras, copy the CUDA texture into the opengl
-			// texture device-to-device.
-			texture1_.make(buf.width(), buf.height());
-			auto dst1 = texture1_.map(0);
-			cudaMemcpy2D(dst1.data, dst1.step1(), buf.devicePtr(), buf.pitch(), buf.width()*4, buf.height(), cudaMemcpyDeviceToDevice);
-			ftl::cuda::flip<uchar4>(dst1, 0);
-			texture1_.unmap(0);
-
-			depth1_.make(buf.width(), buf.height());
-			dst1 = depth1_.map(0);
-			dst1.setTo(cv::Scalar(0.5f));
-			depth1_.unmap(0);
-
-			width_ = texture1_.width();
-			height_ = texture1_.height();
-			return;
-		}
-	}
-	//if (fsid_ >= fss.size()) return;
-
-	//auto &fs = *fss[fsid_];
-
-	_applyPoseEffects(fss);
-
-	UNIQUE_LOCK(mutex_, lk2);
-	//state_.getLeft().fx = intrinsics_->value("focal", 700.0f);
-	//state_.getLeft().fy = state_.getLeft().fx;
-	_draw(fss);
-}
-
-std::pair<const ftl::rgbd::Frame *, const ftl::codecs::Face *> ftl::gui::Camera::_selectFace(std::vector<ftl::rgbd::FrameSet*> &fss) {
-	for (auto *fset : fss) {
-		for (const auto &f : fset->frames) {
-			if (f.hasChannel(Channel::Faces)) {
-				std::vector<ftl::codecs::Face> data;
-				f.get(Channel::Faces, data);
-
-				if (data.size() > 0) {
-					return {&f,&(*data.rbegin())};
-				}
-			}
-		}
-	}
-	return {nullptr, nullptr};
-}
-
-void ftl::gui::Camera::_generateWindow(const ftl::rgbd::Frame &f, const ftl::codecs::Face &face, Eigen::Matrix4d &pose_adjust, ftl::render::ViewPort &vp) {
-	auto cam = ftl::rgbd::Camera::from(intrinsics_);
-	auto d = face;
-
-	float screenWidth = intrinsics_->value("screen_size", 0.6f);  // In meters
-	float screenHeight = (9.0f/16.0f) * screenWidth;
-
-	float screenDistance = (d.depth > cam.minDepth && d.depth < cam.maxDepth) ? d.depth : intrinsics_->value("screen_dist_default", 0.5f);  // Face distance from screen in meters
-
-	auto pos = f.getLeft().screenToCam(float(d.box.x+(d.box.width/2)), float(d.box.y+(d.box.height/2)), screenDistance);
-	Eigen::Vector3f eye;
-	eye[0] = -pos.x;
-	eye[1] = pos.y;
-	eye[2] = -pos.z;
-	//eye[3] = 0;
-
-	Eigen::Translation3f trans(eye);
-	Eigen::Affine3f t(trans);
-	Eigen::Matrix4f viewPose = t.matrix();
-
-	// Calculate where the screen is within current camera space
-	Eigen::Vector4f p1 = viewPose.cast<float>() * (Eigen::Vector4f(screenWidth/2.0, screenHeight/2.0, 0, 1));
-	Eigen::Vector4f p2 = viewPose.cast<float>() * (Eigen::Vector4f(screenWidth/2.0, -screenHeight/2.0, 0, 1));
-	Eigen::Vector4f p3 = viewPose.cast<float>() * (Eigen::Vector4f(-screenWidth/2.0, screenHeight/2.0, 0, 1));
-	Eigen::Vector4f p4 = viewPose.cast<float>() * (Eigen::Vector4f(-screenWidth/2.0, -screenHeight/2.0, 0, 1));
-	p1 = p1 / p1[3];
-	p2 = p2 / p2[3];
-	p3 = p3 / p3[3];
-	p4 = p4 / p4[3];
-	float2 p1screen = cam.camToScreen<float2>(make_float3(p1[0],p1[1],p1[2]));
-	float2 p2screen = cam.camToScreen<float2>(make_float3(p2[0],p2[1],p2[2]));
-	float2 p3screen = cam.camToScreen<float2>(make_float3(p3[0],p3[1],p3[2]));
-	float2 p4screen = cam.camToScreen<float2>(make_float3(p4[0],p4[1],p4[2]));
-
-	std::vector<cv::Point2f> quad_pts;
-	std::vector<cv::Point2f> squre_pts;
-	quad_pts.push_back(cv::Point2f(p1screen.x,p1screen.y));
-	quad_pts.push_back(cv::Point2f(p2screen.x,p2screen.y));
-	quad_pts.push_back(cv::Point2f(p3screen.x,p3screen.y));
-	quad_pts.push_back(cv::Point2f(p4screen.x,p4screen.y));
-	squre_pts.push_back(cv::Point2f(0,0));
-	squre_pts.push_back(cv::Point2f(0,cam.height));
-	squre_pts.push_back(cv::Point2f(cam.width,0));
-	squre_pts.push_back(cv::Point2f(cam.width,cam.height));
-
-	cv::Mat transmtx = cv::getPerspectiveTransform(quad_pts,squre_pts);
-	//cv::Mat transformed = cv::Mat::zeros(overlay_.rows, overlay_.cols, CV_8UC4);
-	//cv::warpPerspective(im1_, im1_, transmtx, im1_.size());
-
-	// TODO: Use the transmtx above for perspective distortion..
-
-	//ftl::render::ViewPort vp;
-	vp.x = std::min(p4screen.x, std::min(p3screen.x, std::min(p1screen.x,p2screen.x)));
-	vp.y = std::min(p4screen.y, std::min(p3screen.y, std::min(p1screen.y,p2screen.y)));
-	vp.width = std::max(p4screen.x, std::max(p3screen.x, std::max(p1screen.x,p2screen.x))) - vp.x;
-	vp.height = std::max(p4screen.y, std::max(p3screen.y, std::max(p1screen.y,p2screen.y))) - vp.y;
-	/*vp.warpMatrix.entries[0] = transmtx.at<float>(0,0);
-	vp.warpMatrix.entries[1] = transmtx.at<float>(1,0);
-	vp.warpMatrix.entries[2] = transmtx.at<float>(2,0);
-	vp.warpMatrix.entries[3] = transmtx.at<float>(0,1);
-	vp.warpMatrix.entries[4] = transmtx.at<float>(1,1);
-	vp.warpMatrix.entries[5] = transmtx.at<float>(2,1);
-	vp.warpMatrix.entries[6] = transmtx.at<float>(0,2);
-	vp.warpMatrix.entries[7] = transmtx.at<float>(1,2);
-	vp.warpMatrix.entries[8] = transmtx.at<float>(2,2);
-	vp.warpMatrix = vp.warpMatrix.getInverse(); //.getInverse();*/
-	//renderer_->setViewPort(ftl::render::ViewPortMode::Warping, vp);
-
-	pose_adjust = viewPose.cast<double>();
-}
-
-void ftl::gui::Camera::_applyPoseEffects(std::vector<ftl::rgbd::FrameSet*> &fss) {
-	if (renderer_->value("window_effect", false)) {
-		auto [frame,face] = _selectFace(fss);
-		if (face) {
-			Eigen::Matrix4d windowPose;
-			ftl::render::ViewPort windowViewPort;
-			_generateWindow(*frame, *face, windowPose, windowViewPort);
-
-			// Apply the window effect
-			renderer_->setViewPort(ftl::render::ViewPortMode::Stretch, windowViewPort);
-			state_.getPose() = windowPose * state_.getPose();
-		}
-	}
-}
-
-void ftl::gui::Camera::setStereo(bool v) {
-	UNIQUE_LOCK(mutex_, lk);
-
-	if (isVirtual()) {
-		stereo_ = v;
-	} else if (v && availableChannels().has(Channel::Right)) {
-		stereo_ = true;
-	} else {
-		stereo_ = false;
-	}
-}
-
-static ftl::codecs::Channel mapToSecondChannel(ftl::codecs::Channel c) {
-	switch (c) {
-		case Channel::Depth		: return Channel::Depth2;
-		case Channel::Normals	: return Channel::Normals2;
-		default: return c;
-	}
-}
-
-void ftl::gui::Camera::_draw(std::vector<ftl::rgbd::FrameSet*> &fss) {
-	frame_.reset();
-	frame_.setOrigin(&state_);
-
-	// Make sure an OpenGL pixel buffer exists
-	texture1_.make(state_.getLeft().width, state_.getLeft().height);
-	depth1_.make(state_.getLeft().width, state_.getLeft().height);
-	if (isStereo()) texture2_.make(state_.getRight().width, state_.getRight().height);
-
-	// Map the GL pixel buffer to a GpuMat
-	frame_.create<cv::cuda::GpuMat>(Channel::Colour) = texture1_.map(renderer_->getCUDAStream());
-	frame_.create<cv::cuda::GpuMat>(Channel::Depth) = depth1_.map(renderer_->getCUDAStream());
-	frame_.createTexture<float>(Channel::Depth);
-	if (isStereo()) frame_.create<cv::cuda::GpuMat>(Channel::Colour2) = texture2_.map((renderer2_) ? renderer2_->getCUDAStream() : 0);
-
-	// TODO: Remove;
-	overlay_.create(state_.getLeft().height, state_.getLeft().width, CV_8UC4);
-	//frame_.create<cv::Mat>(Channel::Overlay) = overlay_;
-
-	//overlay_.setTo(cv::Scalar(0,0,0,0));
-	//bool enable_overlay = overlayer_->value("enabled", false);
-
-	{
-		FTL_Profile("Render",0.034);
-		renderer_->begin(frame_, Channel::Colour);
-		if (isStereo()) {
-			if (!renderer2_) {
-				renderer2_ = ftl::create<ftl::render::CUDARender>(screen_->root(), std::string("vcam")+std::to_string(vcamcount++));
-			}
-			renderer2_->begin(frame_, Channel::Colour2);
-		}
-
-		try {
-			for (auto *fs : fss) {
-				if (!usesFrameset(fs->id)) continue;
-
-				fs->mtx.lock();
-				renderer_->submit(fs, ftl::codecs::Channels<0>(Channel::Colour), transforms_[fs->id]);
-				if (isStereo()) renderer2_->submit(fs, ftl::codecs::Channels<0>(Channel::Colour), transforms_[fs->id]);
-
-				//if (enable_overlay) {
-					// Generate and upload an overlay image.
-				//	overlayer_->apply(*fs, overlay_, state_);
-				//	frame_.upload(Channel::Overlay, renderer_->getCUDAStream());
-				//}
-			}
-
-			renderer_->render();
-			if (isStereo()) renderer2_->render();
-
-			if (channel_ != Channel::Left && channel_ != Channel::Right && channel_ != Channel::None) {
-				renderer_->blend(channel_);
-				if (isStereo()) {
-					renderer2_->blend(mapToSecondChannel(channel_));
-				}
-			}
-
-			//if (enable_overlay) {
-			//	renderer_->blend(Channel::Overlay);
-			//}
-
-			renderer_->end();
-			if (isStereo()) renderer2_->end();
-		} catch(std::exception &e) {
-			LOG(ERROR) << "Exception in render: " << e.what();
-		}
-
-		for (auto *fs : fss) {
-			if (!usesFrameset(fs->id)) continue;
-			fs->mtx.unlock();
-		}
-	}
-
-	if (!post_pipe_) {
-		post_pipe_ = ftl::config::create<ftl::operators::Graph>(screen_->root(), "post_filters");
-		post_pipe_->append<ftl::operators::FXAA>("fxaa");
-		post_pipe_->append<ftl::operators::GTAnalysis>("gtanalyse");
-	}
-
-	post_pipe_->apply(frame_, frame_, 0);
-
-	channels_ = frame_.getChannels();
-
-	frame_.get<cv::cuda::GpuMat>(Channel::Depth).download(im_depth_);
-	cv::flip(im_depth_, im_depth_, 0);
-
-	//frame_.get<cv::cuda::GpuMat>(Channel::Normals).download(im_normals_);
-	//im_normals_.createMatHeader().convertTo(im_normals_f_, CV_32FC4);
-	//cv::flip(im_normals_f_, im_normals_f_, 0);
-
-	// Normalize depth map
-	frame_.get<cv::cuda::GpuMat>(Channel::Depth).convertTo(frame_.get<cv::cuda::GpuMat>(Channel::Depth), CV_32F, 1.0/8.0);
-
-	width_ = texture1_.width();
-	height_ = texture1_.height();
-
-	if (record_stream_ && record_stream_->active()) {
-		// TODO: Allow custom channel selection
-		ftl::rgbd::FrameSet fs2;
-		auto &f = fs2.frames.emplace_back();
-		fs2.count = 1;
-		fs2.mask = 1;
-		//fs2.stale = false;
-		fs2.set(ftl::data::FSFlag::STALE);
-		if (frame_.hasChannel(Channel::Colour2)) {
-			frame_.swapTo(Channels<0>(Channel::Colour) | Channel::Colour2, f);
-			ftl::cuda::flip(f.getTexture<uchar4>(Channel::Colour), 0);
-			ftl::cuda::flip(f.getTexture<uchar4>(Channel::Colour2), 0);
-		} else {
-			frame_.swapTo(Channels<0>(Channel::Colour), f);  // Channel::Colour + Channel::Depth
-			ftl::cuda::flip(f.getTexture<uchar4>(Channel::Colour), 0);
-		}
-
-		fs2.timestamp = ftl::timer::get_time();
-		fs2.id = 0;
-		record_sender_->post(fs2);
-		record_stream_->select(0, Channels<0>(Channel::Colour));
-		// Reverse the flip
-		if (f.hasChannel(Channel::Colour2)) {
-			ftl::cuda::flip(f.getTexture<uchar4>(Channel::Colour), 0);
-			ftl::cuda::flip(f.getTexture<uchar4>(Channel::Colour2), 0);
-		} else {
-			ftl::cuda::flip(f.getTexture<uchar4>(Channel::Colour), 0);
-		}
-		f.swapTo(Channels<0>(Channel::Colour), frame_);
-	} else if (do_snapshot_) {
-		do_snapshot_ = false;
-		cv::Mat flipped;
-		cv::Mat im1;
-
-		frame_.get<cv::cuda::GpuMat>(Channel::Colour).download(im1);
-
-		{
-			//UNIQUE_LOCK(mutex_, lk);
-			cv::flip(im1, flipped, 0);
-		}
-		cv::cvtColor(flipped, flipped, cv::COLOR_BGRA2BGR);
-		cv::imwrite(snapshot_filename_, flipped);
-	}
-
-	// Unmap GL buffer from CUDA and finish updating GL texture
-	texture1_.unmap(renderer_->getCUDAStream());
-	depth1_.unmap(renderer_->getCUDAStream());
-	if (isStereo()) texture2_.unmap(renderer2_->getCUDAStream());
-}
-
-void ftl::gui::Camera::update(int fsid, const ftl::codecs::Channels<0> &c) {
-	if (!isVirtual() && ((1 << fsid) & fsmask_)) {
-		channels_ += c;
-		//if (c.has(Channel::Depth)) {
-			//channels_ += Channel::ColourNormals;
-		//}
-	}
-}
-
-void ftl::gui::Camera::update(std::vector<ftl::rgbd::FrameSet*> &fss) {
-	UNIQUE_LOCK(mutex_, lk);
-
-	framesets_ = &fss;
-	stale_frame_.clear();
-
-	if (screen_->activeCamera() == this) {
-		for (auto *fs : fss) {
-			if (!usesFrameset(fs->id)) continue;
-
-			for (auto &f : fs->frames) {
-				//if (f.hasChanged(Channel::Pose)) {
-					f.patchPose(T_);
-				//}
-			}
-		}
-	}
-
-	//if (fss.size() <= fsid_) return;
-	if (fid_ == 255) {
-		name_ = "Virtual Camera";
-	} else {
-		for (auto *fs : fss) {
-			if (!usesFrameset(fs->id)) continue;
-
-			ftl::rgbd::Frame *frame = nullptr;
-
-			if ((size_t)fid_ >= fs->frames.size()) return;
-			frame = &fs->frames[fid_];
-			channels_ = frame->getChannels();
-
-			if (frame->hasChannel(Channel::Messages)) {
-				msgs_.clear();
-				frame->get(Channel::Messages, msgs_);
-			}
-
-			auto n = frame->get<std::string>("name");
-			if (n) {
-				name_ = *n;
-			} else {
-				name_ = "No name";
-			}
-			state_.getLeft() = frame->getLeftCamera();
-			return;
-		}
-	}
-}
-
-void ftl::gui::Camera::setPose(const Eigen::Matrix4d &p) {
-	eye_[0] = p(0,3);
-	eye_[1] = p(1,3);
-	eye_[2] = p(2,3);
-
-	double sx = Eigen::Vector3d(p(0,0), p(1,0), p(2,0)).norm();
-	double sy = Eigen::Vector3d(p(0,1), p(1,1), p(2,1)).norm();
-	double sz = Eigen::Vector3d(p(0,2), p(1,2), p(2,2)).norm();
-
-	Eigen::Matrix4d rot = p;
-	rot(0,3) = 0.0;
-	rot(1,3) = 0.0;
-	rot(2,3) = 0.0;
-	rot(0,0) = rot(0,0) / sx;
-	rot(1,0) = rot(1,0) / sx;
-	rot(2,0) = rot(2,0) / sx;
-	rot(0,1) = rot(0,1) / sy;
-	rot(1,1) = rot(1,1) / sy;
-	rot(2,1) = rot(2,1) / sy;
-	rot(0,2) = rot(0,2) / sz;
-	rot(1,2) = rot(1,2) / sz;
-	rot(2,2) = rot(2,2) / sz;
-	rotmat_ = rot;
-}
-
-void ftl::gui::Camera::mouseMovement(int rx, int ry, int button) {
-	//if (!src_->hasCapabilities(ftl::rgbd::kCapMovable)) return;
-	if (fid_ < 255) return;
-	if (button == 1) {
-		rx_ += rx;
-		ry_ += ry;
-
-		/*float rrx = ((float)ry * 0.2f * delta_);
-		//orientation_[2] += std::cos(orientation_[1])*((float)rel[1] * 0.2f * delta_);
-		float rry = (float)rx * 0.2f * delta_;
-		float rrz = 0.0;
-
-
-		Eigen::Affine3d r = create_rotation_matrix(rrx, -rry, rrz);
-		rotmat_ = rotmat_ * r.matrix();*/
-	}
-}
-
-void ftl::gui::Camera::keyMovement(int key, int modifiers) {
-	//if (!src_->hasCapabilities(ftl::rgbd::kCapMovable)) return;
-	if (fid_ < 255) return;
-	if (key == 263 || key == 262) {
-		float mag = (modifiers & 0x1) ? 0.01f : 0.1f;
-		float scalar = (key == 263) ? -mag : mag;
-		neye_ += rotmat_*Eigen::Vector4d(scalar,0.0,0.0,1.0);
-		return;
-	} else if (key == 264 || key == 265) {
-		float mag = (modifiers & 0x1) ? 0.01f : 0.1f;
-		float scalar = (key == 264) ? -mag : mag;
-		neye_ += rotmat_*Eigen::Vector4d(0.0,0.0,scalar,1.0);
-		return;
-	} else if (key == 266 || key == 267) {
-		float mag = (modifiers & 0x1) ? 0.01f : 0.1f;
-		float scalar = (key == 266) ? -mag : mag;
-		neye_ += rotmat_*Eigen::Vector4d(0.0,scalar,0.0,1.0);
-		return;
-	} else if (key >= '0' && key <= '5' && modifiers == 2) {  // Ctrl+NUMBER
-		int ix = key - (int)('0');
-		transform_ix_ = ix-1;
-		return;
-	}
-}
-
-void ftl::gui::Camera::showPoseWindow() {
-	posewin_->setVisible(true);
-}
-
-void ftl::gui::Camera::showSettings() {
-
-}
-
-#ifdef HAVE_OPENVR
-bool ftl::gui::Camera::setVR(bool on) {
-	if (on == vr_mode_) {
-		LOG(WARNING) << "VR mode already enabled";
-		return on;
-	}
-	vr_mode_ = on;
-
-	if (on) {
-		setStereo(true);
-
-		UNIQUE_LOCK(mutex_, lk);
-		//src_->set("baseline", baseline_);
-		state_.getLeft().baseline = baseline_;
-
-		Eigen::Matrix3d intrinsic;
-
-		unsigned int size_x, size_y;
-		screen_->getVR()->GetRecommendedRenderTargetSize(&size_x, &size_y);
-		state_.getLeft().width = size_x;
-		state_.getLeft().height = size_y;
-		state_.getRight().width = size_x;
-		state_.getRight().height = size_y;
-
-		intrinsic = getCameraMatrix(screen_->getVR(), vr::Eye_Left);
-		CHECK(intrinsic(0, 2) < 0 && intrinsic(1, 2) < 0);
-		state_.getLeft().fx = intrinsic(0,0);
-		state_.getLeft().fy = intrinsic(0,0);
-		state_.getLeft().cx = intrinsic(0,2);
-		state_.getLeft().cy = intrinsic(1,2);
-
-		intrinsic = getCameraMatrix(screen_->getVR(), vr::Eye_Right);
-		CHECK(intrinsic(0, 2) < 0 && intrinsic(1, 2) < 0);
-		state_.getRight().fx = intrinsic(0,0);
-		state_.getRight().fy = intrinsic(0,0);
-		state_.getRight().cx = intrinsic(0,2);
-		state_.getRight().cy = intrinsic(1,2);
-
-		vr_mode_ = true;
-	}
-	else {
-		vr_mode_ = false;
-		setStereo(false);
-
-		UNIQUE_LOCK(mutex_, lk);
-		state_.getLeft() = ftl::rgbd::Camera::from(intrinsics_);
-		state_.getRight() = state_.getLeft();
-	}
-
-	return vr_mode_;
-}
-#endif
-
-void ftl::gui::Camera::setChannel(Channel c) {
-	UNIQUE_LOCK(mutex_, lk);
-	channel_ = c;
-}
-
-/*static void drawEdges(	const cv::Mat &in, cv::Mat &out,
-						const int ksize = 3, double weight = -1.0, const int threshold = 32,
-						const int threshold_type = cv::THRESH_TOZERO)
-{
-	cv::Mat edges;
-	cv::Laplacian(in, edges, 8, ksize);
-	cv::threshold(edges, edges, threshold, 255, threshold_type);
-
-	cv::Mat edges_color(in.size(), CV_8UC4);
-	cv::addWeighted(edges, weight, out, 1.0, 0.0, out, CV_8UC4);
-}*/
-
-
-void ftl::gui::Camera::active(bool a) {
-	if (a) {
-
-	} else {
-		neye_[0] = eye_[0];
-		neye_[1] = eye_[1];
-		neye_[2] = eye_[2];
-	}
-}
-
-void ftl::gui::Camera::drawOverlay(const Eigen::Vector2f &s) {
-	if (!framesets_) return;
-	//UNIQUE_LOCK(mutex_,lk);
-	for (auto *fs : *framesets_) {
-		if (!usesFrameset(fs->id)) continue;
-
-		// Generate and upload an overlay image.
-		overlayer_->draw(*fs, state_, s);
-	}
-}
-
-const void ftl::gui::Camera::captureFrame() {
-	float now = (float)glfwGetTime();
-	if (!screen_->isVR() && (now - ftime_) < 0.04f) return;
-
-	delta_ = now - ftime_;
-	ftime_ = now;
-
-	//LOG(INFO) << "Frame delta: " << delta_;
-
-	//if (src_ && src_->isReady()) {
-	if (width_ > 0 && height_ > 0) {
-		Eigen::Matrix4d viewPose;
-
-		if (screen_->isVR()) {
-			#ifdef HAVE_OPENVR
-
-			vr::VRCompositor()->SetTrackingSpace(vr::TrackingUniverseStanding);
-			vr::VRCompositor()->WaitGetPoses(rTrackedDevicePose_, vr::k_unMaxTrackedDeviceCount, NULL, 0 );
-
-			if (isStereo() && rTrackedDevicePose_[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid )
-			{
-				Eigen::Matrix4d eye_l = ConvertSteamVRMatrixToMatrix4(
-					vr::VRSystem()->GetEyeToHeadTransform(vr::Eye_Left));
-
-				//Eigen::Matrix4d eye_r = ConvertSteamVRMatrixToMatrix4(
-				//	vr::VRSystem()->GetEyeToHeadTransform(vr::Eye_Left));
-
-				float baseline_in = 2.0 * eye_l(0, 3);
-
-				if (baseline_in != baseline_) {
-					baseline_ = baseline_in;
-					//src_->set("baseline", baseline_);
-					state_.getLeft().baseline = baseline_;
-					state_.getRight().baseline = baseline_;
-				}
-				Eigen::Matrix4d pose = ConvertSteamVRMatrixToMatrix4(rTrackedDevicePose_[vr::k_unTrackedDeviceIndex_Hmd].mDeviceToAbsoluteTracking);
-				Eigen::Vector3d ea = pose.block<3, 3>(0, 0).eulerAngles(0, 1, 2);
-
-				Eigen::Vector3d vreye;
-				vreye[0] = pose(0, 3);
-				vreye[1] = -pose(1, 3);
-				vreye[2] = -pose(2, 3);
-
-				// NOTE: If modified, should be verified with VR headset!
-				Eigen::Matrix3d R;
-				R =		Eigen::AngleAxisd(ea[0], Eigen::Vector3d::UnitX()) *
-						Eigen::AngleAxisd(-ea[1], Eigen::Vector3d::UnitY()) *
-						Eigen::AngleAxisd(-ea[2], Eigen::Vector3d::UnitZ());
-
-				//double rd = 180.0 / 3.141592;
-				//LOG(INFO) << "rotation x: " << ea[0] *rd << ", y: " << ea[1] * rd << ", z: " << ea[2] * rd;
-				// pose.block<3, 3>(0, 0) = R;
-
-				rotmat_.block(0, 0, 3, 3) = R;
-
-				// TODO: Apply a rotation to orient also
-
-				eye_[0] += (neye_[0] - eye_[0]) * lerpSpeed_ * delta_;
-				eye_[1] += (neye_[1] - eye_[1]) * lerpSpeed_ * delta_;
-				eye_[2] += (neye_[2] - eye_[2]) * lerpSpeed_ * delta_;
-
-				Eigen::Translation3d trans(eye_ + vreye);
-				Eigen::Affine3d t(trans);
-				viewPose = t.matrix() * rotmat_;
-
-			} else {
-				//LOG(ERROR) << "No VR Pose";
-			}
-			#endif
-		} else {
-			if (pose_source_.size() == 0) {
-				// Use mouse to move camera
-
-				float rrx = ((float)ry_ * 0.2f * delta_);
-				float rry = (float)rx_ * 0.2f * delta_;
-				float rrz = 0.0;
-
-				Eigen::Affine3d r = create_rotation_matrix(rrx, -rry, rrz);
-				rotmat_ = rotmat_ * r.matrix();
-
-				rx_ = 0;
-				ry_ = 0;
-
-				eye_[0] += (neye_[0] - eye_[0]) * lerpSpeed_ * delta_;
-				eye_[1] += (neye_[1] - eye_[1]) * lerpSpeed_ * delta_;
-				eye_[2] += (neye_[2] - eye_[2]) * lerpSpeed_ * delta_;
-
-				Eigen::Translation3d trans(eye_);
-				Eigen::Affine3d t(trans);
-				viewPose = t.matrix() * rotmat_;
-			} else {
-				// Use some other pose source.
-				if (!ftl::operators::Poser::get(pose_source_, viewPose)) {
-					LOG(ERROR) << "Missing pose: " << pose_source_;
-				}
-			}
-		}
-
-		{
-			UNIQUE_LOCK(mutex_, lk);
-
-			if (isVirtual()) {
-				if (transform_ix_ == -1) {
-					state_.setPose(viewPose);
-				} else if (transform_ix_ >= 0) {
-					transforms_[transform_ix_] = viewPose;
-				}
-			}
-		}
-
-		if (framesets_) draw(*framesets_);
-	}
-
-	//return texture1_;
-}
-
-void ftl::gui::Camera::snapshot(const std::string &filename) {
-	/*cv::Mat flipped;
-
-	{
-		UNIQUE_LOCK(mutex_, lk);
-		//cv::flip(im1_, flipped, 0);
-	}
-	cv::cvtColor(flipped, flipped, cv::COLOR_BGRA2BGR);
-	cv::imwrite(filename, flipped);*/
-	snapshot_filename_ = filename;
-	do_snapshot_ = true;
-}
-
-void ftl::gui::Camera::startVideoRecording(const std::string &filename, const std::string &uri) {
-	if (!record_stream_) {
-		file_stream_ = ftl::create<ftl::stream::File>(screen_->root(), "video2d");
-		file_stream_->setMode(ftl::stream::File::Mode::Write);
-		net_stream_ = ftl::create<ftl::stream::Net>(screen_->root(), "liveStream", screen_->net());
-
-		record_stream_ = ftl::create<ftl::stream::Broadcast>(screen_->root(), "recordStream");
-		//record_stream_->add(file_stream_);
-		//record_stream_->add(net_stream_);
-
-		record_sender_ = ftl::create<ftl::stream::Sender>(screen_->root(), "videoEncode");
-		record_sender_->value("codec", 2);  // Default H264
-		record_sender_->set("iframes", 50);  // Add iframes by default
-		record_sender_->value("stereo", false);  // If both channels, then default to stereo
-
-		record_sender_->onRequest([this](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
-			if (spkt.channel == ftl::codecs::Channel::Pose) {
-				auto pose = ftl::stream::parsePose(pkt);
-				ftl::operators::Poser::set(std::string("live"), pose);
-			}
-		});
-	}
-
-	if (record_stream_->active()) return;
-
-	record_stream_->clear();
-
-	if (filename.size() > 0) {
-		file_stream_->set("filename", filename);
-		record_stream_->add(file_stream_);
-	}
-
-	if (uri.size() > 0) {
-		net_stream_->set("uri", uri);
-		record_stream_->add(net_stream_);
-	}
-
-	record_sender_->setStream(record_stream_);
-
-	LOG(INFO) << "About to record";
-	if (record_stream_->begin()) LOG(INFO) << "Recording started...";
-}
-
-void ftl::gui::Camera::stopVideoRecording() {
-	if (record_stream_ && record_stream_->active()) record_stream_->end();
-}
-
-float ftl::gui::Camera::getDepth(int x, int y) {
-	if (x < 0 || y < 0) { return NAN; }
-	UNIQUE_LOCK(mutex_, lk);
-	if (x >= im_depth_.cols || y >= im_depth_.rows) { return NAN; }
-	LOG(INFO) << y << ", " << x;
-	return im_depth_.createMatHeader().at<float>(y, x);
-}
-
-cv::Point3f ftl::gui::Camera::getPoint(int x, int y) {
-	if (x < 0 || y < 0) { return cv::Point3f(); }
-	UNIQUE_LOCK(mutex_, lk);
-		LOG(INFO) << y << ", " << x;
-	if (x >= im_depth_.cols || y >= im_depth_.rows) { return cv::Point3f(); }
-	float d = im_depth_.createMatHeader().at<float>(y, x);
-
-	auto point = frame_.getLeftCamera().screenToCam(x, y, d);
-	Eigen::Vector4d p(point.x, point.y, point.z, 1.0f);
-	Eigen::Matrix4d pose = frame_.getPose();
-	Eigen::Vector4d point_eigen = pose * p;
-	return cv::Point3f(point_eigen(0), point_eigen(1), point_eigen(2));
-}
-
-/*
-cv::Point3f ftl::gui::Camera::getNormal(int x, int y) {
-	UNIQUE_LOCK(mutex_, lk);
-		LOG(INFO) << y << ", " << x;
-	if (x >= im_normals_.cols || y >= im_normals_.rows) { return cv::Point3f(); }
-	auto n = im_normals_f_.at<cv::Vec4f>(y, x);
-	return cv::Point3f(n[0], n[1], n[2]);
-}
-*/
-
-void ftl::gui::Camera::setTransform(const Eigen::Matrix4d &T) {
-	T_ = T * T_;
-}
-
-Eigen::Matrix4d ftl::gui::Camera::getTransform() const {
-	return T_;
-}

applications/gui/src/camera.hpp

-#ifndef _FTL_GUI_CAMERA_HPP_
-#define _FTL_GUI_CAMERA_HPP_
-
-#include <ftl/rgbd/frameset.hpp>
-#include <ftl/render/CUDARender.hpp>
-#include <ftl/render/overlay.hpp>
-#include <ftl/codecs/writer.hpp>
-#include "gltexture.hpp"
-
-#include <ftl/streams/filestream.hpp>
-#include <ftl/streams/netstream.hpp>
-#include <ftl/streams/sender.hpp>
-#include <ftl/codecs/faces.hpp>
-
-#include <string>
-#include <array>
-
-#ifdef HAVE_OPENVR
-#include <openvr/openvr.h>
-#endif
-
-class StatisticsImage;
-
-namespace ftl {
-namespace gui {
-
-class Screen;
-class PoseWindow;
-
-class Camera {
-	public:
-	Camera(ftl::gui::Screen *screen, int fsmask, int fid, ftl::codecs::Channel chan=ftl::codecs::Channel::Colour);
-	~Camera();
-
-	Camera(const Camera &)=delete;
-
-	int width() const { return width_; }
-	int height() const { return height_; }
-
-	int getFramesetMask() const { return fsmask_; }
-
-	bool usesFrameset(int id) const { return fsmask_ & (1 << id); }
-
-	void setPose(const Eigen::Matrix4d &p);
-
-	void mouseMovement(int rx, int ry, int button);
-	void keyMovement(int key, int modifiers);
-
-	void showPoseWindow();
-	void showSettings();
-
-	void setChannel(ftl::codecs::Channel c);
-	const ftl::codecs::Channel getChannel() { return channel_; }
-	
-	void togglePause();
-	void isPaused();
-	inline bool isVirtual() const { return fid_ == 255; }
-	const ftl::codecs::Channels<0> &availableChannels() { return channels_; }
-	inline bool isStereo() const { return stereo_; }
-
-	void setStereo(bool v);
-
-	/**
-	 * Main function to obtain latest frames.
-	 */
-	void update(std::vector<ftl::rgbd::FrameSet *> &fss);
-
-	/**
-	 * Update the available channels.
-	 */
-	void update(int fsid, const ftl::codecs::Channels<0> &c);
-
-	/**
-	 * Draw virtual camera only if the frameset has been updated since last
-	 * draw.
-	 */
-	void drawUpdated(std::vector<ftl::rgbd::FrameSet*> &fss);
-
-	void draw(std::vector<ftl::rgbd::FrameSet*> &fss);
-
-	void drawOverlay(const Eigen::Vector2f &);
-
-	inline int64_t getFrameTimeMS() const { return int64_t(delta_ * 1000.0f); }
-
-	const ftl::rgbd::Camera &getIntrinsics() const { return state_.getLeft(); }
-	const Eigen::Matrix4d getPose() const { UNIQUE_LOCK(mutex_, lk); return state_.getPose(); }
-
-	/**
-	 * @internal. Used to inform the camera if it is the active camera or not.
-	 */
-	void active(bool);
-
-	const void captureFrame();
-	const GLTexture &getLeft() const { return texture1_; }
-	const GLTexture &getRight() const { return texture2_; }
-	const GLTexture &getDepth() const { return depth1_; }
-
-	void snapshot(const std::string &filename);
-
-	void startVideoRecording(const std::string &filename, const std::string &uri);
-
-	void stopVideoRecording();
-
-	//nlohmann::json getMetaData();
-
-	const std::string &name() const { return name_; }
-
-	StatisticsImage *stats_ = nullptr;
-
-	float getDepth(int x, int y);
-	float getDepth(float x, float y) { return getDepth((int) round(x), (int) round(y)); }
-	cv::Point3f getPoint(int x, int y);
-	cv::Point3f getPoint(float x, float y) { return getPoint((int) round(x), (int) round(y)); }
-	//cv::Point3f getNormal(int x, int y);
-	//cv::Point3f getNormal(float x, float y) { return getNormal((int) round(x), (int) round(y)); }
-	void setTransform(const Eigen::Matrix4d &T);
-	Eigen::Matrix4d getTransform() const;
-	const std::vector<std::string> &getMessages() const { return msgs_; }
-
-#ifdef HAVE_OPENVR
-	bool isVR() { return vr_mode_; }
-	bool setVR(bool on);
-#else
-	bool isVR() { return false; }
-#endif
-
-	private:
-
-	Screen *screen_;
-	unsigned int fsmask_;  // Frameset Mask
-	int fid_;
-
-	int width_;
-	int height_;
-
-	GLTexture texture1_; // first channel (always left at the moment)
-	GLTexture texture2_; // second channel ("right")
-	GLTexture depth1_;
-
-	ftl::gui::PoseWindow *posewin_;
-	//nlohmann::json meta_;
-	Eigen::Vector4d neye_;
-	Eigen::Vector3d eye_;
-	//Eigen::Vector3f orientation_;
-	Eigen::Matrix4d rotmat_;
-	float ftime_;
-	float delta_;
-	float lerpSpeed_;
-	bool sdepth_;
-	bool pause_;
-	bool do_snapshot_ = false;
-	std::string pose_source_;
-	std::string snapshot_filename_;
-	ftl::codecs::Channel channel_;
-	ftl::codecs::Channels<0> channels_;
-	
-	cv::cuda::HostMem im_depth_;
-	//cv::cuda::HostMem im_normals_;
-	cv::Mat im_normals_f_;
-
-	cv::Mat overlay_; // first channel (left)
-	bool stereo_;
-	std::atomic_flag stale_frame_;
-	int rx_;
-	int ry_;
-	std::vector<ftl::rgbd::FrameSet*> *framesets_;
-
-	ftl::render::CUDARender *renderer_;
-	ftl::render::CUDARender *renderer2_;
-	ftl::render::Colouriser *colouriser_;
-	ftl::overlay::Overlay *overlayer_;
-
-	ftl::Configurable *intrinsics_;
-	ftl::operators::Graph *post_pipe_;
-	ftl::rgbd::Frame frame_;
-	ftl::rgbd::FrameState state_;
-	ftl::stream::File *file_stream_;
-	ftl::stream::Net *net_stream_;
-	ftl::stream::Broadcast *record_stream_;
-	ftl::stream::Sender *record_sender_;
-
-	std::string name_;
-
-	std::vector<std::string> msgs_;
-
-	int transform_ix_;
-	std::array<Eigen::Matrix4d,ftl::stream::kMaxStreams> transforms_;  // Frameset transforms for virtual cam
-	Eigen::Matrix4d T_ = Eigen::Matrix4d::Identity();
-
-	mutable MUTEX mutex_;
-
-	#ifdef HAVE_OPENVR
-	vr::TrackedDevicePose_t rTrackedDevicePose_[ vr::k_unMaxTrackedDeviceCount ];
-	bool vr_mode_;
-	float baseline_;
-	#endif
-
-	void _downloadFrames(ftl::cuda::TextureObject<uchar4> &, ftl::cuda::TextureObject<uchar4> &);
-	void _downloadFrames(ftl::cuda::TextureObject<uchar4> &);
-	void _downloadFrames();
-	void _draw(std::vector<ftl::rgbd::FrameSet*> &fss);
-	void _applyPoseEffects(std::vector<ftl::rgbd::FrameSet*> &fss);
-	std::pair<const ftl::rgbd::Frame *, const ftl::codecs::Face *> _selectFace(std::vector<ftl::rgbd::FrameSet*> &fss);
-	void _generateWindow(const ftl::rgbd::Frame &, const ftl::codecs::Face &face, Eigen::Matrix4d &pose_adjust, ftl::render::ViewPort &vp);
-};
-
-}
-}
-
-#endif  // _FTL_GUI_CAMERA_HPP_

applications/gui/src/ctrl_window.cpp

-#include "ctrl_window.hpp"
-
-#include "config_window.hpp"
-
-#include <nanogui/layout.h>
-#include <nanogui/label.h>
-#include <nanogui/combobox.h>
-#include <nanogui/button.h>
-#include <nanogui/entypo.h>
-
-#include <vector>
-#include <string>
-
-using ftl::gui::ControlWindow;
-using ftl::gui::ConfigWindow;
-using std::string;
-using std::vector;
-
-
-ControlWindow::ControlWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl)
-		: nanogui::Window(parent, "Network Connections"), ctrl_(ctrl) {
-	setLayout(new nanogui::GroupLayout());
-
-	using namespace nanogui;
-
-	_updateDetails();
-
-	auto tools = new Widget(this);
-	tools->setLayout(new BoxLayout(	Orientation::Horizontal,
-									Alignment::Middle, 0, 6));
-
-	auto button = new Button(tools, "", ENTYPO_ICON_PLUS);
-	button->setCallback([this] {
-		// Show new connection dialog
-		_addNode();
-	});
-	button->setTooltip("Add new node");
-	
-	// commented-out buttons not working/useful
-	/*
-	button = new Button(tools, "", ENTYPO_ICON_CYCLE);
-	button->setCallback([this] {
-		ctrl_->restart();
-	});
-	button = new Button(tools, "", ENTYPO_ICON_CONTROLLER_PAUS);
-	button->setCallback([this] {
-		ctrl_->pause();
-	});
-	button->setTooltip("Pause all nodes");*/
-
-	new Label(this, "Select Node","sans-bold");
-	auto select = new ComboBox(this, node_titles_);
-	select->setCallback([this](int ix) {
-		//LOG(INFO) << "Change node: " << ix;
-		_changeActive(ix);
-	});
-
-	new Label(this, "Node Options","sans-bold");
-
-	tools = new Widget(this);
-	tools->setLayout(new BoxLayout(	Orientation::Horizontal,
-									Alignment::Middle, 0, 6));
-
-	/*button = new Button(tools, "", ENTYPO_ICON_INFO);
-	button->setCallback([this] {
-		
-	});
-	button->setTooltip("Node status information");*/
-
-	button = new Button(tools, "", ENTYPO_ICON_COG);
-	button->setCallback([this,parent] {
-		auto cfgwin = new ConfigWindow(parent, ctrl_);
-		cfgwin->setTheme(theme());
-	});
-	button->setTooltip("Edit node configuration");
-
-	/*button = new Button(tools, "", ENTYPO_ICON_CYCLE);
-	button->setCallback([this] {
-		ctrl_->restart(_getActiveID());
-	});
-	button->setTooltip("Restart this node");*/
-
-	/*button = new Button(tools, "", ENTYPO_ICON_CONTROLLER_PAUS);
-	button->setCallback([this] {
-		ctrl_->pause(_getActiveID());
-	});
-	button->setTooltip("Pause node processing");*/
-
-	ctrl->getNet()->onConnect([this,select](ftl::net::Peer *p) {
-		_updateDetails();
-		select->setItems(node_titles_);
-	});
-
-	_changeActive(0);
-}
-
-ControlWindow::~ControlWindow() {
-
-}
-
-void ControlWindow::_addNode() {
-	using namespace nanogui;
-
-	FormHelper *form = new FormHelper(this->screen());
-	form->addWindow(Vector2i(100,100), "Add Node");
-
-	auto var = form->addVariable("URI", add_node_uri_);
-	var->setValue("tcp://localhost:9001");
-	var->setFixedWidth(200);
-
-	form->addButton("Add", [this,form](){
-		ctrl_->getNet()->connect(add_node_uri_);
-		form->window()->setVisible(false);
-		delete form;
-	})->setIcon(ENTYPO_ICON_PLUS);
-
-	form->addButton("Close", [form]() {
-		form->window()->setVisible(false);
-		delete form;
-	})->setIcon(ENTYPO_ICON_CROSS);
-}
-
-void ControlWindow::_updateDetails() {
-	node_details_ = ctrl_->getControllers();
-
-	node_titles_.clear();
-	for (auto &d : node_details_) {
-		node_titles_.push_back(d["title"].get<string>());
-	}
-}
-
-void ControlWindow::_changeActive(int ix) {
-	active_ix_ = ix;
-}
-
-ftl::UUID ControlWindow::_getActiveID() {
-	return ftl::UUID(node_details_[active_ix_]["id"].get<string>());
-}
-