From e937706a4a4c58521a5f30d6e03ffcfc63938862 Mon Sep 17 00:00:00 2001
From: Sebastian Hahta <joseha@utu.fi>
Date: Wed, 15 Jan 2020 16:44:18 +0200
Subject: [PATCH] Remove PCL and some other unused code.

---
 .gitlab-ci.yml                                |   2 +-
 CMakeLists.txt                                |  13 -
 README.md                                     |   3 +-
 applications/registration/CMakeLists.txt      |  15 -
 applications/registration/src/aruco.cpp       | 213 -------
 applications/registration/src/aruco.hpp       |  14 -
 applications/registration/src/common.cpp      |  49 --
 applications/registration/src/common.hpp      |  21 -
 .../registration/src/correspondances.cpp      | 545 ------------------
 .../registration/src/correspondances.hpp      | 110 ----
 applications/registration/src/main.cpp        |  21 -
 applications/registration/src/manual.cpp      | 269 ---------
 applications/registration/src/manual.hpp      |  14 -
 applications/registration/src/sfm.cpp         | 164 ------
 applications/registration/src/sfm.hpp         |  44 --
 components/common/cpp/CMakeLists.txt          |   4 +-
 components/common/cpp/include/ftl/config.h.in |   1 -
 .../cpp/include/ftl/utility/opencv_to_pcl.hpp |  22 -
 components/common/cpp/src/opencv_to_pcl.cpp   |  36 --
 components/operators/CMakeLists.txt           |   4 -
 components/renderers/cpp/CMakeLists.txt       |   4 -
 reconstruct/src/voxel_hash.cu                 | 127 ----
 22 files changed, 3 insertions(+), 1692 deletions(-)
 delete mode 100644 applications/registration/CMakeLists.txt
 delete mode 100644 applications/registration/src/aruco.cpp
 delete mode 100644 applications/registration/src/aruco.hpp
 delete mode 100644 applications/registration/src/common.cpp
 delete mode 100644 applications/registration/src/common.hpp
 delete mode 100644 applications/registration/src/correspondances.cpp
 delete mode 100644 applications/registration/src/correspondances.hpp
 delete mode 100644 applications/registration/src/main.cpp
 delete mode 100644 applications/registration/src/manual.cpp
 delete mode 100644 applications/registration/src/manual.hpp
 delete mode 100644 applications/registration/src/sfm.cpp
 delete mode 100644 applications/registration/src/sfm.hpp
 delete mode 100644 components/common/cpp/include/ftl/utility/opencv_to_pcl.hpp
 delete mode 100644 components/common/cpp/src/opencv_to_pcl.cpp
 delete mode 100644 reconstruct/src/voxel_hash.cu

diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index dabc327c0..35916ad80 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -20,7 +20,7 @@ linux:
 #  before_script:
 #    - export DEBIAN_FRONTEND=noninteractive
 #    - apt-get update -qq && apt-get install -y -qq g++ cmake git
-#    - apt-get install -y -qq libopencv-dev libgoogle-glog-dev liburiparser-dev libreadline-dev libmsgpack-dev uuid-dev libpcl-dev
+#    - apt-get install -y -qq libopencv-dev libgoogle-glog-dev liburiparser-dev libreadline-dev libmsgpack-dev uuid-dev
   script:
     - mkdir build
     - cd build
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6640f5d7e..584d3256e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,7 +10,6 @@ include(GNUInstallDirs)
 include(CTest)
 enable_testing()
 
-option(WITH_PCL "Use PCL if available" OFF)
 option(WITH_NVPIPE "Use NvPipe for compression if available" ON)
 option(WITH_OPTFLOW "Use NVIDIA Optical Flow if available" OFF)
 option(WITH_OPENVR "Build with OpenVR support" OFF)
@@ -68,17 +67,9 @@ if(${CMAKE_VERSION} VERSION_GREATER "3.12.0")
 	cmake_policy(SET CMP0074 NEW)
 endif()
 
-if (WITH_PCL)
-	find_package( PCL QUIET COMPONENTS io common visualization registration segmentation)
-endif()
-
 set(CMAKE_CXX_STANDARD 17) # For PCL/VTK https://github.com/PointCloudLibrary/pcl/issues/2686
 set(HAVE_OPENCV TRUE)
 
-if (PCL_FOUND)
-	set(HAVE_PCL TRUE)
-endif()
-
 # Readline library is not required on Windows
 # May also entirely remove dependence on this... it should be optional at least.
 if (NOT WIN32)
@@ -250,10 +241,6 @@ if (BUILD_CALIBRATION)
 	add_subdirectory(applications/calibration-multi)
 endif()
 
-if (HAVE_PCL)
-	#add_subdirectory(applications/registration)
-endif()
-
 if (BUILD_RECONSTRUCT)
 	add_subdirectory(applications/reconstruct)
 endif()
diff --git a/README.md b/README.md
index 2269fe68c..f439aa909 100644
--- a/README.md
+++ b/README.md
@@ -47,5 +47,4 @@ to configure the project.
 You will need to have OpenCV and glog installed. CUDA and LibSGM are optional
 but recommended also. OpenCV should have cuda stereo modules compiled, along
 with the viz module if local point cloud display is required. These are contrib
