From 1012ed80f217ef02a73c56f601a359fc3a0e55e4 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nicolas.pope@utu.fi>
Date: Thu, 20 Jun 2019 08:08:04 +0300
Subject: [PATCH] Partial completion of #95

---
 applications/gui/src/main.cpp                 |   6 +-
 applications/gui/src/src_window.cpp           |  81 ----
 .../reconstruct/include/ftl/registration.hpp  |   8 +-
 applications/reconstruct/src/main.cpp         |  15 +-
 applications/reconstruct/src/registration.cpp |  11 +-
 .../reconstruct/src/virtual_source.cpp        |   3 +-
 .../registration/src/correspondances.cpp      | 371 +++++++++++++++---
 .../registration/src/correspondances.hpp      |  26 +-
 applications/registration/src/manual.cpp      | 163 +++-----
 .../cpp/include/ftl/rgbd_display.hpp          |   8 +-
 components/renderers/cpp/src/rgbd_display.cpp |  20 +-
 .../include/ftl/rgbd/detail/source.hpp        |   2 +-
 .../include/ftl/rgbd/snapshot.hpp             |   8 +-
 .../rgbd-sources/include/ftl/rgbd/source.hpp  |  16 +-
 components/rgbd-sources/src/local.cpp         |   5 +-
 components/rgbd-sources/src/net.cpp           |   2 +-
 components/rgbd-sources/src/net.hpp           |   2 +-
 components/rgbd-sources/src/snapshot.cpp      |  14 +-
 .../rgbd-sources/src/snapshot_source.cpp      |   2 +-
 components/rgbd-sources/src/source.cpp        |  34 +-
 components/rgbd-sources/src/streamer.cpp      |   2 +-
 21 files changed, 484 insertions(+), 315 deletions(-)

diff --git a/applications/gui/src/main.cpp b/applications/gui/src/main.cpp
index d070e3941..979fc3359 100644
--- a/applications/gui/src/main.cpp
+++ b/applications/gui/src/main.cpp
@@ -305,13 +305,13 @@ class FTLApplication : public nanogui::Screen {
 				Eigen::Vector4f camPos;
 
 				try {
-					camPos = src_->point(sx,sy);
+					camPos = src_->point(sx,sy).cast<float>();
 				} catch(...) {
 					return true;
 				}
 				
 				camPos *= -1.0f;
-				Eigen::Vector4f worldPos =  src_->getPose() * camPos;
+				Eigen::Vector4f worldPos =  src_->getPose().cast<float>() * camPos;
 				//lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
 				LOG(INFO) << "Depth at click = " << -camPos[2];
 				return true;
@@ -374,7 +374,7 @@ class FTLApplication : public nanogui::Screen {
 			Eigen::Affine3f t(trans);
 			Eigen::Matrix4f viewPose = (t * r).matrix();
 
-			src_->setPose(viewPose);
+			src_->setPose(viewPose.cast<double>());
 			src_->grab();
 			src_->getFrames(rgb, depth);
 
diff --git a/applications/gui/src/src_window.cpp b/applications/gui/src/src_window.cpp
index 1aa1bd353..ad61ccccb 100644
--- a/applications/gui/src/src_window.cpp
+++ b/applications/gui/src/src_window.cpp
@@ -74,87 +74,6 @@ Eigen::Matrix<T,4,4> lookAt
 	return res;
 }
 
-class VirtualCameraView : public nanogui::ImageView {
-	public:
-	VirtualCameraView(nanogui::Widget *parent) : nanogui::ImageView(parent, 0) {
-		src_ = nullptr;
-		eye_ = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
-		centre_ = Eigen::Vector3f(0.0f, 0.0f, -4.0f);
-		up_ = Eigen::Vector3f(0,1.0f,0);
-		lookPoint_ = Eigen::Vector3f(0.0f,0.0f,-4.0f);
-		lerpSpeed_ = 0.4f;
-		depth_ = false;
-	}
-
-	void setSource(Source *src) { src_ = src; }
-
-	bool mouseButtonEvent(const nanogui::Vector2i &p, int button, bool down, int modifiers) {
-		//LOG(INFO) << "Mouse move: " << p[0];
-		if (src_ && down) {
-			Eigen::Vector4f camPos = src_->point(p[0],p[1]);
-			camPos *= -1.0f;
-			Eigen::Vector4f worldPos =  src_->getPose() * camPos;
-			lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
-			LOG(INFO) << "Depth at click = " << -camPos[2];
-		}
-	}
-
-	bool keyboardEvent(int key, int scancode, int action, int modifiers) {
-		LOG(INFO) << "Key press" << key << " - " << action;
-		if (key == 81 || key == 83) {
-			// TODO Should rotate around lookAt object, but requires correct depth
-			Eigen::Quaternion<float> q;  q = Eigen::AngleAxis<float>((key == 81) ? 0.01f : -0.01f, up_);
-			eye_ = (q * (eye_ - centre_)) + centre_;
-		} else if (key == 84 || key == 82) {
-			float scalar = (key == 84) ? 0.99f : 1.01f;
-			eye_ = ((eye_ - centre_) * scalar) + centre_;
-		}
-	}
-
-	void draw(NVGcontext *ctx) {
-		//net_->broadcast("grab");
-		if (src_) {
-			cv::Mat rgb, depth;
-			centre_ += (lookPoint_ - centre_) * (lerpSpeed_ * 0.1f);
-			Eigen::Matrix4f viewPose = lookAt<float>(eye_,centre_,up_).inverse();
-
-			src_->setPose(viewPose);
-			src_->grab();
-			src_->getFrames(rgb, depth);
-
-			if (depth_) {
-				if (depth.rows > 0) {
-					cv::Mat idepth;
-					depth.convertTo(idepth, CV_8U, 255.0f / 10.0f);  // TODO(nick)
-    				applyColorMap(idepth, idepth, cv::COLORMAP_JET);
-					texture_.update(idepth);
-					bindImage(texture_.texture());
-				}
-			} else {
-				if (rgb.rows > 0) {
-					texture_.update(rgb);
-					bindImage(texture_.texture());
-				}
-			}
-
-			screen()->performLayout(ctx);
-		}
-		ImageView::draw(ctx);
-	}
-
-	void setDepth(bool d) { depth_ = d; }
-
-	private:
-	Source *src_;
-	GLTexture texture_;
-	Eigen::Vector3f eye_;
-	Eigen::Vector3f centre_;
-	Eigen::Vector3f up_;
-	Eigen::Vector3f lookPoint_;
-	float lerpSpeed_;
-	bool depth_;
-};
-
 SourceWindow::SourceWindow(nanogui::Widget *parent, ftl::ctrl::Master *ctrl)
 		: nanogui::Window(parent, "Source View"), ctrl_(ctrl) {
 	setLayout(new nanogui::GroupLayout());
diff --git a/applications/reconstruct/include/ftl/registration.hpp b/applications/reconstruct/include/ftl/registration.hpp
index be5154f30..20f8eac6a 100644
--- a/applications/reconstruct/include/ftl/registration.hpp
+++ b/applications/reconstruct/include/ftl/registration.hpp
@@ -14,11 +14,11 @@
 namespace ftl {
 namespace registration {
 
-void to_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4f> &transformations);
-void from_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4f> &transformations);
+void to_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4d> &transformations);
+void from_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4d> &transformations);
 
