diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
index 134b9088a06ac505d72d77ab6bbc7aafdf90ed44..f510e213d516a5ed18cba761251d05091ff051a0 100644
--- a/applications/registration/src/correspondances.cpp
+++ b/applications/registration/src/correspondances.cpp
@@ -30,61 +30,192 @@ 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) {
+		float sum = 0;
+		int good_values = 0;
+
+		// Calculate mean
+		for (int i_in = 0; i_in < in.size(); ++i_in) {
+			float d = in[i_in].at<float>(i);
+			if (d < 40.0f) {
+				good_values++;
+				sum += d;
+			}
+		}
+
+		if (good_values > 0) {
+			sum /= (float)good_values;
+
+			// Calculate variance
+			float var = 0.0f;
+			for (int i_in = 0; i_in < in.size(); ++i_in) {
+				float d = in[i_in].at<float>(i);
+				if (d < 40.0f) {
+					float delta = d - sum;
+					var += delta*delta;
+				}
+			}
+			if (good_values > 1) var /= (good_values-1);
+			else var = 0.0f;
+
+			if (var < varThresh) {
+				out.at<float>(i) = 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::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::clear() {
+	targ_cloud_.clear();
+	src_cloud_.clear();
+	src_index_ = cv::Mat(rgb1.size(), CV_32SC1, cv::Scalar(-1));
+	targ_index_ = cv::Mat(rgb2.size(), CV_32SC1, cv::Scalar(-1));
+	src_feat_.clear();
+	targ_feat_.clear();
+	log_.clear();
+}
+
+void Correspondance::clearCorrespondances() {
+	src_feat_.clear();
+	targ_feat_.clear();
+	log_.clear();
+	uptodate_ = false;
+}
+
+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));
+
+	for (size_t i = 0; i < buffer_size; ++i) {
+		current->source()->grab();
+		current->target()->grab();
+		current->source()->getFrames(rgb1, d1);
+		current->target()->getFrames(rgb2, d2);
+		buffer[0][i] = d1;
+		buffer[1][i] = d2;
+
+		std::this_thread::sleep_for(std::chrono::milliseconds(50));
+	}
+	averageDepth(buffer[0], d1, 1.0f);
+	averageDepth(buffer[1], d2, 1.0f);
+	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(rgb1, rgb1);
+	wb->balanceWhite(rgb2, rgb2);
+
+	// Build point clouds
+	clear();
+	int six=0;
+	int tix=0;
 
-	PointXYZ pcl_p2;
-	pcl_p2.x = p2[0];
-	pcl_p2.y = p2[1];
-	pcl_p2.z = p2[2];
+	for (int y=0; y<rgb1.rows; y++) {
+		//unsigned char *rgb1ptr = rgb1.ptr(y);
+		//unsigned char *rgb2ptr = rgb2.ptr(y);
+		float *d1ptr = d1.ptr(y);
+		float *d2ptr = d2.ptr(y);
 
-	if (pcl_p2.z >= 40.0f || pcl_p1.z >= 40.0f) {
-		LOG(WARNING) << "Bad point choosen";
+		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 = src_->point(x,y,d1_value);
+				src_cloud_.push_back(p1);
+				src_index_.at<int>(y,x) = six++;
+			}
+
+			if (d2_value < 39.0f) {
+				// Make PCL points with specific depth value
+				pcl::PointXYZ p2 = src_->point(x,y,d2_value);
+				targ_cloud_.push_back(p2);
+				targ_index_.at<int>(y,x) = tix++;
+			}
+		}
+	}
+}
+
+bool Correspondances::add(int tx, int ty, int sx, int sy) {
+	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;
 	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::Matrix4f &T) {
+	//Eigen::Matrix4f transform = transform_ * T;
 	uptodate_ = true;
-	int n_points = targ_cloud_->size();
+	//int n_points = targ_cloud_->size();
 
-	vector<int> idx(n_points);
-	for (int i = 0; i < n_points; i++) { idx[i] = i; }
+	//vector<int> idx(n_points);
+	//for (int i = 0; i < n_points; i++) { idx[i] = i; }
 
 	pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
 	pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
 
-	svd.estimateRigidTransformation(*src_cloud_, idx, *targ_cloud_, idx, transform_);
+	svd.estimateRigidTransformation(*src_cloud_, src_feat_, *targ_cloud_, targ_feat_, T);
+	return validate.validateTransformation(src_cloud_, targ_cloud_, T);
+}
 
-	return validate.validateTransformation(src_cloud_, targ_cloud_, transform_);
+double Correspondances::estimateTransform(Eigen::Matrix4f &T, const vector<int> &src_feat, const vector<int> &targ_feat) {
+	uptodate_ = true;
+
+	pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
+	pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
+
+	svd.estimateRigidTransformation(*src_cloud_, src_feat, *targ_cloud_, targ_feat, T);
+	return validate.validateTransformation(src_cloud_, targ_cloud_, T);
 }
 
 static std::default_random_engine generator;