-modules. PCL is also required to build the reconstruction components and app.
-
+modules.
diff --git a/applications/registration/CMakeLists.txt b/applications/registration/CMakeLists.txt
deleted file mode 100644
index 8b27dcb38..000000000
--- a/applications/registration/CMakeLists.txt
+++ /dev/null
@@ -1,15 +0,0 @@
-set(REGSRC
-	src/main.cpp
-	src/manual.cpp
-	src/correspondances.cpp
-	src/sfm.cpp
-	src/aruco.cpp
-	src/common.cpp
-)
-
-add_executable(ftl-register ${REGSRC})
-
-target_include_directories(ftl-register PRIVATE src)
-
-target_link_libraries(ftl-register ftlcommon ftlnet ftlrgbd Threads::Threads ${OpenCV_LIBS})
-
diff --git a/applications/registration/src/aruco.cpp b/applications/registration/src/aruco.cpp
deleted file mode 100644
index 99d3552d9..000000000
--- a/applications/registration/src/aruco.cpp
+++ /dev/null
@@ -1,213 +0,0 @@
-#include "aruco.hpp"
-#include "common.hpp"
-#include "correspondances.hpp"
-
-#include <ftl/net/universe.hpp>
-#include <ftl/rgbd/source.hpp>
-
-#include <opencv2/aruco.hpp>
-
-using ftl::registration::aruco;
-using ftl::registration::Correspondances;
-using std::map;
-using std::string;
-using std::vector;
-using ftl::Configurable;
-using ftl::net::Universe;
-using ftl::rgbd::Source;
-using std::pair;
-using std::make_pair;
-
-void ftl::registration::aruco(ftl::Configurable *root) {
-	using namespace cv;
-	
-	Universe *net = ftl::create<Universe>(root, "net");
-
-	net->start();
-	net->waitConnections();
-
-	auto sources = ftl::createArray<Source>(root, "sources", net);
- 
-	int curtarget = 0;
-	bool freeze = false;
-	bool depthtoggle = false;
-	double lastScore = 1000.0;
-
-	if (sources.size() == 0) return;
-
-	for (auto *src : sources) src->setChannel(ftl::rgbd::kChanDepth);
-
-	cv::namedWindow("Target", cv::WINDOW_KEEPRATIO);
-	cv::namedWindow("Source", cv::WINDOW_KEEPRATIO);
-
-	map<string, Eigen::Matrix4d> oldTransforms;
-	ftl::registration::loadTransformations(root->value("output", string("./test.json")), oldTransforms);
-
-	//Correspondances c(sources[targsrc], sources[cursrc]);
-	map<string, Correspondances*> corrs;
-	ftl::registration::build_correspondances(sources, corrs, root->value("origin", 0), oldTransforms);
-
-	Correspondances *current = corrs[sources[curtarget]->getURI()];
-
-	map<int, vector<Point2f>> targ_corners;
-	map<int, vector<Point2f>> src_corners;
-	map<int, pair<Vec3d,Vec3d>> targ_trans;
-	map<int, pair<Vec3d,Vec3d>> src_trans;
-
-	while (ftl::running) {
-		if (!freeze) {
-			// Grab the latest frames from sources
-			for (int i=0; i<sources.size(); i++) {
-				if (sources[i]->isReady()) {
-					sources[i]->grab();
-				}
-			}
-		}
-
-		// Get the raw rgb and depth frames from sources
-		Mat ttrgb, trgb, tdepth, ttdepth;
-		Mat tsrgb, srgb, sdepth, tsdepth;
-
-		if (current == nullptr) {
-			srgb = Mat(Size(640,480), CV_8UC3, Scalar(0,0,0));
-			trgb = Mat(Size(640,480), CV_8UC3, Scalar(0,0,0));
-
-			putText(srgb, string("No correspondance for ") + sources[curtarget]->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-
-			if (!trgb.empty()) imshow("Target", trgb);
-			if (!srgb.empty()) imshow("Source", srgb);
-		} else if (current->source()->isReady() && current->target()->isReady()) {
-			current->source()->getFrames(tsrgb, tsdepth);
-			current->target()->getFrames(ttrgb, ttdepth);
-
-			tsrgb.copyTo(srgb);
-			ttrgb.copyTo(trgb);
-			ttdepth.convertTo(tdepth, CV_8U, 255.0f / 10.0f);
-			applyColorMap(tdepth, tdepth, cv::COLORMAP_JET);
-			tsdepth.convertTo(sdepth, CV_8U, 255.0f / 10.0f);
-			applyColorMap(sdepth, sdepth, cv::COLORMAP_JET);
-
-			vector<int> mids_targ, mids_src;
-			vector<vector<Point2f>> mpoints_targ, mpoints_src;
-			Ptr<aruco::DetectorParameters> parameters(new aruco::DetectorParameters);
-			auto dictionary = aruco::getPredefinedDictionary(aruco::DICT_4X4_50);
-
-			aruco::detectMarkers(trgb, dictionary, mpoints_targ, mids_targ, parameters);
-			aruco::drawDetectedMarkers(trgb, mpoints_targ, mids_targ);
-
-			// Cache the marker positions
-			for (size_t i=0; i<mids_targ.size(); i++) {
-				targ_corners[mids_targ[i]] = mpoints_targ[i];
-			}
-
-			vector<Vec3d> rvecs, tvecs;
-			cv::Mat distCoef(cv::Size(14,1), CV_64F, cv::Scalar(0.0));
-			cv::Mat targ_cam = current->target()->cameraMatrix();
-			aruco::estimatePoseSingleMarkers(mpoints_targ, 0.1, targ_cam, distCoef, rvecs, tvecs);
-			for (size_t i=0; i<rvecs.size(); i++) {
-				targ_trans[mids_targ[i]] = make_pair(tvecs[i], rvecs[i]);
-				aruco::drawAxis(trgb, current->target()->cameraMatrix(), distCoef, rvecs[i], tvecs[i], 0.1);
-			}
-
-			aruco::detectMarkers(srgb, dictionary, mpoints_src, mids_src, parameters);
-			aruco::drawDetectedMarkers(srgb, mpoints_src, mids_src);
-
-			rvecs.clear();
-			tvecs.clear();
-			cv::Mat src_cam = current->source()->cameraMatrix();
-			aruco::estimatePoseSingleMarkers(mpoints_src, 0.1, src_cam, distCoef, rvecs, tvecs);
-			for (size_t i=0; i<rvecs.size(); i++) {
-				src_trans[mids_src[i]] = make_pair(tvecs[i], rvecs[i]);
-				aruco::drawAxis(srgb, current->source()->cameraMatrix(), distCoef, rvecs[i], tvecs[i], 0.1);
-			}
-
-			// Cache the marker positions
-			for (size_t i=0; i<mids_src.size(); i++) {
-				src_corners[mids_src[i]] = mpoints_src[i];
-			}
-
-			current->drawTarget(trgb);
-			current->drawTarget(tdepth);
-			current->drawSource(srgb);
-			current->drawSource(sdepth);
-
-			putText(srgb, string("Source: ") + current->source()->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-			putText(trgb, string("Target: ") + current->target()->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-			putText(srgb, string("Score: ") + std::to_string(lastScore), Point(10,40), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-			putText(trgb, string("Score: ") + std::to_string(lastScore), Point(10,40), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-			if (freeze) putText(srgb, string("Paused"), Point(10,50), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-			if (freeze) putText(trgb, string("Paused"), Point(10,50), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-
-			if (!trgb.empty()) imshow("Target", (depthtoggle) ? tdepth : trgb);
-			if (!srgb.empty()) imshow("Source", (depthtoggle) ? sdepth : srgb);
-		}
-
-		int key = cv::waitKey(20);
-		if (key == 27) break;
-		else if (key >= 48 && key <= 57) {
-			curtarget = key-48;
-			if (curtarget >= sources.size()) curtarget = 0;
-			current = corrs[sources[curtarget]->getURI()];
-			//lastScore = 1000.0;
-		} else if (key == 'd') {
-			depthtoggle = !depthtoggle;
-		} else if (key == 'i') {
-			current->icp();
-		} else if (key == 'e') {
-			vector<Vec3d> targfeat;
-			vector<Vec3d> srcfeat;
-
-			for (auto i : targ_trans) {
-				auto si = src_trans.find(i.first);
-				if (si != src_trans.end()) {
-					//Vec3d dt = std::get<0>(si->second) - std::get<0>(i.second);
-					targfeat.push_back(std::get<0>(i.second));
-					srcfeat.push_back(std::get<0>(si->second));
-				}
-			}
-
-			Eigen::Matrix4d t;
-			lastScore = current->estimateTransform(t, srcfeat, targfeat, true);
-			current->setTransform(t);
-			//lastScore = current->icp();
-		} else if (key == 'f') {
-			Mat f1,f2;
-			current->capture(f1,f2);
-
-			LOG(INFO) << "Captured frames";
-
-			// Convert matching marker corners into correspondence points
-			for (auto i : targ_corners) {
-				auto si = src_corners.find(i.first);
-				if (si != src_corners.end()) {
-					for (size_t j=0; j<4; ++j) {
-						current->add(i.second[j].x, i.second[j].y, si->second[j].x, si->second[j].y);
-					}
-				}
-			}
-
-			LOG(INFO) << "Estimating transform...";
-
-			Eigen::Matrix4d t;
-			lastScore = current->estimateTransform(t);
-			current->setTransform(t);
-			lastScore = current->icp();
-		} else if (key == 's') {
-			// Save
-			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::Matrix4d().setIdentity();
-				} else {
-					transforms[x.first] = x.second->transform();
-				}
-			}
-
-			saveTransformations(root->value("output", string("./test.json")), transforms);
-			LOG(INFO) << "Saved!";
-		} else if (key == 32) freeze = !freeze;
-	}
-}
diff --git a/applications/registration/src/aruco.hpp b/applications/registration/src/aruco.hpp
deleted file mode 100644
index 88438eac1..000000000
--- a/applications/registration/src/aruco.hpp
+++ /dev/null
@@ -1,14 +0,0 @@
-#ifndef _FTL_REGISTRATION_ARUCO_HPP_
-#define _FTL_REGISTRATION_ARUCO_HPP_
-
-#include <ftl/configuration.hpp>
-
-namespace ftl {
-namespace registration {
-
-void aruco(ftl::Configurable *root);
-
-}
-}
-
-#endif  // _FTL_REGISTRATION_ARUCO_HPP_
diff --git a/applications/registration/src/common.cpp b/applications/registration/src/common.cpp
deleted file mode 100644
index d225c6367..000000000
--- a/applications/registration/src/common.cpp
+++ /dev/null
@@ -1,49 +0,0 @@
-#include "common.hpp"
-
-using std::string;
-using std::map;
-
-void ftl::registration::from_json(nlohmann::json &json, map<string, Eigen::Matrix4d> &transformations) {
-	for (auto it = json.begin(); it != json.end(); ++it) {
-		Eigen::Matrix4d m;
-		auto data = m.data();
-		for(size_t i = 0; i < 16; i++) { data[i] = it.value()[i]; }
-		transformations[it.key()] = m;
-	}
-}
-
-void ftl::registration::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]); }
-		json[item.first] = val;
-	}
-}
-
-bool ftl::registration::saveTransformations(const string &path, map<string, Eigen::Matrix4d> &data) {
-	nlohmann::json data_json;
-	to_json(data_json, data);
-	std::ofstream file(path);
-
-	if (!file.is_open()) {
-		LOG(ERROR) << "Error writing transformations to file " << path;
-		return false;
-	}
-
-	file << std::setw(4) << data_json;
-	return true;
-}
-
-bool ftl::registration::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;
-		return false;
-	}
-	
-	nlohmann::json json_registration;
-	file >> json_registration;
-	from_json(json_registration, data);
-	return true;
-}
-
diff --git a/applications/registration/src/common.hpp b/applications/registration/src/common.hpp
deleted file mode 100644
index bbb4e3749..000000000
--- a/applications/registration/src/common.hpp
+++ /dev/null
@@ -1,21 +0,0 @@
-#ifndef _FTL_REGISTRATION_COMMON_HPP_
-#define _FTL_REGISTRATION_COMMON_HPP_
-
-#include <ftl/rgbd/source.hpp>
-#include <nlohmann/json.hpp>
-
-#include <string>
-#include <map>
-
-namespace ftl {
-namespace registration {
-
-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);
-void from_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4d> &transformations);
-void to_json(nlohmann::json &json, std::map<std::string, Eigen::Matrix4d> &transformations);
-
-}
-}
-
-#endif  // _FTL_REGISTRATION_COMMON_HPP_
diff --git a/applications/registration/src/correspondances.cpp b/applications/registration/src/correspondances.cpp
deleted file mode 100644
index ce662756a..000000000
--- a/applications/registration/src/correspondances.cpp
+++ /dev/null
@@ -1,545 +0,0 @@
-#include "correspondances.hpp"
-#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;
-using std::map;
-using std::vector;
-using ftl::rgbd::Source;
-using pcl::PointCloud;
-using pcl::PointXYZ;
-using pcl::PointXYZRGB;
-using nlohmann::json;
-using std::tuple;
-using std::make_tuple;
-using cv::Mat;
-
-void ftl::registration::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;
-
-	for (int i=origin-1; i>=0; i--) {
-		if (last == nullptr) {
-			auto *c = new Correspondances(sources[i], sources[origin]);
-			last = c;
-			cs[sources[i]->getURI()] = c;
-			if (old.find(sources[i]->getURI()) != old.end()) {
-				c->setTransform(old[sources[i]->getURI()]);
-			}
-		} else {
-			auto *c = new Correspondances(last, sources[i]);
-			last = c;
-			cs[sources[i]->getURI()] = c;
-			if (old.find(sources[i]->getURI()) != old.end()) {
-				c->setTransform(old[sources[i]->getURI()]);
-			}
-		}
-	}
-
-	last = nullptr;
-
-	for (int i=origin+1; i<sources.size(); i++) {
-		if (last == nullptr) {
-			auto *c = new Correspondances(sources[i], sources[origin]);
-			last = c;
-			cs[sources[i]->getURI()] = c;
-		} else {
-			auto *c = new Correspondances(last, sources[i]);
-			last = c;
-			cs[sources[i]->getURI()] = c;
-		}
-	}
-}
-
-
-Correspondances::Correspondances(Source *src, Source *targ)
-	:	parent_(nullptr), targ_(targ), src_(src),
-		targ_cloud_(new PointCloud<PointXYZ>),
-		src_cloud_(new PointCloud<PointXYZ>), uptodate_(true) {
-	transform_.setIdentity();
-}
-
-Correspondances::Correspondances(Correspondances *parent, Source *src)
-	:	parent_(parent), targ_(parent_->source()), src_(src),
-		targ_cloud_(new PointCloud<PointXYZ>),
-		src_cloud_(new PointCloud<PointXYZ>), uptodate_(true) {
-	transform_.setIdentity();
-}
-
-static void averageDepth(vector<Mat> &in, Mat &out, float varThresh) {
-	const int rows = in[0].rows;
-	const int cols = in[0].cols;
-	float varThresh2 = varThresh*varThresh;
-
-	// 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 (ftl::rgbd::isValidDepth(d)) {
-				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 - sum);
-					var += delta*delta;
-
-					//LOG(INFO) << "VAR " << delta;
-				}
-			}
-			if (good_values > 1) var /= (double)(good_values-1);
-			else var = 0.0;
-
-			if (var < varThresh2) {
-				out.at<float>(i) = (float)sum;
-			} else {
-				out.at<float>(i) = 0.0f;
-			}
-		} else {
-			out.at<float>(i) = 0.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::drawTarget(cv::Mat &img) {
-	using namespace cv;
-
-	for (size_t i=0; i<log_.size(); i++) {
-	//for (auto &p : points) {
-		auto [tx,ty,sx,sy] = log_[i];
-		drawMarker(img, Point(tx,ty), Scalar(0,255,0), MARKER_TILTED_CROSS);
-	}
-	
-	vector<Eigen::Vector2i> tpoints;
-	getTransformedFeatures2D(tpoints);
-	for (size_t i=0; i<tpoints.size(); i++) {
-		Eigen::Vector2i p = tpoints[i];
-		drawMarker(img, Point(p[0],p[1]), Scalar(255,0,0), MARKER_TILTED_CROSS);
-	}
-}
-
-void Correspondances::drawSource(cv::Mat &img) {
-	using namespace cv;
-	
-	for (size_t i=0; i<log_.size(); i++) {
-	//for (auto &p : points) {
-		auto [tx,ty,sx,sy] = log_[i];
-		drawMarker(img, Point(sx,sy), Scalar(0,255,0), MARKER_TILTED_CROSS);
-	}
-	
-	/*vector<Eigen::Vector2i> tpoints;
-	getTransformedFeatures2D(tpoints);
-	for (size_t i=0; i<tpoints.size(); i++) {
-		Eigen::Vector2i p = tpoints[i];
-		drawMarker(img, Point(p[0],p[1]), Scalar(255,0,0), MARKER_TILTED_CROSS);
-	}*/
-}
-
-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;
-}
-
-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) {
-		src_->grab();
-		targ_->grab();
-		src_->getFrames(rgb1, d1);
-		targ_->getFrames(rgb2, d2);
-
-		d1.copyTo(buffer[0][i]);
-		d2.copyTo(buffer[1][i]);
-
-		std::this_thread::sleep_for(std::chrono::milliseconds(50));
-	}
-	averageDepth(buffer[0], d1, 0.02f);
-	averageDepth(buffer[1], d2, 0.02f);
-	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 > 0.1f && 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 > 0.1f && 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++;
-			}
-		}
-	}
-
-	return true; // TODO: return statement was missing; is true correct?
-}
-
-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;
-	}
-
-	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;
-}
-
-void Correspondances::removeLast() {
-	uptodate_ = false;
-	targ_feat_.erase(targ_feat_.end()-1);
-	src_feat_.erase(src_feat_.end()-1);
-	log_.pop_back();
-}
-
-double Correspondances::estimateTransform(Eigen::Matrix4d &T) {
-	pcl::registration::TransformationValidationEuclidean<PointXYZ, PointXYZ, double> validate;
-	pcl::registration::TransformationEstimationSVD<PointXYZ,PointXYZ, double> svd;
-
-	//validate.setMaxRange(0.1);
-
-	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_);
-
-	svd.estimateRigidTransformation(*src_cloud, idx, *targ_cloud, idx, T);
-	return validate.validateTransformation(src_cloud, targ_cloud, T);
-}
-
-double Correspondances::estimateTransform(Eigen::Matrix4d &T, const std::vector<cv::Vec3d> &src_feat, const std::vector<cv::Vec3d> &targ_feat, bool doicp) {
-	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++) {
-		pcl::PointXYZ ps,pt;
-
-		ps.x = src_feat[i][0];
-		ps.y = src_feat[i][1];
-		ps.z = src_feat[i][2];
-		pt.x = targ_feat[i][0];
-		pt.y = targ_feat[i][1];
-		pt.z = targ_feat[i][2];
-
-		idx.push_back(i);
-		src_cloud->push_back(ps);
-		targ_cloud->push_back(pt);
-	}
-
-	pcl::transformPointCloud(*src_cloud, *tsrc_cloud, transform_);
-
-	svd.estimateRigidTransformation(*tsrc_cloud, idx, *targ_cloud, idx, T);
-
-	if (doicp) {
-		pcl::transformPointCloud(*tsrc_cloud, *src_cloud, T);
-
-		pcl::PointCloud<pcl::PointXYZ>::Ptr final_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-		pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ, double> icp;
-
-		icp.setInputSource(src_cloud);
-		icp.setInputTarget(targ_cloud);
-		icp.align(*final_cloud);
-		LOG(INFO) << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore();
-		//transform_ *= icp.getFinalTransformation();
-
-		T = icp.getFinalTransformation() * T * transform_;
-		//uptodate_ = true;
-
-		return icp.getFitnessScore();
-	} else {
-		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;
-	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>);
-
-	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_);
-
-	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;
-
-void Correspondances::convertToPCL(const std::vector<std::tuple<int,int,int,int>> &p, std::vector<std::pair<pcl::PointXYZ, pcl::PointXYZ>> &pp) {
-	for (size_t i=0; i<p.size(); i++) {
-		auto [sx,sy,tx,ty] = p[i];
-		//LOG(INFO) << "POINT " << sx << "," << sy;
-		auto p1 = makePCL(src_, sx, sy);
-		auto p2 = makePCL(targ_, tx, ty);
-		pp.push_back(std::make_pair(p1,p2));
-	}
-}
-
-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);
-	}
-}
-
-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,src_feat_.size()-1);
-	//int dice_roll = distribution(generator);
-	auto dice = std::bind ( distribution, rng );
-
-	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++) {
-
-		sidx.clear();
-		tidx.clear();
-
-		vector<int> idx;
-		for (int k=0; k<K; k++) {
-			int r = dice();
-			idx.push_back(r);
-			sidx.push_back(src_feat_[r]);
-			tidx.push_back(targ_feat_[r]);
-		}
-
-		Eigen::Matrix4d T;
-		float scoreT = estimateTransform(T, sidx, tidx);
-
-		if (scoreT < score) {
-			score = scoreT;
-			bestT = T;
-			best = idx;
-		}
-	}
-
-	return icp(bestT, tr, best);
-
-	// TODO Add these best points to actual clouds.
-	//log_ = best;
-	//tr = bestT;
-	//uptodate_ = true;
-	//return score;
-}
-
-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
deleted file mode 100644
index 72f39cf86..000000000
--- a/applications/registration/src/correspondances.hpp
+++ /dev/null
@@ -1,110 +0,0 @@
-#ifndef _FTL_REG_CORRESPONDANCES_HPP_
-#define _FTL_REG_CORRESPONDANCES_HPP_
-
-#include <ftl/rgbd/source.hpp>
-#include <nlohmann/json.hpp>
-
-#include <pcl/common/transforms.h>
-
-#include <pcl/registration/transformation_estimation_svd.h>
-#include <pcl/registration/transformation_estimation_svd_scale.h>
-#include <pcl/registration/transformation_validation.h>
-#include <pcl/registration/transformation_validation_euclidean.h>
-
-#include <vector>
-#include <string>
-#include <map>
-#include <tuple>
-
-namespace ftl {
-namespace registration {
-
-/**
- * Manage the identified set of correspondances between two sources. There may
- * also be a parent correspondances object to support the chaining of
- * transforms.
- */
-class Correspondances {
-	public:
-	Correspondances(ftl::rgbd::Source *src, ftl::rgbd::Source *targ);
-	Correspondances(Correspondances *parent, ftl::rgbd::Source *src);
-
-	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.
-	 * 
-	 * @param tx X-Coordinate in target image
-	 * @param ty Y-Coordinate in target image
-	 * @param sx X-Coordinate in source image
-	 * @param sy Y-Coordinate in source image
-	 * @return False if point was invalid and not added.
-	 */
-	bool add(int tx, int ty, int sx, int sy);
-
-	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; }
-
-	/**
-	 * Calculate a transform using current set of correspondances.
-	 * 
-	 * @return Validation score of the transform.
-	 */
-	double estimateTransform(Eigen::Matrix4d &);
-	double estimateTransform(Eigen::Matrix4d &T, const std::vector<int> &src_feat, const std::vector<int> &targ_feat);
-	double estimateTransform(Eigen::Matrix4d &T, const std::vector<cv::Vec3d> &src_feat, const std::vector<cv::Vec3d> &targ_feat, bool doicp=false);
-
-	double findBestSubset(Eigen::Matrix4d &tr, 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);
-
-	/**
-	 * Get the estimated transform. This includes any parent transforms to make
-	 * it relative to root camera.
-	 */
-	Eigen::Matrix4d transform();
-
-	void setTransform(Eigen::Matrix4d &t) { uptodate_ = true; transform_ = t; }
-
-	void drawTarget(cv::Mat &);
-	void drawSource(cv::Mat &);
-
-	private:
-	Correspondances *parent_;
-	ftl::rgbd::Source *targ_;
-	ftl::rgbd::Source *src_;
-	pcl::PointCloud<pcl::PointXYZ>::Ptr targ_cloud_;
-	pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud_;
-	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_;
-};
-
-void build_correspondances(const std::vector<ftl::rgbd::Source*> &sources,
-		std::map<std::string, Correspondances*> &cs, int origin,
-		std::map<std::string, Eigen::Matrix4d> &old);
-
-}
-}
-
-#endif  // _FTL_REG_CORRESPONDANCES_HPP_
diff --git a/applications/registration/src/main.cpp b/applications/registration/src/main.cpp
deleted file mode 100644
index bb566ed07..000000000
--- a/applications/registration/src/main.cpp
+++ /dev/null
@@ -1,21 +0,0 @@
-#include <loguru.hpp>
-#include <ftl/configuration.hpp>
-#include <string>
-
-#include "manual.hpp"
-#include "aruco.hpp"
-
-using std::string;
-
-int main(int argc, char **argv) {
-	auto root = ftl::configure(argc, argv, "registration_default");
-
-	string mode = root->value("mode", string("manual"));
-	if (mode == "manual") {
-		ftl::registration::manual(root);
-	} else if (mode == "aruco") {
-		ftl::registration::aruco(root);
-	}
-
-	return 0;
-}
diff --git a/applications/registration/src/manual.cpp b/applications/registration/src/manual.cpp
deleted file mode 100644
index 90f128855..000000000
--- a/applications/registration/src/manual.cpp
+++ /dev/null
@@ -1,269 +0,0 @@
-#include "manual.hpp"
-#include "correspondances.hpp"
-#include "sfm.hpp"
-#include "common.hpp"
-
-#include <loguru.hpp>
-
-#include <ftl/net/universe.hpp>
-#include <ftl/rgbd/source.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 <map>
-
-using std::string;
-using std::vector;
-using std::pair;
-using std::map;
-using std::tuple;
-
-using ftl::net::Universe;
-using ftl::rgbd::Source;
-using ftl::registration::Correspondances;
-
-using cv::Mat;
-using cv::Point;
-using cv::Scalar;
-
-using namespace ftl::registration;
-
-using MouseAction = std::function<void(int, int, int, int)>;
-
-static void setMouseAction(const std::string& winName, const MouseAction &action)
-{
-	cv::setMouseCallback(winName,
-						[] (int event, int x, int y, int flags, void* userdata) {
-	(*(MouseAction*)userdata)(event, x, y, flags);
-	}, (void*)&action);
-}
-
-static MouseAction tmouse;
-static MouseAction smouse;
-
-void ftl::registration::manual(ftl::Configurable *root) {
-	using namespace cv;
-	
-	Universe *net = ftl::create<Universe>(root, "net");
-
-	net->start();
-	net->waitConnections();
-
-	auto sources = ftl::createArray<Source>(root, "sources", net);
- 
-	int curtarget = 0;
-	bool freeze = false;
-
-	vector<Mat> rgb;
-	vector<Mat> depth;
-
-	if (sources.size() == 0) return;
-
-	rgb.resize(sources.size());
-	depth.resize(sources.size());
-
-	cv::namedWindow("Target", cv::WINDOW_KEEPRATIO);
-	cv::namedWindow("Source", cv::WINDOW_KEEPRATIO);
-
-	map<string, Eigen::Matrix4d> oldTransforms;
-	ftl::registration::loadTransformations(root->value("output", string("./test.json")), oldTransforms);
-
-	//Correspondances c(sources[targsrc], sources[cursrc]);
-	map<string, Correspondances*> corrs;
-	ftl::registration::build_correspondances(sources, corrs, root->value("origin", 0), oldTransforms);
-
-	int lastTX = 0;
-	int lastTY = 0;
-	int lastSX = 0;
-	int lastSY = 0;
-
-	tmouse = [&]( int event, int ux, int uy, int) {
-		if (event == 1) {   // click
-			lastTX = ux;
-			lastTY = uy;
-		}
-	};
-	setMouseAction("Target", tmouse);
-
-	smouse = [&]( int event, int ux, int uy, int) {
-		if (event == 1) {   // click
-			lastSX = ux;
-			lastSY = uy;
-		}
-	};
-	setMouseAction("Source", smouse);
-
-	bool depthtoggle = false;
-	double lastScore = 0.0;
-	bool insearch = false;
-	int point_n = -1;
-
-	Correspondances *current = corrs[sources[curtarget]->getURI()];
-
-	while (ftl::running) {
-		if (!freeze) {
-			// Grab the latest frames from sources
-			for (int i=0; i<sources.size(); i++) {
-				if (sources[i]->isReady()) {
-					sources[i]->grab();
-				}
-			}
-		}
-
-		// Get the raw rgb and depth frames from sources
-		Mat ttrgb, trgb, tdepth, ttdepth;
-		Mat tsrgb, srgb, sdepth, tsdepth;
-
-		if (current == nullptr) {
-			srgb = Mat(Size(640,480), CV_8UC3, Scalar(0,0,0));
-			trgb = Mat(Size(640,480), CV_8UC3, Scalar(0,0,0));
-
-			putText(srgb, string("No correspondance for ") + sources[curtarget]->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-
-			if (!trgb.empty()) imshow("Target", trgb);
-			if (!srgb.empty()) imshow("Source", srgb);
-		} else if (current->source()->isReady() && current->target()->isReady()) {
-			current->source()->getFrames(tsrgb, tsdepth);
-			current->target()->getFrames(ttrgb, ttdepth);
-
-			tsrgb.copyTo(srgb);
-			ttrgb.copyTo(trgb);
-			ttdepth.convertTo(tdepth, CV_8U, 255.0f / 10.0f);
-			applyColorMap(tdepth, tdepth, cv::COLORMAP_JET);
-			tsdepth.convertTo(sdepth, CV_8U, 255.0f / 10.0f);
-			applyColorMap(sdepth, sdepth, cv::COLORMAP_JET);
-
-			current->drawTarget(trgb);
-			current->drawTarget(tdepth);
-			current->drawSource(srgb);
-			current->drawSource(sdepth);
-
-			// 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);
-			drawMarker(sdepth, Point(lastSX, lastSY), Scalar(0,0,255), MARKER_TILTED_CROSS);
-			drawMarker(tdepth, Point(lastTX, lastTY), Scalar(0,0,255), MARKER_TILTED_CROSS);
-
-			putText(srgb, string("Source: ") + current->source()->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-			putText(trgb, string("Target: ") + current->target()->getURI(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-			putText(srgb, string("Score: ") + std::to_string(lastScore), Point(10,40), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-			putText(trgb, string("Score: ") + std::to_string(lastScore), Point(10,40), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-			if (freeze) putText(srgb, string("Paused"), Point(10,50), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-			if (freeze) putText(trgb, string("Paused"), Point(10,50), FONT_HERSHEY_PLAIN, 1.0, Scalar(0,0,255), 1);
-
-			if (!trgb.empty()) imshow("Target", (depthtoggle) ? tdepth : trgb);
-			if (!srgb.empty()) imshow("Source", (depthtoggle) ? sdepth : srgb);
-		}
-	
-		int key = cv::waitKey(20);
-		if (key == 27) break;
-		else if (key >= 48 && key <= 57) {
-			curtarget = key-48;
-			if (curtarget >= sources.size()) curtarget = 0;
-			current = corrs[sources[curtarget]->getURI()];
-			lastScore = 1000.0;
-		} else if (key == 'd') {
-			depthtoggle = !depthtoggle;
-		} else if (key == 'c') {
-			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(t);
-				current->setTransform(t);
-			}
-		} else if (key == 'u') {
-			current->removeLast();
-			Eigen::Matrix4d t;
-			lastScore = current->estimateTransform(t);
-			current->setTransform(t);
-		} else if (key == 'i') {
-			current->icp();
-		} else if (key == 's') {
-			// Save
-			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::Matrix4d().setIdentity();
-				} else {
-					transforms[x.first] = x.second->transform();
-				}
-			}
-
-			saveTransformations(root->value("output", string("./test.json")), transforms);
-			LOG(INFO) << "Saved!";
-		} else if (key == 't') {
-			current->source()->setPose(current->transform());
-		} else if (key == 'g') {
-			if (!insearch) {
-				insearch = true;
-
-				ftl::pool.push([&lastScore,&insearch,current](int id) {
-					LOG(INFO) << "START";
-					lastScore = 1000.0;
-					do {
-						Eigen::Matrix4d tr;
-						float s = current->findBestSubset(tr, 20, 100); // (int)(current->screenPoints().size() * 0.4f)
-						LOG(INFO) << "SCORE = " << s;
-						if (s < lastScore) {
-							lastScore = s;
-							current->setTransform(tr);
-							//current->source()->setPose(current->transform());
-						}
-					} while (ftl::running && insearch && lastScore > 0.000001);
-					insearch = false;
-					LOG(INFO) << "DONE: " << lastScore;
-				});
-				/*for (int i=0; i<feat.size(); i++) {
-					auto [sx,sy,tx,ty] = feat[i];
-					current->add(tx,ty,sx,sy);
-				}
-				lastsScore = current->estimateTransform();*/
-			} else {
-				insearch = false;
-			}
-		} else if (key == 'f') {
-			if (!insearch) {
-				Mat f1,f2;
-				current->capture(f1,f2);
-
-				vector<tuple<int,int,int,int>> feat;
-				if (ftl::registration::featuresSIFT(f1, f2, feat, 10)) {
-					for (int j=0; j<feat.size(); j++) {
-						auto [sx,sy,tx,ty] = feat[j];
-						current->add(tx, ty, sx, sy);
-					}
-				}
-
-				LOG(INFO) << "Found " << current->screenPoints().size() << " features";
-			} else {
-				LOG(ERROR) << "Can't add features whilst searching...";
-			}
-		} else if (key == 32) freeze = !freeze;
-	}
-
-	// store transformations in map<string Matrix4f>
-
-	// TODO:	(later) extract features and correspondencies from complete cloud
-	//			and do ICP to find best possible transformation
-
-	// saveTransformations(const string &path, map<string, Matrix4f> &data)
-}
diff --git a/applications/registration/src/manual.hpp b/applications/registration/src/manual.hpp
deleted file mode 100644
index 1b315cb03..000000000
--- a/applications/registration/src/manual.hpp
+++ /dev/null
@@ -1,14 +0,0 @@
-#ifndef _FTL_REGISTRATION_MANUAL_HPP_
-#define _FTL_REGISTRATION_MANUAL_HPP_
-
-#include <ftl/configurable.hpp>
-
-namespace ftl {
-namespace registration {
-
-void manual(ftl::Configurable *root);
-
-}
-}
-
-#endif  // _FTL_REGISTRATION_MANUAL_HPP_
diff --git a/applications/registration/src/sfm.cpp b/applications/registration/src/sfm.cpp
deleted file mode 100644
index 43711312d..000000000
--- a/applications/registration/src/sfm.cpp
+++ /dev/null
@@ -1,164 +0,0 @@
-#include "sfm.hpp"
-#include <opencv2/sfm.hpp>
-#include <opencv2/viz.hpp>
-#include <opencv2/calib3d.hpp>
-#include <opencv2/core.hpp>
-#include <opencv2/features2d.hpp>
-#include <opencv2/xfeatures2d.hpp>
-
-using std::vector;
-using std::tuple;
-
-using namespace cv;
-using namespace cv::sfm;
-using namespace cv::xfeatures2d;
-using ftl::registration::CalibrationChessboard;
-
-CalibrationChessboard::CalibrationChessboard(ftl::Configurable *root) {
-	pattern_size_ = Size(9, 6);
-	image_size_ = Size(1280, 720);
-	pattern_square_size_ = 36.0;//0.036;
-	// CALIB_CB_NORMALIZE_IMAfE | CALIB_CB_EXHAUSTIVE | CALIB_CB_ACCURACY 
-	chessboard_flags_ = cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_ACCURACY | cv::CALIB_CB_EXHAUSTIVE;
-}
-
-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::drawPoints(Mat &img, const vector<Vec2f> &points) {
-	cv::drawChessboardCorners(img, pattern_size_, points, true);
-}
-
-static void get_tracks(const vector<vector<DMatch>> &matches, const vector<KeyPoint> &kp1,
-		const vector<KeyPoint> &kp2, vector<vector<Vec2d>> &tracks, int k) {
-		
-	for (size_t i=0; i<matches.size(); i++) {
-		// TODO Generalise to any number of keypoint frames
-		if (k >= matches[i].size()) continue;
-		Point2f point1 = kp1[matches[i][k].queryIdx].pt;
-    	Point2f point2 = kp2[matches[i][k].trainIdx].pt;
-    	
-    	vector<Vec2d> track;
-    	track.push_back(Vec2d(point1.x,point1.y));
-    	track.push_back(Vec2d(point2.x,point2.y));
-    	tracks.push_back(track);
-    }
-}
-
-static void get_tracks(const vector<DMatch> &matches, const vector<KeyPoint> &kp1,
-		const vector<KeyPoint> &kp2, vector<vector<Vec2d>> &tracks) {
-		
-	for (size_t i=0; i<matches.size(); i++) {
-		// TODO Generalise to any number of keypoint frames
-		//if (k >= matches[i].size()) continue;
-		Point2f point1 = kp1[matches[i].queryIdx].pt;
-    	Point2f point2 = kp2[matches[i].trainIdx].pt;
-    	
-    	vector<Vec2d> track;
-    	track.push_back(Vec2d(point1.x,point1.y));
-    	track.push_back(Vec2d(point2.x,point2.y));
-    	tracks.push_back(track);
-    }
-}
-
-static void convert_tracks(const vector<vector<Vec2d>> &tracks, vector<tuple<int,int,int,int>> &points2d) {
-	//int n_frames = 2;
-	int n_tracks = tracks.size();
-	
-	//for (int i = 0; i < n_frames; ++i)
-	//{
-		//Mat_<double> frame(2, n_tracks);
-		for (int j = 0; j < n_tracks; ++j)
-		{
-			//frame(0,j) = tracks[j][i][0];
-			//frame(1,j) = tracks[j][i][1];
-			points2d.push_back(std::make_tuple(tracks[j][0][0], tracks[j][0][1], tracks[j][1][0], tracks[j][1][1]));
-		}
-	//}
-}
-
-bool ftl::registration::featuresChess(cv::Mat &frame1, cv::Mat &frame2, std::vector<std::tuple<int,int,int,int>> &points) {
-	CalibrationChessboard cb(nullptr);
-
-	vector<Vec2f> points1, points2;
-	if (!cb.findPoints(frame1, points1)) {
-		LOG(ERROR) << "Could not find chessboard (1)";
-		return false;
-	}
-	if (!cb.findPoints(frame2, points2)) {
-		LOG(ERROR) << "Could not find chessboard (2)";
-		return false;
-	}
-
-	if (points1.size() != points2.size()) return false;
-
-	for (size_t i=0; i<points1.size(); i++) {
-		points.push_back(std::make_tuple(points1[i][0], points1[i][1], points2[i][0], points2[i][1]));
-	}
-	return true;
-}
-
-bool ftl::registration::featuresSIFT(cv::Mat &frame1, cv::Mat &frame2, std::vector<std::tuple<int,int,int,int>> &points, int K) {
-	int minHessian = 400;
-	Ptr<SIFT> detector = SIFT::create(10000,3,0.04,10);
-	//detector->setHessianThreshold(minHessian);
-
-	vector<vector<KeyPoint>> keypoints;
-	vector<Mat> descriptors;
-	
-	{
-		vector<KeyPoint> kps;
-		Mat desc;
-		detector->detectAndCompute(frame1, Mat(), kps, desc);
-		keypoints.push_back(kps);
-		descriptors.push_back(desc);
-	}
-
-	{
-		vector<KeyPoint> kps;
-		Mat desc;
-		detector->detectAndCompute(frame2, Mat(), kps, desc);
-		keypoints.push_back(kps);
-		descriptors.push_back(desc);
-	}
-	
-	// TODO This can be done on node machines...
-	
-	//-- Step 2: Matching descriptor vectors using FLANN matcher
-	// Match between each sequential pair of images in the feed.
-	FlannBasedMatcher matcher;
-	std::vector<std::vector< DMatch >> matches;
-	matcher.knnMatch( descriptors[0], descriptors[1], matches, K );
-	
-	const float ratio_thresh = 0.9f;
-    std::vector<DMatch> good_matches;
-    for (size_t i = 0; i < matches.size(); i++)
-    {
-		for (int k=0; k<K-1; k++) {
-			if (matches[i][k].distance < ratio_thresh * matches[i][k+1].distance)
-			{
-				good_matches.push_back(matches[i][k]);
-			} else break;
-		}
-    }
-	
-	// --- SFM ---
-
-	//for (int i=0; i<K; i++) {
-		vector<vector<Vec2d>> tracks;
-		get_tracks(good_matches, keypoints[0], keypoints[1], tracks);
-		convert_tracks(tracks, points);
-	//}
-
-	//cout << "Tracks: " << tracks.size() << endl;
-	return true;
-}
diff --git a/applications/registration/src/sfm.hpp b/applications/registration/src/sfm.hpp
deleted file mode 100644
index 97e27da10..000000000
--- a/applications/registration/src/sfm.hpp
+++ /dev/null
@@ -1,44 +0,0 @@
-#ifndef _FTL_REGISTRATION_SFM_HPP_
-#define _FTL_REGISTRATION_SFM_HPP_
-
-#include <ftl/configurable.hpp>
-#include <opencv2/opencv.hpp>
-#include <vector>
-#include <tuple>
-
-namespace ftl {
-namespace registration {
-
-/**
- * @brief	Chessboard calibration pattern. Uses OpenCV's
- * 			findChessboardCornersSB function.
- * @todo	Parameters hardcoded in constructor
- *
- * All parameters:
- * 	- pattern size (inner corners)
- * 	- square size, millimeters (TODO: meters)
- * 	- image size, pixels
- * 	- flags, see ChessboardCornersSB documentation
- */
-class CalibrationChessboard {
-public:
-	CalibrationChessboard(ftl::Configurable *root);
-	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);
-
-private:
-	int chessboard_flags_ = 0;
-	float pattern_square_size_;
-	cv::Size pattern_size_;
-	cv::Size image_size_;
-};
-
-bool featuresSIFT(cv::Mat &frame1, cv::Mat &frame2, std::vector<std::tuple<int,int,int,int>> &points, int);
-
-bool featuresChess(cv::Mat &frame1, cv::Mat &frame2, std::vector<std::tuple<int,int,int,int>> &points);
-
-}
-}
-
-#endif  // _FTL_REGISTRATION_SFM_HPP_
diff --git a/components/common/cpp/CMakeLists.txt b/components/common/cpp/CMakeLists.txt
index de0f7707c..e2f1e70fa 100644
--- a/components/common/cpp/CMakeLists.txt
+++ b/components/common/cpp/CMakeLists.txt
@@ -4,7 +4,6 @@ set(COMMONSRC
 	src/configuration.cpp
 	src/configurable.cpp
 	src/loguru.cpp
-	src/opencv_to_pcl.cpp
 	src/cuda_common.cpp
 	src/ctpl_stl.cpp
 	src/timer.cpp
@@ -17,11 +16,10 @@ add_library(ftlcommon ${COMMONSRC})
 target_compile_options(ftlcommon PUBLIC $<$<COMPILE_LANGUAGE:CXX>:-fPIC>)
 
 target_include_directories(ftlcommon PUBLIC
-	${PCL_INCLUDE_DIRS}
 	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
 	$<INSTALL_INTERFACE:include>
 	PRIVATE src)
-target_link_libraries(ftlcommon Threads::Threads ${OS_LIBS} ${OpenCV_LIBS} ${PCL_LIBRARIES} ${URIPARSER_LIBRARIES} ${CUDA_LIBRARIES})
+target_link_libraries(ftlcommon Threads::Threads ${OS_LIBS} ${OpenCV_LIBS} ${URIPARSER_LIBRARIES} ${CUDA_LIBRARIES})
 
 add_subdirectory(test)
 
diff --git a/components/common/cpp/include/ftl/config.h.in b/components/common/cpp/include/ftl/config.h.in
index 540b33203..52fa616b0 100644
--- a/components/common/cpp/include/ftl/config.h.in
+++ b/components/common/cpp/include/ftl/config.h.in
@@ -17,7 +17,6 @@
 #cmakedefine HAVE_CUDA
 #cmakedefine HAVE_OPENCV
 #cmakedefine HAVE_OPTFLOW
-#cmakedefine HAVE_PCL
 #cmakedefine HAVE_RENDER
 #cmakedefine HAVE_LIBSGM
 #cmakedefine HAVE_REALSENSE
diff --git a/components/common/cpp/include/ftl/utility/opencv_to_pcl.hpp b/components/common/cpp/include/ftl/utility/opencv_to_pcl.hpp
deleted file mode 100644
index f273d5e23..000000000
--- a/components/common/cpp/include/ftl/utility/opencv_to_pcl.hpp
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef _FTL_COMMON_OPENCV_TO_PCL_HPP_
-#define _FTL_COMMON_OPENCV_TO_PCL_HPP_
-
-#include <ftl/config.h>
-#include <opencv2/opencv.hpp>
-
-#if defined HAVE_PCL
-#include <pcl/common/common_headers.h>
-
-namespace ftl {
-namespace utility {
-
-/**
- * Convert an OpenCV point cloud matrix and RGB image to a PCL XYZRGB point cloud.
- */
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr matToPointXYZ(const cv::Mat &cvcloud, const cv::Mat &rgbimage);
-
-};
-};
-#endif  // HAVE_PCL
-
-#endif  // _FTL_COMMON_OPENCV_TO_PCL_HPP_
diff --git a/components/common/cpp/src/opencv_to_pcl.cpp b/components/common/cpp/src/opencv_to_pcl.cpp
deleted file mode 100644
index c56196b31..000000000
--- a/components/common/cpp/src/opencv_to_pcl.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-#include <ftl/utility/opencv_to_pcl.hpp>
-
-#if defined HAVE_PCL
-pcl::PointCloud<pcl::PointXYZRGB>::Ptr ftl::utility::matToPointXYZ(const cv::Mat &cvcloud, const cv::Mat &rgbimage) {
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
-	point_cloud_ptr->width = cvcloud.cols * cvcloud.rows;
-	point_cloud_ptr->height = 1;
-
-	for(int i=0;i<cvcloud.rows;i++) {
-	for(int j=0;j<cvcloud.cols;j++) {
-		//std::cout<<i<<endl;
-
-		pcl::PointXYZRGB point;
-		cv::Vec3f cvpoint = cvcloud.at<cv::Vec3f>(i,j);
-		point.x = cvpoint[0];
-		point.y = cvpoint[1];
-		point.z = cvpoint[2];
-
-		if (point.x == INFINITY || point.y == INFINITY || point.z == INFINITY || point.z < 600.0f) {
-			point.x = 0; point.y = 0; point.z = 0;
-		}
-
-		cv::Point3_<uchar> prgb = rgbimage.at<cv::Point3_<uchar>>(i, j);
-
-		// when color needs to be added:
-		uint32_t rgb = (static_cast<uint32_t>(prgb.z) << 16 | static_cast<uint32_t>(prgb.y) << 8 | static_cast<uint32_t>(prgb.x));
-		// cppcheck-suppress invalidPointerCast
-		point.rgb = *reinterpret_cast<float*>(&rgb);
-
-		point_cloud_ptr -> points.push_back(point);
-	}
-	}
-
-	return point_cloud_ptr;
-}
-#endif  // HAVE_PCL
\ No newline at end of file
diff --git a/components/operators/CMakeLists.txt b/components/operators/CMakeLists.txt
index 2a55cce0d..3bff44a4c 100644
--- a/components/operators/CMakeLists.txt
+++ b/components/operators/CMakeLists.txt
@@ -39,11 +39,7 @@ endif()
 
 add_library(ftloperators ${OPERSRC})
 
-# These cause errors in CI build and are being removed from PCL in newer versions
-# target_compile_options(ftlrender PUBLIC ${PCL_DEFINITIONS})
-
 target_include_directories(ftloperators PUBLIC
-	${PCL_INCLUDE_DIRS}
 	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
 	$<INSTALL_INTERFACE:include>
 	PRIVATE src)
diff --git a/components/renderers/cpp/CMakeLists.txt b/components/renderers/cpp/CMakeLists.txt
index 14bacda71..112650fee 100644
--- a/components/renderers/cpp/CMakeLists.txt
+++ b/components/renderers/cpp/CMakeLists.txt
@@ -10,11 +10,7 @@ add_library(ftlrender
 	src/tri_render.cpp
 )
 
-# These cause errors in CI build and are being removed from PCL in newer versions
-# target_compile_options(ftlrender PUBLIC ${PCL_DEFINITIONS})
-
 target_include_directories(ftlrender PUBLIC
-	${PCL_INCLUDE_DIRS}
 	$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
 	$<INSTALL_INTERFACE:include>
 	PRIVATE src)
diff --git a/reconstruct/src/voxel_hash.cu b/reconstruct/src/voxel_hash.cu
deleted file mode 100644
index 6dfe838a7..000000000
--- a/reconstruct/src/voxel_hash.cu
+++ /dev/null
@@ -1,127 +0,0 @@
-#include <ftl/voxel_hash.hpp>
-
-using ftl::voxhash::VoxelHash;
-using ftl::voxhash::hash;
-using ftl::voxhash::HashBucket;
-using ftl::voxhash::HashEntry;
-
-__device__ HashEntry *ftl::voxhash::cuda::lookupBlock(HashBucket *table, int3 vox) {
-	uint32_t hashcode = hash<HASH_SIZE>(vox.x / 8, vox.y / 8, vox.z / 8);
-	HashBucket *bucket = table[hashcode];
-
-	while (bucket) {
-		for (int i=0; i<ENTRIES_PER_BUCKET; i++) {
-			HashEntry *entry = &bucket->entries[i];
-			if (entry->position[0] == vox.x && entry->position[1] == vox.y && entry->position[2] == vox.z) {
-				return entry;
-			}
-		}
-		if (bucket->entries[ENTRIES_PER_BUCKET-1].offset) {
-			bucket += bucket->entries[ENTRIES_PER_BUCKET-1].offset;
-		} else {
-			break;
-		}
-	}
-
-	return nullptr;
-}
-
-__device__ HashEntry *ftl::voxhash::cuda::insertBlock(HashBucket *table, int3 vox) {
-	uint32_t hashcode = hash<HASH_SIZE>(vox.x / 8, vox.y / 8, vox.z / 8);
-	HashBucket *bucket = table[hashcode];
-
-	while (bucket) {
-		for (int i=0; i<ENTRIES_PER_BUCKET; i++) {
-			HashEntry *entry = &bucket->entries[i];
-			if (entry->position[0] == vox.x && entry->position[1] == vox.y && entry->position[2] == vox.z) {
-				return entry;
-			} else if (entry->pointer == 0) {
-				// Allocate block.
-				entry->position[0] = vox.x;
-				entry->position[1] = vox.y;
-				entry->position[2] = vox.z;
-			}
-		}
-		if (bucket->entries[ENTRIES_PER_BUCKET-1].offset) {
-			bucket += bucket->entries[ENTRIES_PER_BUCKET-1].offset;
-		} else {
-			// Find a new bucket to append to this list
-		}
-	}
-
-	return nullptr;
-}
-
-__device__ Voxel *ftl::voxhash::cuda::insert(HashBucket *table, VoxelBlocks *blocks, int3 vox) {
-	HashEntry *entry = insertBlock(table, vox);
-	VoxelBlock *block = blocks[entry->pointer];
-	return &block->voxels[vox.x % 8][vox.y % 8][vox.z % 8];
-}
-
-__device__ Voxel *ftl::voxhash::cuda::lookup(HashBucket *table, int3 vox) {
-	HashEntry *entry = lookupBlock(table, vox);
-	if (entry == nullptr) return nullptr;
-	VoxelBlock *block = blocks[entry->pointer];
-	return &block->voxels[vox.x % 8][vox.y % 8][vox.z % 8];
-}
-
-VoxelHash::VoxelHash(float resolution) : resolution_(resolution) {
-	// Allocate CPU memory
-	table_cpu_.resize(HASH_SIZE);
-
-	// Allocate and init GPU memory
-	table_gpu_ = table_cpu_;
-}
-
-VoxelHash::~VoxelHash() {
-
-}
-
-void VoxelHash::nextFrame() {
-	// Clear the hash for now...
-}
-
-__global__ static void merge_view_kernel(PtrStepSz<cv::Point3f> cloud, PtrStepSz<uchar3> rgb, PtrStepSz<double> pose, ftl::voxhash::cuda::VoxelHash vhash) {
-	for (STRIDE_Y(v,cloud.rows) {
-		for (STRIDE_X(u,cloud.cols) {
-			cv::Point3f p = cloud.at<cv::Point3f>(v,u);
-			// TODO Lock hash block on insert.
-			Voxel *voxel = ftl::voxhash::cuda::insert(vhash.table, make_int3(p.x,p.y,p.z));
-			uchar3 colour = rgb.at<uchar3>(v,u);
-			voxel->colorRGB[0] = colour[0];
-			voxel->colorRGB[1] = colour[1];
-			voxel->colorRGB[2] = colour[2];
-			voxel->weight = 1;
-		}
-	}
-}
-
-static void merge_view_call(const PtrStepSz<cv::Point3f> &cloud, const PtrStepSz<uchar3> &rgb, const PtrStepSz<double> &pose, ftl::voxhash::cuda::VoxelHash vhash) {
-	dim3 grid(1,1,1);
-	dim3 threads(128, 1, 1);
-	grid.x = cv::cuda::device::divUp(l.cols, 128);
-	grid.y = cv::cuda::device::divUp(l.rows, 21);
-
-	merge_view_kernel<<<grid,threads>>>(cloud, rgb, pose, vhash);
-	cudaSafeCall( cudaGetLastError() );
-}
-
-void VoxelHash::mergeView(const cv::Mat &cloud, const cv::Mat &rgb, const cv::Mat &pose) {
-	// Upload GPU Mat
-	gpu_cloud_.upload(cloud);
-	gpu_rgb_.upload(rgb);
-	gpu_pose_.upload(pose);
-
-	// Call kernel to insert each point in hash table
-	merge_view_call(gpu_cloud_, gpu_rgb_, gpu_pose_, getCUDAStructure());
-}
-
-// Called per voxel
-__global__ static void render_kernel(PtrStepSz<uchar3> out, PtrStepSz<double> cam, ftl::voxhash::cuda::VoxelHash vhash) {
-
-}
-
-void VoxelHash::render(cv::Mat &output, const cv::Mat &cam) {
-	// Call kernel to process every voxel and add to image
-	// Download gpu mat into output.
-}
-- 
GitLab