-bool loadTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4f> &data);
-bool saveTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4f> &data);
+bool loadTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4d> &data);
+bool saveTransformations(const std::string &path, std::map<std::string, Eigen::Matrix4d> &data);
 
 /** @brief	Find transformation matrix for transforming clouds_source to clouds_target.
  *			Assumes that corresponding points in clouds_source[i] and clouds_target[i] have same indices.
diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp
index d1c2c095a..f3a29a9fd 100644
--- a/applications/reconstruct/src/main.cpp
+++ b/applications/reconstruct/src/main.cpp
@@ -102,13 +102,13 @@ static void run(ftl::Configurable *root) {
 	if (!merge) {
 		LOG(WARNING) << "Input merging not configured, using only first input in configuration";
 		inputs = { inputs[0] };
-		inputs[0].source->setPose(Eigen::Matrix4f::Identity());
+		inputs[0].source->setPose(Eigen::Matrix4d::Identity());
 	}
 
 	if (inputs.size() > 1) {
-		std::map<std::string, Eigen::Matrix4f> transformations;
+		std::map<std::string, Eigen::Matrix4d> transformations;
 
-		if ((*merge)["register"]) {
+		/*if ((*merge)["register"]) {
 			LOG(INFO) << "Registration requested";
 
 			ftl::registration::Registration *reg = ftl::registration::ChessboardRegistration::create(*merge);
@@ -129,14 +129,14 @@ static void run(ftl::Configurable *root) {
 
 			free(reg);
 		}
-		else {
+		else {*/
 			if (loadTransformations(string(FTL_LOCAL_CONFIG_ROOT) + "/registration.json", transformations)) {
 				LOG(INFO) << "Loaded camera trasformations from file";
 			}
 			else {
 				LOG(ERROR) << "Error loading camera transformations from file";
 			}