@@ -99,48 +230,38 @@ 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 Correspondances::findBestSubset(Eigen::Matrix4f &tr, int K, int N) {
 	double score = 10000.0f;
 	vector<tuple<int,int,int,int>> best;
 	Eigen::Matrix4f bestT;
 
-	pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ> validate;
-	pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ> svd;
-
 	std::mt19937 rng(std::chrono::steady_clock::now().time_since_epoch().count());
 	std::uniform_int_distribution<int> distribution(0,p.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();
 
 		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);
+			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);
+		float scoreT = estimateTransform(T, sidx, tidx);
 
 		if (scoreT < score) {
 			score = scoreT;
@@ -150,7 +271,7 @@ double Correspondances::findBest(Eigen::Matrix4f &tr, const std::vector<std::tup
 	}
 
 	// TODO Add these best points to actual clouds.
-	log_ = best;
+	//log_ = best;
 	tr = bestT;
 	//uptodate_ = true;
 	return score;
diff --git a/applications/registration/src/correspondances.hpp b/applications/registration/src/correspondances.hpp
index bd08dd6dd9a6fbea3678a8667f35a92391714b41..6a58dfb18783a450f2c70acd869906afe7174cc6 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.
@@ -57,7 +62,7 @@ class Correspondances {
 	 */
 	double estimateTransform();
 
-	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 findBest(Eigen::Matrix4f &tr, const std::vector<std::tuple<int,int,int,int>> &p, int K, int N);
 
 	void convertToPCL(const std::vector<std::tuple<int,int,int,int>> &p, std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &p2);
 
@@ -78,6 +83,7 @@ class Correspondances {
 	Eigen::Matrix4f transform_;
 	bool uptodate_;
 	std::vector<std::tuple<int,int,int,int>> log_;
+	cv::Mat &index_;
 };
 
 }
diff --git a/applications/registration/src/manual.cpp b/applications/registration/src/manual.cpp
index 6c4783eb06c4ec9ba251eff53bc2edaf32ea31e2..98926415e3f4ed86d02bdc177ee4294104096161 100644
--- a/applications/registration/src/manual.cpp
+++ b/applications/registration/src/manual.cpp
@@ -128,52 +128,6 @@ static void build_correspondances(const vector<Source*> &sources, map<string, Co
 	}
 }
 
-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) {
-		float sum = 0;
-		int good_values = 0;
-
-		// Calculate mean
-		for (int i_in = 0; i_in < in.size(); ++i_in) {
-			float d = in[i_in].at<float>(i);
-			if (d < 40.0f) {
-				good_values++;
-				sum += d;
-			}
-		}
-
-		if (good_values > 0) {
-			sum /= (float)good_values;
-
-			// Calculate variance
-			float var = 0.0f;
-			for (int i_in = 0; i_in < in.size(); ++i_in) {
-				float d = in[i_in].at<float>(i);
-				if (d < 40.0f) {
-					float delta = d - sum;
-					var += delta*delta;
-				}
-			}
-			if (good_values > 1) var /= (good_values-1);
-			else var = 0.0f;
-
-			if (var < varThresh) {
-				out.at<float>(i) = sum;
-			} else {
-				out.at<float>(i) = 41.0f;
-			}
-		} else {
-			out.at<float>(i) = 41.0f;
-		}
-	}
-}
-
 void ftl::registration::manual(ftl::Configurable *root) {
 	using namespace cv;
 	
@@ -372,37 +326,8 @@ void ftl::registration::manual(ftl::Configurable *root) {
 				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, 1.0f);
-			averageDepth(buffer[1], d2, 1.0f);
-			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);
+			Mat f1,f2;
+			current->capture(f1,f2);
 
 			vector<tuple<int,int,int,int>> tfeat;
 			if (!insearch && ftl::registration::featuresSIFT(f1, f2, tfeat, 10)) {
diff --git a/components/rgbd-sources/include/ftl/rgbd/source.hpp b/components/rgbd-sources/include/ftl/rgbd/source.hpp
index 885046c3a449f9e441ed0cb71e53c1cd0ae4fb85..595fe7f9899d6850e09ef6f2e208bedefd5a9afb 100644
--- a/components/rgbd-sources/include/ftl/rgbd/source.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/source.hpp
@@ -133,6 +133,12 @@ class Source : public ftl::Configurable {
 	 */
 	Eigen::Vector4f point(uint x, uint y);
 
+	/**
+	 * Get a point in camera coordinates at specified pixel location, with a
+	 * given depth value.
+	 */
+	Eigen::Vector4f point(uint x, uint y, float d);
+
 	/**
 	 * Force the internal implementation to be reconstructed.
 	 */
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index 787dbdadaa51ef8efb1d9f3d1938c4bc302d2c81..f2049b791a3c4a2e9a3c8160602806ff22776eb3 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -157,6 +157,13 @@ Eigen::Vector4f Source::point(uint ux, uint uy) {
 	return Eigen::Vector4f(x*depth,y*depth,depth,1.0);
 }
 
+Eigen::Vector4f Source::point(uint ux, uint uy, float d) {
+	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;
+	return Eigen::Vector4f(x*d,y*d,d,1.0);
+}
+
 void Source::setPose(const Eigen::Matrix4f &pose) {
 	pose_ = pose;
 	if (impl_) impl_->setPose(pose);