-		}
+		//}
 
 		for (auto &input : inputs) {
 			string uri = input.source->getURI();
@@ -146,7 +146,7 @@ static void run(ftl::Configurable *root) {
 				LOG(WARNING) << "Using only first configured source";
 				// TODO: use target source if configured and found
 				inputs = { inputs[0] };
-				inputs[0].source->setPose(Eigen::Matrix4f::Identity());
+				inputs[0].source->setPose(Eigen::Matrix4d::Identity());
 				break;
 			}
 			input.source->setPose(T->second);
@@ -244,7 +244,8 @@ static void run(ftl::Configurable *root) {
 				// Send to GPU and merge view into scene
 				inputs[i].gpu.updateParams(inputs[i].params);
 				inputs[i].gpu.updateData(depth, rgba);
-				scene->integrate(inputs[i].source->getPose(), inputs[i].gpu, inputs[i].params, nullptr);
+				// TODO(Nick): Use double pose
+				scene->integrate(inputs[i].source->getPose().cast<float>(), inputs[i].gpu, inputs[i].params, nullptr);
 			}
 		} else {
 			active = 1;
diff --git a/applications/reconstruct/src/registration.cpp b/applications/reconstruct/src/registration.cpp
index ec27f43ba..ad8c1e8cd 100644
--- a/applications/reconstruct/src/registration.cpp
+++ b/applications/reconstruct/src/registration.cpp
@@ -40,17 +40,18 @@ using pcl::PointXYZRGB;
 
 using cv::Mat;
 using Eigen::Matrix4f;
+using Eigen::Matrix4d;
 
-void from_json(nlohmann::json &json, map<string, Matrix4f> &transformations) {
+void from_json(nlohmann::json &json, map<string, Matrix4d> &transformations) {
 	for (auto it = json.begin(); it != json.end(); ++it) {
-		Eigen::Matrix4f m;
+		Eigen::Matrix4d m;
 		auto data = m.data();
 		for(size_t i = 0; i < 16; i++) { data[i] = it.value()[i]; }
 		transformations[it.key()] = m;
 	}
 }
 
-void to_json(nlohmann::json &json, map<string, Matrix4f> &transformations) {
+void to_json(nlohmann::json &json, map<string, Matrix4d> &transformations) {
 	for (auto &item : transformations) {
 		auto val = nlohmann::json::array();
 		for(size_t i = 0; i < 16; i++) { val.push_back((float) item.second.data()[i]); }
@@ -58,7 +59,7 @@ void to_json(nlohmann::json &json, map<string, Matrix4f> &transformations) {
 	}
 }
 
-bool loadTransformations(const string &path, map<string, Matrix4f> &data) {
+bool loadTransformations(const string &path, map<string, Matrix4d> &data) {
 	std::ifstream file(path);
 	if (!file.is_open()) {
 		LOG(ERROR) << "Error loading transformations from file " << path;
@@ -71,7 +72,7 @@ bool loadTransformations(const string &path, map<string, Matrix4f> &data) {
 	return true;
 }
 
-bool saveTransformations(const string &path, map<string, Matrix4f> &data) {
+bool saveTransformations(const string &path, map<string, Matrix4d> &data) {
 	nlohmann::json data_json;
 	to_json(data_json, data);
 	std::ofstream file(path);
diff --git a/applications/reconstruct/src/virtual_source.cpp b/applications/reconstruct/src/virtual_source.cpp
index 578b36090..0bc4abf4a 100644
--- a/applications/reconstruct/src/virtual_source.cpp
+++ b/applications/reconstruct/src/virtual_source.cpp
@@ -50,7 +50,8 @@ bool VirtualSource::grab() {
 		params.m_sensorDepthWorldMin = params_.minDepth;
 		params.m_sensorDepthWorldMax = params_.maxDepth;
 
-		rays_->render(scene_->getHashData(), scene_->getHashParams(), params, host_->getPose());
+		// TODO(Nick) Use double precision pose here
+		rays_->render(scene_->getHashData(), scene_->getHashParams(), params, host_->getPose().cast<float>());
 
 		//unique_lock<mutex> lk(mutex_);
 		if (rays_->isIntegerDepth()) {
diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
index 134b9088a..5292905e9 100644
--- a/applications/registration/src/correspondances.cpp
+++ b/applications/registration/src/correspondances.cpp
@@ -2,6 +2,9 @@
 #include <nlohmann/json.hpp>
 #include <random>
 #include <chrono>
+#include <thread>
+#include <opencv2/xphoto.hpp>
+#include <pcl/registration/icp.h>
 
 using ftl::registration::Correspondances;
 using std::string;
@@ -14,6 +17,7 @@ using pcl::PointXYZRGB;
 using nlohmann::json;
 using std::tuple;
 using std::make_tuple;
+using cv::Mat;
 
 
 Correspondances::Correspondances(Source *src, Source *targ)
@@ -30,61 +34,270 @@ Correspondances::Correspondances(Correspondances *parent, Source *src)
 	transform_.setIdentity();
 }
 
-bool Correspondances::add(int tx, int ty, int sx, int sy) {
-	Eigen::Vector4f p1 = targ_->point(tx,ty);
-	Eigen::Vector4f p2 = src_->point(sx,sy);
+static void averageDepth(vector<Mat> &in, Mat &out, float varThresh) {
+	const int rows = in[0].rows;
+	const int cols = in[0].cols;
+
+	// todo: create new only if out is empty (or wrong shape/type)
+	out = Mat(rows, cols, CV_32FC1);
+
+	for (int i = 0; i < rows * cols; ++i) {
+		double sum = 0;
+		int good_values = 0;
+
+		// Calculate mean
+		for (int i_in = 0; i_in < in.size(); ++i_in) {
+			double d = in[i_in].at<float>(i);
+			if (d < 40.0) {
+				good_values++;
+				sum += d;
+			}
+		}
+
+		if (good_values > 0) {
+			sum /= (double)good_values;
+
+			// Calculate variance
+			double var = 0.0;
+			for (int i_in = 0; i_in < in.size(); ++i_in) {
+				double d = in[i_in].at<float>(i);
+				if (d < 40.0) {
+					double delta = (d*1000.0 - sum*1000.0);
+					var += delta*delta;
+				}
+			}
+			if (good_values > 1) var /= (double)(good_values-1);
+			else var = 0.0;
+
+			//LOG(INFO) << "VAR " << var;
+
+			if (var < varThresh) {
+				out.at<float>(i) = (float)sum;
+			} else {
+				out.at<float>(i) = 41.0f;
+			}
+		} else {
+			out.at<float>(i) = 41.0f;
+		}
+	}
+}
+
+static PointXYZ makePCL(Source *s, int x, int y) {
+	Eigen::Vector4d p1 = s->point(x,y);
 	PointXYZ pcl_p1;
 	pcl_p1.x = p1[0];
 	pcl_p1.y = p1[1];
 	pcl_p1.z = p1[2];
+	return pcl_p1;
+}
+
+void Correspondances::clear() {
+	targ_cloud_->clear();
+	src_cloud_->clear();
+	src_index_ = cv::Mat(cv::Size(src_->parameters().width, src_->parameters().height), CV_32SC1, cv::Scalar(-1));
+	targ_index_ = cv::Mat(cv::Size(targ_->parameters().width, targ_->parameters().height), CV_32SC1, cv::Scalar(-1));
+	src_feat_.clear();
+	targ_feat_.clear();
+	log_.clear();
+}
+
+void Correspondances::clearCorrespondances() {
+	src_feat_.clear();
+	targ_feat_.clear();
+	log_.clear();
+	uptodate_ = false;
+}
 
-	PointXYZ pcl_p2;
-	pcl_p2.x = p2[0];
-	pcl_p2.y = p2[1];
-	pcl_p2.z = p2[2];
+bool Correspondances::capture(cv::Mat &rgb1, cv::Mat &rgb2) {
+	Mat d1, d2;
+	size_t buffer_size = 10;
+	size_t buffer_i = 0;
+	vector<vector<Mat>> buffer(2, vector<Mat>(buffer_size));
 
-	if (pcl_p2.z >= 40.0f || pcl_p1.z >= 40.0f) {
-		LOG(WARNING) << "Bad point choosen";
+	for (size_t i = 0; i < buffer_size; ++i) {
+		src_->grab();
+		targ_->grab();
+		src_->getFrames(rgb1, d1);
+		targ_->getFrames(rgb2, d2);
+		buffer[0][i] = d1;
+		buffer[1][i] = d2;
+
+		std::this_thread::sleep_for(std::chrono::milliseconds(50));
+	}
+	averageDepth(buffer[0], d1, 0.00000005f);
+	averageDepth(buffer[1], d2, 0.00000005f);
+	Mat d1_v, d2_v;
+	d1.convertTo(d1_v, CV_8U, 255.0f / 10.0f);
+	d2.convertTo(d2_v, CV_8U, 255.0f / 10.0f);
+	applyColorMap(d1_v, d1_v, cv::COLORMAP_JET);
+	applyColorMap(d2_v, d2_v, cv::COLORMAP_JET);
+
+	cv::imshow("smooth d1", d1_v);
+	cv::imshow("smooth d2", d2_v);
+
+	// Should be done in RGB-Depth source class
+	cv::Ptr<cv::xphoto::WhiteBalancer> wb;
+	wb = cv::xphoto::createSimpleWB();
+	wb->balanceWhite(rgb1, rgb1);
+	wb->balanceWhite(rgb2, rgb2);
+
+	// Build point clouds
+	clear();
+	int six=0;
+	int tix=0;
+
+	for (int y=0; y<rgb1.rows; y++) {
+		//unsigned char *rgb1ptr = rgb1.ptr(y);
+		//unsigned char *rgb2ptr = rgb2.ptr(y);
+		float *d1ptr = (float*)d1.ptr(y);
+		float *d2ptr = (float*)d2.ptr(y);
+
+		for (int x=0; x<rgb1.cols; x++) {
+			float d1_value = d1ptr[x];
+			float d2_value = d2ptr[x];
+
+			if (d1_value < 39.0f) {
+				// Make PCL points with specific depth value
+				pcl::PointXYZ p1;
+				Eigen::Vector4d p1e = src_->point(x,y,d1_value);
+				p1.x = p1e[0];
+				p1.y = p1e[1];
+				p1.z = p1e[2];
+				src_cloud_->push_back(p1);
+				src_index_.at<int>(y,x) = six;
+				six++;
+			}
+
+			if (d2_value < 39.0f) {
+				// Make PCL points with specific depth value
+				pcl::PointXYZ p2;
+				Eigen::Vector4d p2e = targ_->point(x,y,d2_value);
+				p2.x = p2e[0];
+				p2.y = p2e[1];
+				p2.z = p2e[2];
+				targ_cloud_->push_back(p2);
+				targ_index_.at<int>(y,x) = tix;
+				tix++;
+			}
+		}
+	}
+}
+
+bool Correspondances::add(int tx, int ty, int sx, int sy) {
+	LOG(INFO) << "Adding...";
+	int tix = targ_index_.at<int>(ty,tx);
+	int six = src_index_.at<int>(sy,sx);
+
+	// Validate feature
+	if (tix == -1 || six == -1) {
+		LOG(WARNING) << "Bad point";
 		return false;
 	}
 
-	targ_cloud_->push_back(pcl_p1);
-	src_cloud_->push_back(pcl_p2);
 	log_.push_back(make_tuple(tx,ty,sx,sy));
 
+	// Record correspondance point cloud point indexes
+	src_feat_.push_back(six);
+	targ_feat_.push_back(tix);
+
 	uptodate_ = false;
+	LOG(INFO) << "Point added: " << tx << "," << ty << " and " << sx << "," << sy;
 	return true;
 }
 
-PointXYZ makePCL(Source *s, int x, int y) {
-	Eigen::Vector4f p1 = s->point(x,y);
-	PointXYZ pcl_p1;
-	pcl_p1.x = p1[0];
-	pcl_p1.y = p1[1];
-	pcl_p1.z = p1[2];
-	return pcl_p1;
-}
-
 void Correspondances::removeLast() {
 	uptodate_ = false;
-	targ_cloud_->erase(targ_cloud_->end()-1);
-	src_cloud_->erase(src_cloud_->end()-1);
+	targ_feat_.erase(targ_feat_.end()-1);
+	src_feat_.erase(src_feat_.end()-1);
 	log_.pop_back();
 }
 
-double Correspondances::estimateTransform() {
+double Correspondances::estimateTransform(Eigen::Matrix4d &T) {
+	pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ, double> validate;
+	pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ, double> svd;
+
+	validate.setMaxRange(0.1);
+
+	svd.estimateRigidTransformation(*src_cloud_, src_feat_, *targ_cloud_, targ_feat_, T);
+	return validate.validateTransformation(src_cloud_, targ_cloud_, T);
+}
+
+double Correspondances::estimateTransform(Eigen::Matrix4d &T, const vector<int> &src_feat, const vector<int> &targ_feat) {
+	pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ, double> validate;
+	pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ, double> svd;
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+	vector<int> idx;
+
+	for (int i=0; i<src_feat.size(); i++) {
+		idx.push_back(i);
+		src_cloud->push_back(src_cloud_->at(src_feat[i]));
+		targ_cloud->push_back(targ_cloud_->at(targ_feat[i]));
+	}
+
+	pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
+
+	validate.setMaxRange(0.1);
+
+	svd.estimateRigidTransformation(*tsrc_cloud, idx, *targ_cloud, idx, T);
+	T = T * transform_;
 	uptodate_ = true;
-	int n_points = targ_cloud_->size();
+	float score = validate.validateTransformation(src_cloud, targ_cloud, T);
+	return score;
+}
+
+double Correspondances::icp() {
+	//pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr final_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
-	vector<int> idx(n_points);
-	for (int i = 0; i < n_points; i++) { idx[i] = i; }
+	pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
-	pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
-	pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
+	vector<int> idx;
 
-	svd.estimateRigidTransformation(*src_cloud_, idx, *targ_cloud_, idx, transform_);
+	for (int i=0; i<src_feat_.size(); i++) {
+		idx.push_back(i);
+		src_cloud->push_back(src_cloud_->at(src_feat_[i]));
+		targ_cloud->push_back(targ_cloud_->at(targ_feat_[i]));
+	}
 
-	return validate.validateTransformation(src_cloud_, targ_cloud_, transform_);
+	pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
+
+	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ, double> icp;
+	icp.setInputSource(tsrc_cloud);
+	icp.setInputTarget(targ_cloud);
+	icp.align(*final_cloud);
+	LOG(INFO) << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore();
+	transform_ *= icp.getFinalTransformation();
+	return icp.getFitnessScore();
+}
+
+double Correspondances::icp(const Eigen::Matrix4d &T_in, Eigen::Matrix4d &T_out, const vector<int> &idx) {
+	//pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr final_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr tsrc_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+	for (int i=0; i<idx.size(); i++) {
+		src_cloud->push_back(src_cloud_->at(src_feat_[idx[i]]));
+		targ_cloud->push_back(targ_cloud_->at(targ_feat_[idx[i]]));
+	}
+
+	pcl::transformPointCloud(*src_cloud, *tsrc_cloud, T_in);
+
+	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ, double> icp;
+	icp.setInputSource(tsrc_cloud);
+	icp.setInputTarget(targ_cloud);
+	icp.align(*final_cloud);
+	LOG(INFO) << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore();
+	T_out = T_in * icp.getFinalTransformation();
+	return icp.getFitnessScore();
 }
 
 static std::default_random_engine generator;
@@ -99,64 +312,94 @@ void Correspondances::convertToPCL(const std::vector<std::tuple<int,int,int,int>
 	}
 }
 
-double Correspondances::findBest(Eigen::Matrix4f &tr, const std::vector<std::tuple<int,int,int,int>> &p, const std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &pp, int K, int N) {
-	double score = 10000.0f;
-	vector<tuple<int,int,int,int>> best;
-	Eigen::Matrix4f bestT;
+void Correspondances::getFeatures3D(std::vector<Eigen::Vector4d> &f) {
+	for (int i=0; i<src_feat_.size(); i++) {
+		Eigen::Vector4d p;
+		const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
+		p[0] = pp.x;
+		p[1] = pp.y;
+		p[2] = pp.z;
+		p[3] = 1.0;
+		f.push_back(p);
+	}
+}
 
-	pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
-	pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
+void Correspondances::getTransformedFeatures(std::vector<Eigen::Vector4d> &f) {
+	for (int i=0; i<src_feat_.size(); i++) {
+		Eigen::Vector4d p;
+		const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
+		p[0] = pp.x;
+		p[1] = pp.y;
+		p[2] = pp.z;
+		p[3] = 1.0;
+		f.push_back(transform_ * p);
+	}
+}
+
+void Correspondances::getTransformedFeatures2D(std::vector<Eigen::Vector2i> &f) {
+	for (int i=0; i<src_feat_.size(); i++) {
+		Eigen::Vector4d p;
+		const pcl::PointXYZ &pp = src_cloud_->at(src_feat_[i]);
+		p[0] = pp.x;
+		p[1] = pp.y;
+		p[2] = pp.z;
+		p[3] = 1.0;
+		f.push_back(src_->point(transform_ * p));
+	}
+}
+
+double Correspondances::findBestSubset(Eigen::Matrix4d &tr, int K, int N) {
+	double score = 10000.0f;
+	vector<int> best;
+	Eigen::Matrix4d bestT;
 
 	std::mt19937 rng(std::chrono::steady_clock::now().time_since_epoch().count());
-	std::uniform_int_distribution<int> distribution(0,p.size()-1);
+	std::uniform_int_distribution<int> distribution(0,src_feat_.size()-1);
 	//int dice_roll = distribution(generator);
 	auto dice = std::bind ( distribution, rng );
 
-	vector<int> idx(K);
-	for (int i = 0; i < K; i++) { idx[i] = i; }
+	vector<int> sidx(K);
+	vector<int> tidx(K);
+	//for (int i = 0; i < K; i++) { idx[i] = i; }
+
+	// 1. Build complete point clouds using filtered and smoothed depth maps
+	// 2. Build a full index map of x,y to point cloud index.
+	// 3. Generate random subsets of features using index map
+	// 4. Find minimum
 
 	for (int n=0; n<N; n++) {
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_t(new PointCloud<PointXYZ>);
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_s(new PointCloud<PointXYZ>);
 
-		vector<tuple<int,int,int,int>> ps;
+		sidx.clear();
+		tidx.clear();
 
+		vector<int> idx;
 		for (int k=0; k<K; k++) {
 			int r = dice();
-			//LOG(INFO) << "DICE " << r << "  -  " << K;
-			//ps.push_back(p[r]);
-			auto [sx,sy,tx,ty] = p[r];
-			//LOG(INFO) << "POINT " << sx << "," << sy;
-			//auto p1 = makePCL(src_, sx, sy);
-			//auto p2 = makePCL(targ_, tx, ty);
-
-			auto [p1,p2] = pp[r];
-
-			if (p1.z >= 30.0f || p2.z >= 30.0f) { k--; continue; }
-			ps.push_back(std::make_tuple(tx,ty,sx,sy));
-			cloud_s->push_back(p1);
-			cloud_t->push_back(p2);
+			idx.push_back(r);
+			sidx.push_back(src_feat_[r]);
+			tidx.push_back(targ_feat_[r]);
 		}
 
-		Eigen::Matrix4f T;
-		svd.estimateRigidTransformation(*cloud_s, idx, *cloud_t, idx, T);
-		float scoreT = validate.validateTransformation(cloud_s, cloud_t, T);
+		Eigen::Matrix4d T;
+		float scoreT = estimateTransform(T, sidx, tidx);
 
 		if (scoreT < score) {
 			score = scoreT;
-			best = ps;
 			bestT = T;
+			best = idx;
 		}
 	}
 
+	return icp(bestT, tr, best);
+
 	// TODO Add these best points to actual clouds.
-	log_ = best;
-	tr = bestT;
+	//log_ = best;
+	//tr = bestT;
 	//uptodate_ = true;
-	return score;
+	//return score;
 }
 
-Eigen::Matrix4f Correspondances::transform() {
-	if (!uptodate_) estimateTransform();
+Eigen::Matrix4d Correspondances::transform() {
+	if (!uptodate_) estimateTransform(transform_);
 	return (parent_) ? parent_->transform() * transform_ : transform_;
 }
diff --git a/applications/registration/src/correspondances.hpp b/applications/registration/src/correspondances.hpp
index bd08dd6dd..57f4442f6 100644
--- a/applications/registration/src/correspondances.hpp
+++ b/applications/registration/src/correspondances.hpp
@@ -32,6 +32,11 @@ class Correspondances {
 	ftl::rgbd::Source *source() { return src_; }
 	ftl::rgbd::Source *target() { return targ_; }
 
+	void clear();
+	void clearCorrespondances();
+
+	bool capture(cv::Mat &rgb1, cv::Mat &rgb2);
+
 	/**
 	 * Add a new correspondance point. The point will only be added if there is
 	 * a valid depth value at that location.
@@ -47,6 +52,9 @@ class Correspondances {
 	void removeLast();
 
 	const std::vector<std::tuple<int,int,int,int>> &screenPoints() const { return log_; }
+	void getFeatures3D(std::vector<Eigen::Vector4d> &f);
+	void getTransformedFeatures(std::vector<Eigen::Vector4d> &f);
+	void getTransformedFeatures2D(std::vector<Eigen::Vector2i> &f);
 
 	void setPoints(const std::vector<std::tuple<int,int,int,int>> &p) { log_ = p; }
 
@@ -55,9 +63,13 @@ class Correspondances {
 	 * 
 	 * @return Validation score of the transform.
 	 */
-	double estimateTransform();
+	double estimateTransform(Eigen::Matrix4d &);
+	double estimateTransform(Eigen::Matrix4d &T, const std::vector<int> &src_feat, const std::vector<int> &targ_feat);
+
+	double findBestSubset(Eigen::Matrix4d &tr, int K, int N);
 
-	double findBest(Eigen::Matrix4f &tr, const std::vector<std::tuple<int,int,int,int>> &p, const std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &p2, int K, int N);
+	double icp();
+	double icp(const Eigen::Matrix4d &T_in, Eigen::Matrix4d &T_out, const std::vector<int> &idx);
 
 	void convertToPCL(const std::vector<std::tuple<int,int,int,int>> &p, std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &p2);
 
@@ -65,9 +77,9 @@ class Correspondances {
 	 * Get the estimated transform. This includes any parent transforms to make
 	 * it relative to root camera.
 	 */
-	Eigen::Matrix4f transform();
+	Eigen::Matrix4d transform();
 
-	void setTransform(Eigen::Matrix4f &t) { uptodate_ = true; transform_ = t; }
+	void setTransform(Eigen::Matrix4d &t) { uptodate_ = true; transform_ = t; }
 
 	private:
 	Correspondances *parent_;
@@ -75,9 +87,13 @@ class Correspondances {
 	ftl::rgbd::Source *src_;
 	pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud_;
 	pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud_;
-	Eigen::Matrix4f transform_;
+	Eigen::Matrix4d transform_;
 	bool uptodate_;
 	std::vector<std::tuple<int,int,int,int>> log_;
+	cv::Mat src_index_;
+	cv::Mat targ_index_;
+	std::vector<int> targ_feat_;
+	std::vector<int> src_feat_;
 };
 
 }
diff --git a/applications/registration/src/manual.cpp b/applications/registration/src/manual.cpp
index ca7f1cbf5..be729155c 100644
--- a/applications/registration/src/manual.cpp
+++ b/applications/registration/src/manual.cpp
@@ -14,7 +14,6 @@
 #include <opencv2/imgcodecs.hpp>
 #include <opencv2/videoio.hpp>
 #include <opencv2/highgui.hpp>
-#include <opencv2/xphoto.hpp>
 
 #include <map>
 
@@ -46,16 +45,16 @@ static void setMouseAction(const std::string& winName, const MouseAction &action
 static MouseAction tmouse;
 static MouseAction smouse;
 
-void from_json(nlohmann::json &json, map<string, Eigen::Matrix4f> &transformations) {
+void from_json(nlohmann::json &json, map<string, Eigen::Matrix4d> &transformations) {
 	for (auto it = json.begin(); it != json.end(); ++it) {
-		Eigen::Matrix4f m;
+		Eigen::Matrix4d m;
 		auto data = m.data();
 		for(size_t i = 0; i < 16; i++) { data[i] = it.value()[i]; }
 		transformations[it.key()] = m;
 	}
 }
 
-static void to_json(nlohmann::json &json, map<string, Eigen::Matrix4f> &transformations) {
+static void to_json(nlohmann::json &json, map<string, Eigen::Matrix4d> &transformations) {
 	for (auto &item : transformations) {
 		auto val = nlohmann::json::array();
 		for(size_t i = 0; i < 16; i++) { val.push_back((float) item.second.data()[i]); }
@@ -63,7 +62,7 @@ static void to_json(nlohmann::json &json, map<string, Eigen::Matrix4f> &transfor
 	}
 }
 
-static bool saveTransformations(const string &path, map<string, Eigen::Matrix4f> &data) {
+static bool saveTransformations(const string &path, map<string, Eigen::Matrix4d> &data) {
 	nlohmann::json data_json;
 	to_json(data_json, data);
 	std::ofstream file(path);
@@ -77,7 +76,7 @@ static bool saveTransformations(const string &path, map<string, Eigen::Matrix4f>
 	return true;
 }
 
-bool loadTransformations(const string &path, map<string, Eigen::Matrix4f> &data) {
+bool loadTransformations(const string &path, map<string, Eigen::Matrix4d> &data) {
 	std::ifstream file(path);
 	if (!file.is_open()) {
 		LOG(ERROR) << "Error loading transformations from file " << path;
@@ -90,7 +89,7 @@ bool loadTransformations(const string &path, map<string, Eigen::Matrix4f> &data)
 	return true;
 }
 
-static void build_correspondances(const vector<Source*> &sources, map<string, Correspondances*> &cs, int origin, map<string, Eigen::Matrix4f> &old) {
+static void build_correspondances(const vector<Source*> &sources, map<string, Correspondances*> &cs, int origin, map<string, Eigen::Matrix4d> &old) {
 	Correspondances *last = nullptr;
 
 	cs[sources[origin]->getURI()] = nullptr;
@@ -128,32 +127,6 @@ static void build_correspondances(const vector<Source*> &sources, map<string, Co
 	}
 }
 
-void averageDepth(vector<Mat> &in, Mat &out) {
-	const int rows = in[0].rows;
-	const int cols = in[0].cols;
-
-	// todo: create new only if out is empty (or wrong shape/type)
-	out = Mat(rows, cols, CV_32FC1);
-
-	for (int i = 0; i < rows * cols; ++i) {
-		float sum = 0;
-		int good_values = 0;
-		for (int i_in = 0; i_in < in.size(); ++i_in) {
-			float d = in[i_in].at<float>(i);
-			if (d < 40) {
-				good_values++;
-				sum += d;
-			}
-		}
-
-		if (good_values > 0) {
-			out.at<float>(i) = sum / (float) good_values;
-		} else {
-			out.at<float>(i) = 41.0f;
-		}
-	}
-}
-
 void ftl::registration::manual(ftl::Configurable *root) {
 	using namespace cv;
 	
@@ -178,7 +151,7 @@ void ftl::registration::manual(ftl::Configurable *root) {
 	cv::namedWindow("Target", cv::WINDOW_KEEPRATIO);
 	cv::namedWindow("Source", cv::WINDOW_KEEPRATIO);
 
-	map<string, Eigen::Matrix4f> oldTransforms;
+	map<string, Eigen::Matrix4d> oldTransforms;
 	loadTransformations(root->value("output", string("./test.json")), oldTransforms);
 
 	//Correspondances c(sources[targsrc], sources[cursrc]);
@@ -209,12 +182,9 @@ void ftl::registration::manual(ftl::Configurable *root) {
 	bool depthtoggle = false;
 	double lastScore = 0.0;
 	bool insearch = false;
+	int point_n = -1;
 
 	Correspondances *current = corrs[sources[curtarget]->getURI()];
-	
-	// Features for current frame...
-	vector<tuple<int,int,int,int>> feat;
-	vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> pclfeat;
 
 	while (ftl::running) {
 		if (!freeze) {
@@ -249,21 +219,38 @@ void ftl::registration::manual(ftl::Configurable *root) {
 			tsdepth.convertTo(sdepth, CV_8U, 255.0f / 10.0f);
 			applyColorMap(sdepth, sdepth, cv::COLORMAP_JET);
 
-			Ptr<xphoto::WhiteBalancer> wb;
+			/*Ptr<xphoto::WhiteBalancer> wb;
 			wb = xphoto::createSimpleWB();
 			wb->balanceWhite(tsrgb, srgb);
-			wb->balanceWhite(ttrgb, trgb);
+			wb->balanceWhite(ttrgb, trgb);*/
 
 			// Apply points and labels...
 			auto &points = current->screenPoints();
-			for (auto &p : points) {
-				auto [tx,ty,sx,sy] = p;
+			if (point_n == -1) {
+				for (int i=0; i<points.size(); i++) {
+				//for (auto &p : points) {
+					auto [tx,ty,sx,sy] = points[i];
+					drawMarker(srgb, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
+					drawMarker(sdepth, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
+					drawMarker(trgb, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
+					drawMarker(tdepth, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
+				}
+			} else if(point_n < points.size()) {
+				auto [tx,ty,sx,sy] = points[point_n];
 				drawMarker(srgb, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
 				drawMarker(sdepth, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
 				drawMarker(trgb, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
 				drawMarker(tdepth, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
 			}
 
+			vector<Eigen::Vector2i> tpoints;
+			current->getTransformedFeatures2D(tpoints);
+			for (int i=0; i<tpoints.size(); i++) {
+				Eigen::Vector2i p = tpoints[i];
+				drawMarker(trgb, Point(p[0],p[1]), Scalar(255,0,0), MARKER_TILTED_CROSS);
+				drawMarker(tdepth, Point(p[0],p[1]), Scalar(255,0,0), MARKER_TILTED_CROSS);
+			}
+
 			// Add most recent click position
 			drawMarker(srgb, Point(lastSX, lastSY), Scalar(0,0,255), MARKER_TILTED_CROSS);
 			drawMarker(trgb, Point(lastTX, lastTY), Scalar(0,0,255), MARKER_TILTED_CROSS);
@@ -287,32 +274,42 @@ void ftl::registration::manual(ftl::Configurable *root) {
 			curtarget = key-48;
 			if (curtarget >= sources.size()) curtarget = 0;
 			current = corrs[sources[curtarget]->getURI()];
-			feat.clear();
-			pclfeat.clear();
 			lastScore = 1000.0;
 		} else if (key == 'd') {
 			depthtoggle = !depthtoggle;
 		} else if (key == 'c') {
-			feat.clear();
-			pclfeat.clear();
+			current->clear();
+			lastScore = 1000.0;
+		} else if (key == 'n') {
+			point_n++;
+		} else if (key == 'p') {
+			LOG(INFO) << "Captured..";
 			lastScore = 1000.0;
+			Mat f1,f2;
+			current->capture(f1,f2);
 		}else if (key == 'a') {
+			Eigen::Matrix4d t;
 			if (current->add(lastTX,lastTY,lastSX,lastSY)) {
 				lastTX = lastTY = lastSX = lastSY = 0;
-				lastScore = current->estimateTransform();
+				lastScore = current->estimateTransform(t);
+				current->setTransform(t);
 			}
 		} else if (key == 'u') {
 			current->removeLast();
-			lastScore = current->estimateTransform();
+			Eigen::Matrix4d t;
+			lastScore = current->estimateTransform(t);
+			current->setTransform(t);
+		} else if (key == 'i') {
+			current->icp();
 		} else if (key == 's') {
 			// Save
-			map<string, Eigen::Matrix4f> transforms;
+			map<string, Eigen::Matrix4d> transforms;
 			//transforms[sources[targsrc]->getURI()] = Eigen::Matrix4f().setIdentity();
 			//transforms[sources[cursrc]->getURI()] = c.transform();
 
 			for (auto x : corrs) {
 				if (x.second == nullptr) {
-					transforms[x.first] = Eigen::Matrix4f().setIdentity();
+					transforms[x.first] = Eigen::Matrix4d().setIdentity();
 				} else {
 					transforms[x.first] = x.second->transform();
 				}
@@ -325,19 +322,18 @@ void ftl::registration::manual(ftl::Configurable *root) {
 		} else if (key == 'g') {
 			if (!insearch) {
 				insearch = true;
-				LOG(INFO) << "Features matched = " << feat.size();
 
-				ftl::pool.push([&lastScore,&insearch,current,&feat,&pclfeat](int id) {
+				ftl::pool.push([&lastScore,&insearch,current](int id) {
 					LOG(INFO) << "START";
 					lastScore = 1000.0;
 					do {
-						Eigen::Matrix4f tr;
-						float s = current->findBest(tr, feat, pclfeat, 20, 100);
-						//LOG(INFO) << "SCORE = " << s;
+						Eigen::Matrix4d tr;
+						float s = current->findBestSubset(tr, (int)(current->screenPoints().size() * 0.4f), 100);
+						LOG(INFO) << "SCORE = " << s;
 						if (s < lastScore) {
 							lastScore = s;
 							current->setTransform(tr);
-							current->source()->setPose(current->transform());
+							//current->source()->setPose(current->transform());
 						}
 					} while (ftl::running && insearch && lastScore > 0.000001);
 					insearch = false;
@@ -347,55 +343,24 @@ void ftl::registration::manual(ftl::Configurable *root) {
 					auto [sx,sy,tx,ty] = feat[i];
 					current->add(tx,ty,sx,sy);
 				}
-				lastScore = current->estimateTransform();*/
+				lastsScore = current->estimateTransform();*/
 			} else {
 				insearch = false;
 			}
 		} else if (key == 'f') {
-			Mat f1, d1, f2, d2;
-
-			size_t buffer_size = 10;
-			size_t buffer_i = 0;
-			vector<vector<Mat>> buffer(2, vector<Mat>(buffer_size));
-
-			for (size_t i = 0; i < buffer_size; ++i) {
-				current->source()->grab();
-				current->target()->grab();
-				current->source()->getFrames(f1, d1);
-				current->target()->getFrames(f2, d2);
-				buffer[0][i] = d1;
-				buffer[1][i] = d2;
-
-				std::this_thread::sleep_for(std::chrono::milliseconds(20));
-			}
-			averageDepth(buffer[0], d1);
-			averageDepth(buffer[1], d2);
-			Mat d1_v, d2_v;
-			d1.convertTo(d1_v, CV_8U, 255.0f / 10.0f);
-			d2.convertTo(d2_v, CV_8U, 255.0f / 10.0f);
-			applyColorMap(d1_v, d1_v, cv::COLORMAP_JET);
-			applyColorMap(d2_v, d2_v, cv::COLORMAP_JET);
-
-			cv::imshow("smooth d1", d1_v);
-			cv::imshow("smooth d2", d2_v);
-
-			Ptr<xphoto::WhiteBalancer> wb;
-			wb = xphoto::createSimpleWB();
-			wb->balanceWhite(f1, f1);
-			wb->balanceWhite(f2, f2);
-
-			vector<tuple<int,int,int,int>> tfeat;
-			if (!insearch && ftl::registration::featuresSIFT(f1, f2, tfeat, 10)) {
-				vector<tuple<int,int,int,int>> tfeat2;
-				for (int j=0; j<tfeat.size(); j++) {
-					auto [sx,sy,tx,ty] = tfeat[j];
-					tfeat2.push_back(std::make_tuple(tx,ty,sx,sy));
+			if (!insearch) {
+				Mat f1,f2;
+				current->capture(f1,f2);
+
+				vector<tuple<int,int,int,int>> feat;
+				if (ftl::registration::featuresSIFT(f1, f2, feat, 2)) {
+					for (int j=0; j<feat.size(); j++) {
+						auto [sx,sy,tx,ty] = feat[j];
+						current->add(tx, ty, sx, sy);
+					}
 				}
-				current->setPoints(tfeat2);
-				feat.insert(feat.end(), tfeat.begin(), tfeat.end());
 
-				//vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> tpclfeat;
-				current->convertToPCL(tfeat,pclfeat);
+				LOG(INFO) << "Found " << current->screenPoints().size() << " features";
 			} else {
 				LOG(ERROR) << "Can't add features whilst searching...";
 			}
diff --git a/components/renderers/cpp/include/ftl/rgbd_display.hpp b/components/renderers/cpp/include/ftl/rgbd_display.hpp
index 2d0c26a36..9c1be76fb 100644
--- a/components/renderers/cpp/include/ftl/rgbd_display.hpp
+++ b/components/renderers/cpp/include/ftl/rgbd_display.hpp
@@ -28,10 +28,10 @@ class Display : public ftl::Configurable {
 	Source *source_;
 	std::string name_;
 	std::vector<std::function<void(int)>> key_handlers_;
-	Eigen::Vector3f eye_;
-	Eigen::Vector3f centre_;
-	Eigen::Vector3f up_;
-	Eigen::Vector3f lookPoint_;
+	Eigen::Vector3d eye_;
+	Eigen::Vector3d centre_;
+	Eigen::Vector3d up_;
+	Eigen::Vector3d lookPoint_;
 	float lerpSpeed_;
 	bool active_;
 	MouseAction mouseaction_;
diff --git a/components/renderers/cpp/src/rgbd_display.cpp b/components/renderers/cpp/src/rgbd_display.cpp
index 0623bf38c..a0d79f8ae 100644
--- a/components/renderers/cpp/src/rgbd_display.cpp
+++ b/components/renderers/cpp/src/rgbd_display.cpp
@@ -64,20 +64,20 @@ void Display::init() {
 	source_ = nullptr;
 	cv::namedWindow(name_, cv::WINDOW_KEEPRATIO);
 
-	eye_ = Eigen::Vector3f(0.0f, 0.0f, 0.0f);
-	centre_ = Eigen::Vector3f(0.0f, 0.0f, -4.0f);
-	up_ = Eigen::Vector3f(0,1.0f,0);
-	lookPoint_ = Eigen::Vector3f(0.0f,0.0f,-4.0f);
+	eye_ = Eigen::Vector3d(0.0, 0.0, 0.0);
+	centre_ = Eigen::Vector3d(0.0, 0.0, -4.0);
+	up_ = Eigen::Vector3d(0,1.0,0);
+	lookPoint_ = Eigen::Vector3d(0.0,0.0,-4.0);
 	lerpSpeed_ = 0.4f;
 
 	// Keyboard camera controls
 	onKey([this](int key) {
 		//LOG(INFO) << "Key = " << key;
 		if (key == 81 || key == 83) {
-			Eigen::Quaternion<float> q;  q = Eigen::AngleAxis<float>((key == 81) ? 0.01f : -0.01f, up_);
+			Eigen::Quaternion<double> q;  q = Eigen::AngleAxis<double>((key == 81) ? 0.01 : -0.01, up_);
 			eye_ = (q * (eye_ - centre_)) + centre_;
 		} else if (key == 84 || key == 82) {
-			float scalar = (key == 84) ? 0.99f : 1.01f;
+			double scalar = (key == 84) ? 0.99 : 1.01;
 			eye_ = ((eye_ - centre_) * scalar) + centre_;
 		}
 	});
@@ -86,10 +86,10 @@ void Display::init() {
 	mouseaction_ = [this]( int event, int ux, int uy, int) {
 		//LOG(INFO) << "Mouse " << ux << "," << uy;
 		if (event == 1 && source_) {   // click
-			Eigen::Vector4f camPos = source_->point(ux,uy);
+			Eigen::Vector4d camPos = source_->point(ux,uy);
 			camPos *= -1.0f;
-			Eigen::Vector4f worldPos =  source_->getPose() * camPos;
-			lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
+			Eigen::Vector4d worldPos =  source_->getPose() * camPos;
+			lookPoint_ = Eigen::Vector3d(worldPos[0],worldPos[1],worldPos[2]);
 			LOG(INFO) << "Depth at click = " << -camPos[2];
 		}
 	};
@@ -118,7 +118,7 @@ void Display::update() {
 	if (!source_) return;
 
 	centre_ += (lookPoint_ - centre_) * (lerpSpeed_ * 0.1f);
-	Eigen::Matrix4f viewPose = lookAt<float>(eye_,centre_,up_).inverse();
+	Eigen::Matrix4d viewPose = lookAt<double>(eye_,centre_,up_).inverse();
 	source_->setPose(viewPose);
 
 	Mat rgb, depth;
diff --git a/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp b/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
index 65d4b5d3f..cee6e6179 100644
--- a/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/detail/source.hpp
@@ -22,7 +22,7 @@ class Source {
 
 	virtual bool grab()=0;
 	virtual bool isReady() { return false; };
-	virtual void setPose(const Eigen::Matrix4f &pose) { };
+	virtual void setPose(const Eigen::Matrix4d &pose) { };
 
 	protected:
 	ftl::rgbd::Source *host_;
diff --git a/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp b/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp
index 7e055b7cc..9643b1312 100644
--- a/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/snapshot.hpp
@@ -24,9 +24,9 @@ public:
 	explicit SnapshotWriter(const std::string &filename);
 	~SnapshotWriter();
 	
-	bool addCameraRGBD(const std::string &name, const cv::Mat &rgb, const cv::Mat &depth, const Eigen::Matrix4f &pose, const ftl::rgbd::Camera &params);
+	bool addCameraRGBD(const std::string &name, const cv::Mat &rgb, const cv::Mat &depth, const Eigen::Matrix4d &pose, const ftl::rgbd::Camera &params);
 	bool addMat(const std::string &name, const cv::Mat &mat, const std::string &format="tiff");
-	bool addEigenMatrix4f(const std::string &name, const Eigen::Matrix4f &m, const std::string &format="pfm");
+	bool addEigenMatrix4d(const std::string &name, const Eigen::Matrix4d &m, const std::string &format="pfm");
 	bool addFile(const std::string &name, const std::vector<uchar> &buf);
 	bool addFile(const std::string &name, const uchar *buf, const size_t len);
 
@@ -38,7 +38,7 @@ private:
 struct SnapshotEntry {
 	cv::Mat rgb;
 	cv::Mat depth;
-	Eigen::Matrix4f pose;
+	Eigen::Matrix4d pose;
 	ftl::rgbd::Camera params;
 	uint status;
 	SnapshotEntry() : status(1+2+4+8) {};
@@ -49,7 +49,7 @@ public:
 	explicit SnapshotReader(const std::string &filename);
 	~SnapshotReader();
 	
-	bool getCameraRGBD(const std::string &id, cv::Mat &rgb, cv::Mat &depth, Eigen::Matrix4f &pose, ftl::rgbd::Camera &params);
+	bool getCameraRGBD(const std::string &id, cv::Mat &rgb, cv::Mat &depth, Eigen::Matrix4d &pose, ftl::rgbd::Camera &params);
 	std::vector<std::string> getIds();
 
 private:
diff --git a/components/rgbd-sources/include/ftl/rgbd/source.hpp b/components/rgbd-sources/include/ftl/rgbd/source.hpp
index 885046c3a..a810fb6f9 100644
--- a/components/rgbd-sources/include/ftl/rgbd/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/source.hpp
@@ -116,12 +116,12 @@ class Source : public ftl::Configurable {
 	 * cameras this will move the camera, for physical cameras it is set by the
 	 * registration process as it attempts to work out a cameras relative pose.
 	 */
-	virtual void setPose(const Eigen::Matrix4f &pose);
+	virtual void setPose(const Eigen::Matrix4d &pose);
 
 	/**
 	 * Get the camera position as a pose matrix.
 	 */
-	const Eigen::Matrix4f &getPose() const;
+	const Eigen::Matrix4d &getPose() const;
 
 	/**
 	 * Check what features this source has available.
@@ -131,7 +131,15 @@ class Source : public ftl::Configurable {
 	/**
 	 * Get a point in camera coordinates at specified pixel location.
 	 */
-	Eigen::Vector4f point(uint x, uint y);
+	Eigen::Vector4d point(uint x, uint y);
+
+	/**
+	 * Get a point in camera coordinates at specified pixel location, with a
+	 * given depth value.
+	 */
+	Eigen::Vector4d point(uint x, uint y, double d);
+
+	Eigen::Vector2i point(const Eigen::Vector4d &p);
 
 	/**
 	 * Force the internal implementation to be reconstructed.
@@ -151,7 +159,7 @@ class Source : public ftl::Configurable {
 	cv::Mat rgb_;
 	cv::Mat depth_;
 	Camera params_;		// TODO Find better solution
-	Eigen::Matrix4f pose_;
+	Eigen::Matrix4d pose_;
 	ftl::net::Universe *net_;
 	std::shared_mutex mutex_;
 
diff --git a/components/rgbd-sources/src/local.cpp b/components/rgbd-sources/src/local.cpp
index a0d6334b3..02bf0d36e 100644
--- a/components/rgbd-sources/src/local.cpp
+++ b/components/rgbd-sources/src/local.cpp
@@ -270,10 +270,11 @@ bool LocalSource::get(cv::Mat &l, cv::Mat &r) {
 				0, 0, cv::INTER_LINEAR);
 	}
 
-	cv::Ptr<cv::xphoto::WhiteBalancer> wb;
+	// Note: this seems to be too slow on CPU...
+	/*cv::Ptr<cv::xphoto::WhiteBalancer> wb;
 	wb = cv::xphoto::createSimpleWB();
 	wb->balanceWhite(l, l);
-	wb->balanceWhite(r, r);
+	wb->balanceWhite(r, r);*/
 
 	if (flip_v_) {
 		Mat tl, tr;
diff --git a/components/rgbd-sources/src/net.cpp b/components/rgbd-sources/src/net.cpp
index bd4d5127f..674bc2471 100644
--- a/components/rgbd-sources/src/net.cpp
+++ b/components/rgbd-sources/src/net.cpp
@@ -115,7 +115,7 @@ void NetSource::_recvChunk(int frame, int chunk, bool delta, const vector<unsign
 	//}
 }
 
-void NetSource::setPose(const Eigen::Matrix4f &pose) {
+void NetSource::setPose(const Eigen::Matrix4d &pose) {
 	if (!active_) return;
 
 	vector<unsigned char> vec((unsigned char*)pose.data(), (unsigned char*)(pose.data()+(pose.size())));
diff --git a/components/rgbd-sources/src/net.hpp b/components/rgbd-sources/src/net.hpp
index 3a986d064..ce88d10c3 100644
--- a/components/rgbd-sources/src/net.hpp
+++ b/components/rgbd-sources/src/net.hpp
@@ -25,7 +25,7 @@ class NetSource : public detail::Source {
 	bool grab();
 	bool isReady();
 
-	void setPose(const Eigen::Matrix4f &pose);
+	void setPose(const Eigen::Matrix4d &pose);
 
 	void reset();
 
diff --git a/components/rgbd-sources/src/snapshot.cpp b/components/rgbd-sources/src/snapshot.cpp
index 51b8a635f..5599364c6 100644
--- a/components/rgbd-sources/src/snapshot.cpp
+++ b/components/rgbd-sources/src/snapshot.cpp
@@ -5,7 +5,7 @@
 using namespace ftl::rgbd;
 
 using cv::Mat;
-using Eigen::Matrix4f;
+using Eigen::Matrix4d;
 
 using cv::imencode;
 using cv::imdecode;
@@ -13,8 +13,6 @@ using cv::imdecode;
 using std::string;
 using std::vector;
 
-using Eigen::Matrix4f;
-
 // TODO: move to camera_params
 using ftl::rgbd::Camera;
 
@@ -120,18 +118,18 @@ bool SnapshotWriter::addMat(const string &name, const Mat &mat, const std::strin
 	return retval;
 }
 
-bool SnapshotWriter::addEigenMatrix4f(const string &name, const Matrix4f &m, const string &format) {
+bool SnapshotWriter::addEigenMatrix4d(const string &name, const Matrix4d &m, const string &format) {
 	Mat tmp;
 	cv::eigen2cv(m, tmp);
 	return addMat(name, tmp, format);
 }
 
 bool SnapshotWriter::addCameraRGBD(const string &name, const Mat &rgb, const Mat &depth,
-							 const Matrix4f &pose, const Camera &params) {
+							 const Matrix4d &pose, const Camera &params) {
 	bool retval = true;
 	retval &= addMat(name + "-RGB", rgb);
 	retval &= addMat(name + "-D", depth);
-	retval &= addEigenMatrix4f(name + "-POSE", pose);
+	retval &= addEigenMatrix4d(name + "-POSE", pose);
 
 	nlohmann::json j;
 	to_json(j, params);
@@ -223,7 +221,7 @@ bool SnapshotReader::readArchive() {
 			if (!readEntry(data)) continue;
 			Mat m_ = cv::imdecode(Mat(data), 0);
 			if ((m_.rows != 4) || (m_.cols != 4)) continue;
-			cv::Matx44f pose_(m_);
+			cv::Matx44d pose_(m_);
 			cv::cv2eigen(pose_, snapshot.pose);
 			snapshot.status &= ~(1 << 2);
 		}
@@ -256,7 +254,7 @@ vector<string> SnapshotReader::getIds() {
 }
 
 bool SnapshotReader::getCameraRGBD(const string &id, Mat &rgb, Mat &depth,
-							 Matrix4f &pose, Camera &params) {
+							 Matrix4d &pose, Camera &params) {
 	if (data_.find(id) == data_.end()) {
 		LOG(ERROR) << "entry not found: " << id;
 		return false;
diff --git a/components/rgbd-sources/src/snapshot_source.cpp b/components/rgbd-sources/src/snapshot_source.cpp
index 96c16e4d0..367fc2861 100644
--- a/components/rgbd-sources/src/snapshot_source.cpp
+++ b/components/rgbd-sources/src/snapshot_source.cpp
@@ -10,7 +10,7 @@ using ftl::rgbd::detail::SnapshotSource;
 using std::string;
 
 SnapshotSource::SnapshotSource(ftl::rgbd::Source *host, SnapshotReader &reader, const string &id) : detail::Source(host) {
-    Eigen::Matrix4f pose;
+    Eigen::Matrix4d pose;
     reader.getCameraRGBD(id, rgb_, depth_, pose, params_);
     setPose(pose);
 }
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 787dbdada..b3b9e82b2 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -27,7 +27,7 @@ using ftl::rgbd::detail::NetSource;
 using ftl::rgbd::detail::ImageSource;
 using ftl::rgbd::capability_t;
 
-Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), pose_(Eigen::Matrix4f::Identity()), net_(nullptr) {
+Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), pose_(Eigen::Matrix4d::Identity()), net_(nullptr) {
 	impl_ = nullptr;
 	params_ = {0};
 	reset();
@@ -38,7 +38,7 @@ Source::Source(ftl::config::json_t &cfg) : Configurable(cfg), pose_(Eigen::Matri
 	});
 }
 
-Source::Source(ftl::config::json_t &cfg, ftl::net::Universe *net) : Configurable(cfg), pose_(Eigen::Matrix4f::Identity()), net_(net) {
+Source::Source(ftl::config::json_t &cfg, ftl::net::Universe *net) : Configurable(cfg), pose_(Eigen::Matrix4d::Identity()), net_(net) {
 	impl_ = nullptr;
 	params_ = {0};
 	reset();
@@ -147,22 +147,38 @@ void Source::getFrames(cv::Mat &rgb, cv::Mat &depth) {
 	depth = depth_;
 }
 
-Eigen::Vector4f Source::point(uint ux, uint uy) {
+Eigen::Vector4d Source::point(uint ux, uint uy) {
 	const auto &params = parameters();
-	const float x = ((float)ux+(float)params.cx) / (float)params.fx;
-	const float y = ((float)uy+(float)params.cy) / (float)params.fy;
+	const double x = ((double)ux+params.cx) / params.fx;
+	const double y = ((double)uy+params.cy) / params.fy;
 
 	SHARED_LOCK(mutex_,lk);
-	const float depth = depth_.at<float>(uy,ux);
-	return Eigen::Vector4f(x*depth,y*depth,depth,1.0);
+	const double depth = depth_.at<float>(uy,ux);
+	return Eigen::Vector4d(x*depth,y*depth,depth,1.0);
 }
 
-void Source::setPose(const Eigen::Matrix4f &pose) {
+Eigen::Vector4d Source::point(uint ux, uint uy, double d) {
+	const auto &params = parameters();
+	const double x = ((double)ux+params.cx) / params.fx;
+	const double y = ((double)uy+params.cy) / params.fy;
+	return Eigen::Vector4d(x*d,y*d,d,1.0);
+}
+
+Eigen::Vector2i Source::point(const Eigen::Vector4d &p) {
+	const auto &params = parameters();
+	double x = p[0] / p[2];
+	double y = p[1] / p[2];
+	x *= params.fx;
+	y *= params.fy;
+	return Eigen::Vector2i((int)(x - params.cx), (int)(y - params.cy));
+}
+
+void Source::setPose(const Eigen::Matrix4d &pose) {
 	pose_ = pose;
 	if (impl_) impl_->setPose(pose);
 }
 
-const Eigen::Matrix4f &Source::getPose() const {
+const Eigen::Matrix4d &Source::getPose() const {
 	return pose_;
 }
 
diff --git a/components/rgbd-sources/src/streamer.cpp b/components/rgbd-sources/src/streamer.cpp
index 34ee61f0a..45ac6d496 100644
--- a/components/rgbd-sources/src/streamer.cpp
+++ b/components/rgbd-sources/src/streamer.cpp
@@ -51,7 +51,7 @@ Streamer::Streamer(nlohmann::json &config, Universe *net)
 		SHARED_LOCK(mutex_,slk);
 
 		if (sources_.find(uri) != sources_.end()) {
-			Eigen::Matrix4f pose;
+			Eigen::Matrix4d pose;
 			memcpy(pose.data(), buf.data(), buf.size());
 			sources_[uri]->src->setPose(pose);
 		}
-- 
GitLab