diff --git a/CMakeLists.txt b/CMakeLists.txt
index 37d4fa1008247dcf60123d83e0ce5293e917491a..4b160d9b1d15986e20dd19d4083d0f7d9080429a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -20,6 +20,7 @@ option(BUILD_RECONSTRUCT "Enable the reconstruction component" ON)
 option(BUILD_RENDERER "Enable the renderer component" ON)
 option(BUILD_GUI "Enable the GUI" ON)
 option(BUILD_CALIBRATION "Enable the calibration component" OFF)
+option(BUILD_TOOLS "Compile developer and research tools" ON)
 
 set(THREADS_PREFER_PTHREAD_FLAG ON)
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/")
@@ -259,6 +260,7 @@ add_subdirectory(applications/calibration)
 #add_subdirectory(applications/player)
 #add_subdirectory(applications/recorder)
 #add_subdirectory(applications/merger)
+add_subdirectory(applications/tools)
 
 if (HAVE_AVFORMAT)
 	add_subdirectory(applications/ftl2mkv)
diff --git a/applications/gui/src/camera.cpp b/applications/gui/src/camera.cpp
index 585d99a4ce9c3ed5383fddab8a698892ce453339..4a30668cc5afdf116869ff589ef635b765384d81 100644
--- a/applications/gui/src/camera.cpp
+++ b/applications/gui/src/camera.cpp
@@ -192,7 +192,7 @@ ftl::gui::Camera::Camera(ftl::gui::Screen *screen, int fsid, int fid, ftl::codec
 		state_.getLeft().cx = -(double)state_.getLeft().width / 2.0;
 		state_.getLeft().cy = -(double)state_.getLeft().height / 2.0;
 		state_.getLeft().minDepth = host->value("minDepth", 0.1f);
-		state_.getLeft().maxDepth = host->value("maxDepth", 20.0f);
+		state_.getLeft().maxDepth = host->value("maxDepth", 15.0f);
 		state_.getLeft().doffs = 0;
 		state_.getLeft().baseline = host->value("baseline", 0.05f);
 
@@ -203,7 +203,7 @@ ftl::gui::Camera::Camera(ftl::gui::Screen *screen, int fsid, int fid, ftl::codec
 		state_.getRight().cx = host->value("centre_x_right", -(double)state_.getLeft().width / 2.0);
 		state_.getRight().cy = host->value("centre_y_right", -(double)state_.getLeft().height / 2.0);
 		state_.getRight().minDepth = host->value("minDepth", 0.1f);
-		state_.getRight().maxDepth = host->value("maxDepth", 20.0f);
+		state_.getRight().maxDepth = host->value("maxDepth", 15.0f);
 		state_.getRight().doffs = 0;
 		state_.getRight().baseline = host->value("baseline", 0.05f);
 
diff --git a/applications/gui/src/media_panel.cpp b/applications/gui/src/media_panel.cpp
index d7e012c5204cbab76e13bde9eed5925a46fa94ff..8723000a1f18019d8021590cbd46b81176fb9fdc 100644
--- a/applications/gui/src/media_panel.cpp
+++ b/applications/gui/src/media_panel.cpp
@@ -87,10 +87,14 @@ MediaPanel::MediaPanel(ftl::gui::Screen *screen, ftl::gui::SourceWindow *sourceW
 	});
 
 	button = new Button(this, "", ENTYPO_ICON_CONTROLLER_PAUS);
-	button->setCallback([this,button]() {
+	button->setCallback([this,button,sourceWindow]() {
 		//paused_ = !paused_;
-		paused_ = !(bool)ftl::config::get("[reconstruction]/controls/paused");
-		ftl::config::update("[reconstruction]/controls/paused", paused_);
+		//paused_ = !(bool)ftl::config::get("[reconstruction]/controls/paused");
+		//ftl::config::update("[reconstruction]/controls/paused", paused_);
+
+		paused_ = !paused_;
+		sourceWindow->paused(paused_);
+
 		if (paused_) {
 			button->setIcon(ENTYPO_ICON_CONTROLLER_PLAY);
 		} else {
diff --git a/applications/gui/src/src_window.cpp b/applications/gui/src/src_window.cpp
index 9681239c3f84ae6709087ae938303e3300b329be..fe63a858b4cb6f105dffa82a6637bd1c5f608d82 100644
--- a/applications/gui/src/src_window.cpp
+++ b/applications/gui/src/src_window.cpp
@@ -21,6 +21,7 @@
 #include "ftl/operators/segmentation.hpp"
 #include "ftl/operators/mask.hpp"
 #include "ftl/operators/antialiasing.hpp"
+#include <ftl/operators/smoothing.hpp>
 
 #ifdef HAVE_LIBARCHIVE
 #include "ftl/rgbd/snapshot.hpp"
@@ -72,22 +73,26 @@ SourceWindow::SourceWindow(ftl::gui::Screen *screen)
 	});
 
 	pre_pipeline_ = ftl::config::create<ftl::operators::Graph>(screen->root(), "pre_filters");
+	//pre_pipeline_->append<ftl::operators::HFSmoother>("hfnoise");
 	//pre_pipeline_->append<ftl::operators::ColourChannels>("colour");  // Convert BGR to BGRA
 	pre_pipeline_->append<ftl::operators::CrossSupport>("cross");
 	pre_pipeline_->append<ftl::operators::DiscontinuityMask>("discontinuity");
 	pre_pipeline_->append<ftl::operators::CullDiscontinuity>("remove_discontinuity");
 	pre_pipeline_->append<ftl::operators::VisCrossSupport>("viscross")->set("enabled", false);
 
+	paused_ = false;
 	cycle_ = 0;
 	receiver_->onFrameSet([this](ftl::rgbd::FrameSet &fs) {
-		// Enforce interpolated colour
-		for (int i=0; i<fs.frames.size(); ++i) {
-			fs.frames[i].createTexture<uchar4>(Channel::Colour, true);
-		}
+		if (!paused_) {
+			// Enforce interpolated colour
+			for (int i=0; i<fs.frames.size(); ++i) {
+				fs.frames[i].createTexture<uchar4>(Channel::Colour, true);
+			}
 
-		pre_pipeline_->apply(fs, fs, 0);
+			pre_pipeline_->apply(fs, fs, 0);
 
-		fs.swapTo(frameset_);
+			fs.swapTo(frameset_);
+		}
 
 		// Request the channels required by current camera configuration
 		interceptor_->select(frameset_.id, _aggregateChannels());
diff --git a/applications/gui/src/src_window.hpp b/applications/gui/src/src_window.hpp
index 1aa9b5345aacc8b926a0e1c2902586218c08741d..6cbcb7c316fc1db0e8a1e3272f151f4afc3260bf 100644
--- a/applications/gui/src/src_window.hpp
+++ b/applications/gui/src/src_window.hpp
@@ -48,6 +48,8 @@ class SourceWindow : public nanogui::Window {
 	void recordVideo(const std::string &filename);
 	void stopRecordingVideo();
 
+	inline void paused(bool p) { paused_ = p; }
+
 	private:
 	ftl::gui::Screen *screen_;
 
@@ -73,6 +75,7 @@ class SourceWindow : public nanogui::Window {
 	ftl::audio::Speaker *speaker_;
 
 	ftl::rgbd::FrameSet frameset_;
+	bool paused_;
 
 	void _updateCameras(const std::vector<std::string> &netcams);
 	void _createDefaultCameras(ftl::rgbd::FrameSet &fs, bool makevirtual);
diff --git a/applications/tools/CMakeLists.txt b/applications/tools/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a96555932de3a2389e62059687813ea8f04a216d
--- /dev/null
+++ b/applications/tools/CMakeLists.txt
@@ -0,0 +1 @@
+add_subdirectory(codec_eval)
diff --git a/applications/tools/codec_eval/CMakeLists.txt b/applications/tools/codec_eval/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..5124feb550feddd91cae24e1978e87b212dc6a2a
--- /dev/null
+++ b/applications/tools/codec_eval/CMakeLists.txt
@@ -0,0 +1,21 @@
+# Need to include staged files and libs
+#include_directories(${PROJECT_SOURCE_DIR}/reconstruct/include)
+#include_directories(${PROJECT_BINARY_DIR})
+
+set(PSNRSRC
+	src/main.cpp
+)
+
+add_executable(codec-eval ${PSNRSRC})
+
+target_include_directories(codec-eval PUBLIC
+	#$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+	#$<INSTALL_INTERFACE:include>
+	PRIVATE src)
+
+if (CUDA_FOUND)
+set_property(TARGET codec-eval PROPERTY CUDA_SEPARABLE_COMPILATION ON)
+endif()
+
+#target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include)
+target_link_libraries(codec-eval ftlcommon ftlrgbd Threads::Threads ${OpenCV_LIBS} ftlctrl ftlnet ftlrender ftloperators ftlstreams ftlaudio)
diff --git a/applications/tools/codec_eval/src/main.cpp b/applications/tools/codec_eval/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c1730af505208497c38666e28f7e9df68f36259b
--- /dev/null
+++ b/applications/tools/codec_eval/src/main.cpp
@@ -0,0 +1,287 @@
+/*
+ * Copyright 2019 Nicolas Pope. All rights reserved.
+ *
+ * See LICENSE.
+ */
+
+#define LOGURU_WITH_STREAMS 1
+#include <loguru.hpp>
+#include <ftl/config.h>
+#include <ftl/configuration.hpp>
+#include <ftl/master.hpp>
+#include <ftl/threads.hpp>
+#include <ftl/codecs/channels.hpp>
+#include <ftl/codecs/depth_convert_cuda.hpp>
+
+#include <fstream>
+#include <string>
+#include <vector>
+#include <thread>
+#include <chrono>
+
+#include <opencv2/opencv.hpp>
+#include <opencv2/quality/qualitypsnr.hpp>
+#include <ftl/net/universe.hpp>
+
+#include <ftl/streams/filestream.hpp>
+#include <ftl/streams/receiver.hpp>
+#include <ftl/streams/sender.hpp>
+#include <ftl/streams/netstream.hpp>
+
+#include <ftl/operators/colours.hpp>
+#include <ftl/operators/mask.hpp>
+#include <ftl/operators/segmentation.hpp>
+#include <ftl/operators/depth.hpp>
+
+#ifdef WIN32
+#pragma comment(lib, "Rpcrt4.lib")
+#endif
+
+using ftl::net::Universe;
+using std::string;
+using std::vector;
+using ftl::config::json_t;
+using ftl::codecs::Channel;
+using ftl::codecs::codec_t;
+using ftl::codecs::definition_t;
+
+using json = nlohmann::json;
+using std::this_thread::sleep_for;
+using std::chrono::milliseconds;
+
+static ftl::rgbd::Generator *createFileGenerator(ftl::Configurable *root, const std::string &filename) {
+	ftl::stream::File *stream = ftl::create<ftl::stream::File>(root, "player");
+	stream->set("filename", filename);
+
+	ftl::stream::Receiver *gen = ftl::create<ftl::stream::Receiver>(root, "receiver");
+	gen->setStream(stream);
+
+	stream->begin();
+	stream->select(0, Channel::Colour + Channel::Depth);  // TODO: Choose these elsewhere
+	return gen;
+}
+
+static void visualizeDepthMap(	const cv::Mat &depth, cv::Mat &out,
+								const float max_depth)
+{
+	DCHECK(max_depth > 0.0);
+
+	depth.convertTo(out, CV_8U, 255.0f / max_depth);
+	out = 255 - out;
+	//cv::Mat mask = (depth >= max_depth); // TODO (mask for invalid pixels)
+	
+	applyColorMap(out, out, cv::COLORMAP_JET);
+	//out.setTo(cv::Scalar(0), mask);
+	//cv::cvtColor(out,out, cv::COLOR_BGR2BGRA);
+}
+
+static void run(ftl::Configurable *root) {
+	Universe *net = ftl::create<Universe>(root, "net");
+	ftl::ctrl::Master ctrl(root, net);
+
+	// Controls
+	auto *controls = ftl::create<ftl::Configurable>(root, "controls");
+
+	net->start();
+	net->waitConnections();
+
+	ftl::codecs::Packet pkt;
+	pkt.codec = codec_t::HEVC;
+	pkt.bitrate = 255;
+	pkt.definition = definition_t::Any;
+	pkt.flags = ftl::codecs::kFlagFloat | ftl::codecs::kFlagMappedDepth;
+	pkt.frame_count = 1;
+
+	auto *enc = ftl::codecs::allocateEncoder();
+	auto *dec = ftl::codecs::allocateDecoder(pkt);
+	cv::cuda::GpuMat gtmp, g_yuv8, g_yuv16;
+
+	double count = 0.0;
+	double avgpsnr = 0.0;
+	double avgbitrate = 0.0;
+
+	bool toggle_filter = false;
+	bool toggle_median = false;
+	bool toggle_mask = false;
+	bool toggle_bilat = false;
+
+	auto *pre_pipeline_ = ftl::config::create<ftl::operators::Graph>(root, "pre_filters");
+	//pre_pipeline_->append<ftl::operators::HFSmoother>("hfnoise");
+	pre_pipeline_->append<ftl::operators::ColourChannels>("colour");  // Convert BGR to BGRA
+	pre_pipeline_->append<ftl::operators::CrossSupport>("cross");
+	auto *maskfilter = pre_pipeline_->append<ftl::operators::DiscontinuityMask>("discontinuity");
+
+	auto *post_filter = ftl::config::create<ftl::operators::Graph>(root, "post_filters");
+	//post_filter->append<ftl::operators::CullDiscontinuity>("mask");
+	auto *bilat = post_filter->append<ftl::operators::DepthBilateralFilter>("bilateral", Channel::Depth);
+
+	bilat->set("edge_discontinuity", 0.04f);
+	bilat->set("max_discontinuity", 0.07f);
+	bilat->set("radius", 5);
+	bilat->set("iterations", 5);
+
+	ftl::rgbd::Frame oframe;
+
+	maskfilter->set("radius",3);
+	//maskfilter->set("threshold", 0.04f);
+
+	// Check paths for FTL files to load.
+	auto paths = (*root->get<nlohmann::json>("paths"));
+	int i = 0; //groups.size();
+	for (auto &x : paths.items()) {
+		std::string path = x.value().get<std::string>();
+		auto eix = path.find_last_of('.');
+		auto ext = path.substr(eix+1);
+
+		// Command line path is ftl file
+		if (ext == "ftl") {
+			auto *gen = createFileGenerator(root, path);
+
+			gen->onFrameSet([&](ftl::rgbd::FrameSet &fs) {
+				fs.id = i;
+
+				if (!fs.frames[0].hasChannel(Channel::Depth)) return true;
+
+				pre_pipeline_->apply(fs,fs,0);
+
+				//fs.frames[0].copyTo(ftl::codecs::Channels<0>(Channel::Colour), oframe);
+				fs.frames[0].get<cv::cuda::GpuMat>(Channel::Colour).copyTo(oframe.create<cv::cuda::GpuMat>(Channel::Colour));
+				fs.frames[0].get<cv::cuda::GpuMat>(Channel::Mask).copyTo(oframe.create<cv::cuda::GpuMat>(Channel::Mask));
+
+				auto &gmat = fs.frames[0].get<cv::cuda::GpuMat>(Channel::Depth);
+				auto &gtmp = oframe.create<cv::cuda::GpuMat>(Channel::Depth);
+
+
+				ftl::codecs::Packet pkt;
+				pkt.codec = codec_t::HEVC;
+				pkt.bitrate = 255;
+				pkt.definition = definition_t::Any;
+				pkt.flags = ftl::codecs::kFlagFloat | ftl::codecs::kFlagMappedDepth;
+				pkt.frame_count = 1;
+
+				g_yuv8.create(gmat.size(), CV_8UC4);
+				g_yuv16.create(gmat.size(), CV_16UC4);
+				gtmp.create(gmat.size(), CV_32F);
+				ftl::cuda::depth_to_vuya(gmat, g_yuv8, 16.0f, cv::cuda::Stream::Null());
+
+				if (enc && dec) {
+					if (!enc->encode(g_yuv8, pkt)) return true;
+					if (!dec->decode(pkt, g_yuv16)) return true;
+				} else {
+					LOG(ERROR) << "No encoder or decoder";
+				}
+
+				double data_rate = double(pkt.data.size()*8*20) / double(1024*1024);
+				avgbitrate += data_rate;
+
+				if (toggle_median) {
+					cv::Mat tmp;
+					g_yuv16.download(tmp);
+					cv::medianBlur(tmp, tmp, 5);
+					g_yuv16.upload(tmp);
+				}
+
+				if (toggle_filter) ftl::cuda::smooth_y(g_yuv16, cv::cuda::Stream::Null());
+				ftl::cuda::vuya_to_depth(gtmp, g_yuv16, 16.0f, cv::cuda::Stream::Null());
+
+				if (toggle_bilat) post_filter->apply(oframe,oframe,0);
+				
+				cv::Mat tmp1, tmp2, tmp3, maski;
+				gmat.download(tmp1);
+				gtmp.download(tmp2);
+				fs.frames[0].get<cv::cuda::GpuMat>(Channel::Mask).download(maski);
+
+				cv::Mat maskd = (maski > 0);
+				if (toggle_mask) {
+					tmp1.setTo(cv::Scalar(0.0f), maskd);
+					tmp2.setTo(cv::Scalar(0.0f), maskd);
+				}
+
+				cv::Mat mask = (tmp1 >= 15.0f);
+				tmp1.setTo(cv::Scalar(0), mask);
+
+				double psnr = cv::quality::QualityPSNR::compute(tmp1,tmp2,tmp3,16.0f)[0];
+				avgpsnr += psnr;
+				count += 1.0;
+
+				//cv::absdiff(tmp1,tmp2,tmp1);
+				//visualizeDepthMap(tmp1,tmp1,1.0f);
+				//tmp1.convertTo(tmp1, CV_8U, 255.0f / 1.0f);
+
+				tmp3.convertTo(tmp3, CV_8U, 255.0f);
+				cv::cvtColor(tmp3,tmp3,cv::COLOR_GRAY2BGR);
+
+				//tmp3.setTo(cv::Scalar(0,0,255), maskd);
+
+				cv::putText(tmp3,
+						std::string("PSNR ") + std::to_string(psnr) + std::string("db"),
+						cv::Point2i(10, 30),
+						cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, cv::Scalar(64, 64, 255), 1);
+
+				cv::putText(tmp3,
+						std::string("Bitrate ") + std::to_string(data_rate) + std::string("Mbps"),
+						cv::Point2i(10, 50),
+						cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, cv::Scalar(64, 64, 255), 1);
+
+				cv::putText(tmp3,
+						std::string("Avg PSNR ") + std::to_string(avgpsnr / count) + std::string("dB"),
+						cv::Point2i(10, 70),
+						cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, cv::Scalar(64, 64, 255), 1);
+
+				cv::putText(tmp3,
+						std::string("Avg Bitrate ") + std::to_string(avgbitrate / count) + std::string("Mbps"),
+						cv::Point2i(10, 90),
+						cv::FONT_HERSHEY_COMPLEX_SMALL, 1.0, cv::Scalar(64, 64, 255), 1);
+
+				cv::imshow("Frame", tmp3);
+
+				/*cv::Mat ytmp;
+				g_yuv16.download(ytmp);
+				cv::Mat planes[4];
+				cv::split(ytmp,planes);
+				cv::imshow("Y Channel", planes[2]);*/
+
+				visualizeDepthMap(tmp2,tmp2,8.0f);
+				cv::imshow("Depth", tmp2);
+
+				auto k = cv::waitKey(1);
+
+				if (k == 'f') {
+					toggle_filter = !toggle_filter;
+				} else if (k == 'm') {
+					toggle_median = !toggle_median;
+				} else if (k == 'd') {
+					toggle_mask = !toggle_mask;
+				} else if (k == 'b') {
+					toggle_bilat = !toggle_bilat;
+				}
+
+				return true;
+			});
+
+			++i;
+		}
+	}
+
+	LOG(INFO) << "Start timer";
+	ftl::timer::start(true);
+
+	LOG(INFO) << "Shutting down...";
+	ftl::timer::stop();
+	ftl::pool.stop(true);
+	ctrl.stop();
+	net->shutdown();
+
+	//cudaProfilerStop();
+
+	LOG(INFO) << "Deleting...";
+
+	delete net;
+
+	ftl::config::cleanup();  // Remove any last configurable objects.
+	LOG(INFO) << "Done.";
+}
+
+int main(int argc, char **argv) {
+	run(ftl::configure(argc, argv, "tools_default"));
+}
diff --git a/components/codecs/include/ftl/codecs/depth_convert_cuda.hpp b/components/codecs/include/ftl/codecs/depth_convert_cuda.hpp
index 8b38773dd3b6762d935740c9118b49c8c5a11567..7c4011dfbeb279680fe0cf25fb5cfbe7df93a574 100644
--- a/components/codecs/include/ftl/codecs/depth_convert_cuda.hpp
+++ b/components/codecs/include/ftl/codecs/depth_convert_cuda.hpp
@@ -10,6 +10,8 @@ void depth_to_vuya(const cv::cuda::PtrStepSz<float> &depth, const cv::cuda::PtrS
 
 void vuya_to_depth(const cv::cuda::PtrStepSz<float> &depth, const cv::cuda::PtrStepSz<ushort4> &rgba, float maxdepth, cv::cuda::Stream stream);
 
+void smooth_y(const cv::cuda::PtrStepSz<ushort4> &rgba, cv::cuda::Stream stream);
+
 }
 }
 
diff --git a/components/codecs/src/depth_convert.cu b/components/codecs/src/depth_convert.cu
index 94fa5e4beb4ba300624630073aa37ffc7b1dae2e..d11a78a3da1fa7dfd1b899cc629605d383b2c4b3 100644
--- a/components/codecs/src/depth_convert.cu
+++ b/components/codecs/src/depth_convert.cu
@@ -30,7 +30,9 @@ __global__ void depth_to_vuya_kernel(cv::cuda::PtrStepSz<float> depth, cv::cuda:
 	const unsigned int y = blockIdx.y*blockDim.y + threadIdx.y;
 
 	if (x < depth.cols && y < depth.rows) {
-        float d = max(0.0f,min(maxdepth,depth(y,x)));
+		//float d = max(0.0f,min(maxdepth,depth(y,x)));
+		float d = max(0.0f,depth(y,x));
+		if (d >= maxdepth) d = 0.0f;
         float L = d / maxdepth;
         const float p = P;
         
@@ -62,6 +64,11 @@ void ftl::cuda::depth_to_vuya(const cv::cuda::PtrStepSz<float> &depth, const cv:
  *
  */
 
+ __device__ inline ushort round8(ushort v) {
+     return (v >> 8) + ((v >> 7) & 0x1);  // Note: Make no PSNR difference
+     //return v >> 8;
+ }
+
  // Video is assumed to be 10bit encoded, returning ushort instead of uchar.
 __global__ void vuya_to_depth_kernel(cv::cuda::PtrStepSz<float> depth, cv::cuda::PtrStepSz<ushort4> rgba, float maxdepth) {
 	const unsigned int x = blockIdx.x*blockDim.x + threadIdx.x;
@@ -71,9 +78,9 @@ __global__ void vuya_to_depth_kernel(cv::cuda::PtrStepSz<float> depth, cv::cuda:
 		ushort4 in = rgba(y,x);
 
 		// Only the top 8 bits contain any data
-        float L = float(in.z >> 8) / 255.0f;
-        float Ha = float(in.y >> 8) / 255.0f;
-		float Hb = float(in.x >> 8) / 255.0f;
+        float L = float(round8(in.z)) / 255.0f;
+        float Ha = float(round8(in.y)) / 255.0f;
+		float Hb = float(round8(in.x)) / 255.0f;
 
         const float p = P;
         
@@ -97,3 +104,108 @@ void ftl::cuda::vuya_to_depth(const cv::cuda::PtrStepSz<float> &depth, const cv:
 	vuya_to_depth_kernel<<<gridSize, blockSize, 0, cv::cuda::StreamAccessor::getStream(stream)>>>(depth, rgba, maxdepth);
 	cudaSafeCall( cudaGetLastError() );
 }
+
+// ==== Decode filters =========================================================
+
+ // Video is assumed to be 10bit encoded, returning ushort instead of uchar.
+ template <int RADIUS>
+ __global__ void discon_y_kernel(cv::cuda::PtrStepSz<ushort4> vuya) {
+	const int x = blockIdx.x*blockDim.x + threadIdx.x;
+	const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+	if (x >= RADIUS && x < vuya.cols-RADIUS && y >= RADIUS && y < vuya.rows-RADIUS) {
+        ushort4 in = vuya(y,x);
+        ushort inY = round8(in.z);
+        bool isdiscon = false;
+
+        #pragma unroll
+        for (int v=-1; v<=1; ++v) {
+            #pragma unroll
+            for (int u=-1; u<=1; ++u) {
+                ushort inn = round8(vuya(y+v,x+u).z);
+                isdiscon |= (abs(int(inY)-int(inn)) > 1);
+            }
+        }
+
+		if (isdiscon) vuya(y,x).w = 1;
+		/*if (isdiscon) {
+			#pragma unroll
+			for (int v=-RADIUS; v<=RADIUS; ++v) {
+				#pragma unroll
+				for (int u=-RADIUS; u<=RADIUS; ++u) {
+					vuya(y+v,x+u).w = 1;
+				}
+			}
+		}*/
+	}
+}
+
+ // Video is assumed to be 10bit encoded, returning ushort instead of uchar.
+ template <int RADIUS>
+ __global__ void smooth_y_kernel(cv::cuda::PtrStepSz<ushort4> vuya) {
+	const int x = blockIdx.x*blockDim.x + threadIdx.x;
+	const int y = blockIdx.y*blockDim.y + threadIdx.y;
+
+	if (x >= RADIUS && y >= RADIUS && x < vuya.cols-RADIUS-1 && y < vuya.rows-RADIUS-1) {
+        ushort4 in = vuya(y,x);
+        bool isdiscon = false;
+		//int minerr = 65000;
+		ushort best = in.z;
+		//ushort miny = 65000;
+
+		//float sumY = 0.0f;
+		//float weights = 0.0f;
+		float mcost = 1.e10f;
+
+		// 1) In small radius, is there a discontinuity?
+		
+		if (in.w == 1) {
+			//vuya(y,x).z = 30000;
+			//return;
+
+			#pragma unroll
+			for (int v=-RADIUS; v<=RADIUS; ++v) {
+				#pragma unroll
+				for (int u=-RADIUS; u<=RADIUS; ++u) {
+					ushort4 inn = vuya(y+v,x+u);
+					if (inn.w == 0) {
+						float err = fabsf(float(in.z) - float(inn.z));
+						float dist = v*v + u*u;
+						float cost = err*err; //err*err*dist;
+						if (mcost > cost) {
+							mcost = cost;
+							best = inn.z;
+						}
+						//minerr = min(minerr, err);
+						//if (err == minerr) best = inn.z;
+						//miny = min(miny, inn.z);
+						//sumY += float(in.z);
+						//weights += 1.0f;
+					}
+				}
+			}
+
+			//printf("Min error: %d\n",minerr);
+		
+			vuya(y,x).z = best; //ushort(sumY / weights);
+		}
+        
+		// 2) If yes, use minimum Y value
+		// This acts only to remove discon values... instead a correction is needed
+		// Weight based on distance from discon and difference from current value
+		//     - points further from discon are more reliable
+		//     - most similar Y is likely to be correct depth.
+		//     - either use Y with strongest weight or do weighted average.
+        //if (isdiscon) in.z = maxy;
+        //if (isdiscon) vuya(y,x) = in;
+	}
+}
+
+void ftl::cuda::smooth_y(const cv::cuda::PtrStepSz<ushort4> &rgba, cv::cuda::Stream stream) {
+	const dim3 gridSize((rgba.cols + T_PER_BLOCK - 1)/T_PER_BLOCK, (rgba.rows + T_PER_BLOCK - 1)/T_PER_BLOCK);
+    const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
+
+    discon_y_kernel<1><<<gridSize, blockSize, 0, cv::cuda::StreamAccessor::getStream(stream)>>>(rgba);
+	smooth_y_kernel<6><<<gridSize, blockSize, 0, cv::cuda::StreamAccessor::getStream(stream)>>>(rgba);
+	cudaSafeCall( cudaGetLastError() );
+}
diff --git a/components/operators/include/ftl/operators/depth.hpp b/components/operators/include/ftl/operators/depth.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..b7ceba8dab56c1f168c6f6fc3bf29e005ff07b33
--- /dev/null
+++ b/components/operators/include/ftl/operators/depth.hpp
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <ftl/config.h>
+#include <ftl/operators/operator.hpp>
+#include <opencv2/core.hpp>
+
+
+namespace ftl {
+namespace operators {
+
+class DepthBilateralFilter : public::ftl::operators::Operator {
+	public:
+	explicit DepthBilateralFilter(ftl::Configurable*);
+	DepthBilateralFilter(ftl::Configurable*, const std::tuple<ftl::codecs::Channel> &);
+
+	~DepthBilateralFilter() {};
+
+	inline ftl::operators::Operator::Type type() const override { return ftl::operators::Operator::Type::OneToOne; }
+	bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, cudaStream_t stream) override;
+
+	private:
+	cv::cuda::GpuMat disp_int_;
+	cv::cuda::GpuMat disp_int_result_;
+	double scale_;
+	int radius_;
+	int iter_;
+	float edge_disc_;
+	float max_disc_;
+	float sigma_range_;
+	ftl::codecs::Channel channel_;
+
+    cv::cuda::GpuMat table_color_;
+	cv::cuda::GpuMat table_space_;
+};
+
+}
+}
diff --git a/components/operators/src/depth.cpp b/components/operators/src/depth.cpp
index 8983f33add79c6a3d6239c4660580a04299aa6da..0fdbd60b6b06660e9abe1d5220ee1977088da81a 100644
--- a/components/operators/src/depth.cpp
+++ b/components/operators/src/depth.cpp
@@ -6,10 +6,105 @@
 #include "ftl/operators/filling.hpp"
 #include "ftl/operators/segmentation.hpp"
 #include "ftl/operators/disparity.hpp"
+#include "ftl/operators/depth.hpp"
 #include "ftl/operators/mask.hpp"
 
+#include "./disparity/opencv/disparity_bilateral_filter.hpp"
+
 using ftl::operators::DepthChannel;
+using ftl::operators::DepthBilateralFilter;
 using ftl::codecs::Channel;
+using cv::Mat;
+using cv::cuda::GpuMat;
+
+static void calc_color_weighted_table(GpuMat& table_color, float sigma_range, int len)
+{
+	Mat cpu_table_color(1, len, CV_32F);
+
+	float* line = cpu_table_color.ptr<float>();
+
+	for(int i = 0; i < len; i++)
+		line[i] = static_cast<float>(std::exp(-double(i * i) / (2 * sigma_range * sigma_range)));
+
+	table_color.upload(cpu_table_color);
+}
+
+static void calc_space_weighted_filter(GpuMat& table_space, int win_size, float dist_space)
+{
+	int half = (win_size >> 1);
+
+	Mat cpu_table_space(half + 1, half + 1, CV_32F);
+
+	for (int y = 0; y <= half; ++y)
+	{
+		float* row = cpu_table_space.ptr<float>(y);
+		for (int x = 0; x <= half; ++x)
+			row[x] = exp(-sqrt(float(y * y) + float(x * x)) / dist_space);
+	}
+
+	table_space.upload(cpu_table_space);
+}
+
+// ==== Depth Bilateral Filter =================================================
+
+DepthBilateralFilter::DepthBilateralFilter(ftl::Configurable* cfg) :
+		ftl::operators::Operator(cfg) {
+	
+	scale_ = 16.0;
+	radius_ = cfg->value("radius", 7);
+	iter_ = cfg->value("iter", 2);
+	sigma_range_ = 10.0f;
+	edge_disc_ = cfg->value("edge_discontinuity", 0.04f);
+	max_disc_ = cfg->value("max_discontinuity", 0.1f);
+	channel_ = Channel::Depth;
+	
+
+	calc_color_weighted_table(table_color_, sigma_range_, 255);
+    calc_space_weighted_filter(table_space_, radius_ * 2 + 1, radius_ + 1.0f);
+}
+
+DepthBilateralFilter::DepthBilateralFilter(ftl::Configurable* cfg, const std::tuple<ftl::codecs::Channel> &p) :
+		ftl::operators::Operator(cfg) {
+	
+	scale_ = 16.0;
+	radius_ = cfg->value("radius", 7);
+	iter_ = cfg->value("iter", 2);
+	sigma_range_ = 10.0f;
+	edge_disc_ = cfg->value("edge_discontinuity", 0.04f);
+	max_disc_ = cfg->value("max_discontinuity", 0.1f);
+	channel_ = std::get<0>(p);
+	
+
+	calc_color_weighted_table(table_color_, sigma_range_, 255);
+    calc_space_weighted_filter(table_space_, radius_ * 2 + 1, radius_ + 1.0f);
+}
+
+bool DepthBilateralFilter::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out,
+									 cudaStream_t stream) {
+
+	if (!in.hasChannel(Channel::Colour)) {
+		LOG(ERROR) << "Joint Bilateral Filter is missing Colour";
+		return false;
+	} else if (!in.hasChannel(Channel::Depth)) {
+		LOG(ERROR) << "Joint Bilateral Filter is missing Depth";
+		return false;
+	}
+
+	auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
+	const GpuMat &rgb = in.get<GpuMat>(Channel::Colour);
+	GpuMat &depth = in.get<GpuMat>(channel_);
+
+	ftl::cuda::device::disp_bilateral_filter::disp_bilateral_filter<float>(depth, rgb, rgb.channels(), iter_,
+			table_color_.ptr<float>(), (float *)table_space_.data, table_space_.step / sizeof(float),
+			radius_, edge_disc_, max_disc_, stream);
+
+	//disp_in.convertTo(disp_int_, CV_16SC1, scale_, cvstream);
+	//filter_->apply(disp_in, rgb, disp_out, cvstream);
+	//disp_int_result_.convertTo(disp_out, disp_in.type(), 1.0/scale_, cvstream);
+	return true;
+}
+
+// =============================================================================
 
 DepthChannel::DepthChannel(ftl::Configurable *cfg) : ftl::operators::Operator(cfg) {
 	pipe_ = nullptr;
diff --git a/components/operators/src/disparity/opencv/disparity_bilateral_filter.cpp b/components/operators/src/disparity/opencv/disparity_bilateral_filter.cpp
index 4479828083208d3bd6a4dfc37981d588d5365978..05b928e9c8c3e5fb5e544517bb07bbda3796a8d1 100644
--- a/components/operators/src/disparity/opencv/disparity_bilateral_filter.cpp
+++ b/components/operators/src/disparity/opencv/disparity_bilateral_filter.cpp
@@ -156,8 +156,8 @@ namespace
     {
         using namespace ftl::cuda::device::disp_bilateral_filter;
 
-        const short edge_disc = std::max<short>(short(1), short(ndisp * edge_threshold + 0.5));
-        const short max_disc = short(ndisp * max_disc_threshold + 0.5);
+        const T edge_disc = std::max<T>(T(1), T(ndisp * edge_threshold + 0.5));
+        const T max_disc = T(ndisp * max_disc_threshold + 0.5);
 
         size_t table_space_step = table_space.step / sizeof(float);
 
diff --git a/components/operators/src/disparity/opencv/disparity_bilateral_filter.cu b/components/operators/src/disparity/opencv/disparity_bilateral_filter.cu
index 134095101766f0780685b5838522864873135a71..7d2a90bacf865ef4d2010a8fdb6c97fa3f301c53 100644
--- a/components/operators/src/disparity/opencv/disparity_bilateral_filter.cu
+++ b/components/operators/src/disparity/opencv/disparity_bilateral_filter.cu
@@ -77,12 +77,18 @@ namespace ftl { namespace cuda { namespace device
             }
 		};
 
+		template <typename T>
+		__device__ inline T Abs(T v) { return ::abs(v); }
+
+		template <>
+		__device__ inline float Abs<float>(float v) { return fabsf(v); }
+
         template <int channels, typename T>
         __global__ void disp_bilateral_filter(int t, T* disp, size_t disp_step,
             const uchar* img, size_t img_step, int h, int w,
             const float* ctable_color, const float * ctable_space, size_t ctable_space_step,
             int cradius,
-            short cedge_disc, short cmax_disc)
+            T cedge_disc, T cmax_disc)
         {
             const int y = blockIdx.y * blockDim.y + threadIdx.y;
             const int x = ((blockIdx.x * blockDim.x + threadIdx.x) << 1) + ((y + t) & 1);
@@ -97,7 +103,7 @@ namespace ftl { namespace cuda { namespace device
                 dp[3] = *(disp + (y+1) * disp_step + x + 0);
                 dp[4] = *(disp + (y  ) * disp_step + x + 1);
 
-                if(::abs(dp[1] - dp[0]) >= cedge_disc || ::abs(dp[2] - dp[0]) >= cedge_disc || ::abs(dp[3] - dp[0]) >= cedge_disc || ::abs(dp[4] - dp[0]) >= cedge_disc)
+                if(Abs(dp[1] - dp[0]) >= cedge_disc || Abs(dp[2] - dp[0]) >= cedge_disc || Abs(dp[3] - dp[0]) >= cedge_disc || Abs(dp[4] - dp[0]) >= cedge_disc)
                 {
                     const int ymin = ::max(0, y - cradius);
                     const int xmin = ::max(0, x - cradius);
@@ -122,11 +128,11 @@ namespace ftl { namespace cuda { namespace device
 
                             const T disp_reg = disp_y[xi];
 
-                            cost[0] += ::min(cmax_disc, ::abs(disp_reg - dp[0])) * weight;
-                            cost[1] += ::min(cmax_disc, ::abs(disp_reg - dp[1])) * weight;
-                            cost[2] += ::min(cmax_disc, ::abs(disp_reg - dp[2])) * weight;
-                            cost[3] += ::min(cmax_disc, ::abs(disp_reg - dp[3])) * weight;
-                            cost[4] += ::min(cmax_disc, ::abs(disp_reg - dp[4])) * weight;
+                            cost[0] += ::min(cmax_disc, Abs(disp_reg - dp[0])) * weight;
+                            cost[1] += ::min(cmax_disc, Abs(disp_reg - dp[1])) * weight;
+                            cost[2] += ::min(cmax_disc, Abs(disp_reg - dp[2])) * weight;
+                            cost[3] += ::min(cmax_disc, Abs(disp_reg - dp[3])) * weight;
+                            cost[4] += ::min(cmax_disc, Abs(disp_reg - dp[4])) * weight;
                         }
                     }
 
@@ -165,7 +171,7 @@ namespace ftl { namespace cuda { namespace device
         }
 
         template <typename T>
-        void disp_bilateral_filter(cv::cuda::PtrStepSz<T> disp, cv::cuda::PtrStepSzb img, int channels, int iters, const float *table_color, const float* table_space, size_t table_step, int radius, short edge_disc, short max_disc, cudaStream_t stream)
+        void disp_bilateral_filter(cv::cuda::PtrStepSz<T> disp, cv::cuda::PtrStepSzb img, int channels, int iters, const float *table_color, const float* table_space, size_t table_step, int radius, T edge_disc, T max_disc, cudaStream_t stream)
         {
             dim3 threads(32, 8, 1);
             dim3 grid(1, 1, 1);
@@ -212,8 +218,9 @@ namespace ftl { namespace cuda { namespace device
                 cudaSafeCall( cudaDeviceSynchronize() );
         }
 
-        template void disp_bilateral_filter<uchar>(cv::cuda::PtrStepSz<uchar> disp, cv::cuda::PtrStepSzb img, int channels, int iters, const float *table_color, const float *table_space, size_t table_step, int radius, short, short, cudaStream_t stream);
-        template void disp_bilateral_filter<short>(cv::cuda::PtrStepSz<short> disp, cv::cuda::PtrStepSzb img, int channels, int iters, const float *table_color, const float *table_space, size_t table_step, int radius, short, short, cudaStream_t stream);
+        template void disp_bilateral_filter<uchar>(cv::cuda::PtrStepSz<uchar> disp, cv::cuda::PtrStepSzb img, int channels, int iters, const float *table_color, const float *table_space, size_t table_step, int radius, uchar, uchar, cudaStream_t stream);
+		template void disp_bilateral_filter<short>(cv::cuda::PtrStepSz<short> disp, cv::cuda::PtrStepSzb img, int channels, int iters, const float *table_color, const float *table_space, size_t table_step, int radius, short, short, cudaStream_t stream);
+		template void disp_bilateral_filter<float>(cv::cuda::PtrStepSz<float> disp, cv::cuda::PtrStepSzb img, int channels, int iters, const float *table_color, const float *table_space, size_t table_step, int radius, float, float, cudaStream_t stream);
     } // namespace bilateral_filter
 }}} // namespace ftl { namespace cuda { namespace cudev
 
diff --git a/components/operators/src/disparity/opencv/disparity_bilateral_filter.hpp b/components/operators/src/disparity/opencv/disparity_bilateral_filter.hpp
index 1f39d07f0e7023b813951c940a895924ddb480b3..b6c0d79fa3342dda8a6674d1290e07b9737c055a 100644
--- a/components/operators/src/disparity/opencv/disparity_bilateral_filter.hpp
+++ b/components/operators/src/disparity/opencv/disparity_bilateral_filter.hpp
@@ -3,6 +3,6 @@ namespace ftl { namespace cuda { namespace device
     namespace disp_bilateral_filter
     {
         template<typename T>
-        void disp_bilateral_filter(cv::cuda::PtrStepSz<T> disp, cv::cuda::PtrStepSzb img, int channels, int iters, const float *, const float *, size_t, int radius, short edge_disc, short max_disc, cudaStream_t stream);
+        void disp_bilateral_filter(cv::cuda::PtrStepSz<T> disp, cv::cuda::PtrStepSzb img, int channels, int iters, const float *, const float *, size_t, int radius, T edge_disc, T max_disc, cudaStream_t stream);
     }
 }}}
diff --git a/components/operators/src/mask.cu b/components/operators/src/mask.cu
index 91ddf19dd3b6451d7802cc622a08a660adfdd360..84011d897d065b9e42968dc0d531b95c789686d0 100644
--- a/components/operators/src/mask.cu
+++ b/components/operators/src/mask.cu
@@ -70,7 +70,7 @@ __global__ void cull_discontinuity_kernel(ftl::cuda::TextureObject<int> mask, ft
 
 	if (x < depth.width() && y < depth.height()) {
 		Mask m(mask.tex2D((int)x,(int)y));
-		if (m.isDiscontinuity()) depth(x,y) = 1000.0f;
+		if (m.isDiscontinuity()) depth(x,y) = 0.0f;
 	}
 }
 
diff --git a/components/renderers/cpp/src/screen.cu b/components/renderers/cpp/src/screen.cu
index 8610359d2bbec8eaa6f6972116a5f58cf6433b7a..c4991ee6e65dd6801acd847a67a4713be8edfd13 100644
--- a/components/renderers/cpp/src/screen.cu
+++ b/components/renderers/cpp/src/screen.cu
@@ -14,31 +14,36 @@ using ftl::render::SplatParams;
  */
  __global__ void screen_coord_kernel(TextureObject<float> depth,
         TextureObject<float> depth_out,
-		TextureObject<short2> screen_out, SplatParams params, float4x4 pose, Camera camera) {
+		TextureObject<short2> screen_out, Camera vcamera, float4x4 pose, Camera camera) {
 	const int x = blockIdx.x*blockDim.x + threadIdx.x;
 	const int y = blockIdx.y*blockDim.y + threadIdx.y;
 
-    uint2 screenPos = make_uint2(30000,30000);
-    screen_out(x,y) = make_short2(screenPos.x, screenPos.y);
-
-    const float d = depth.tex2D(x, y);
-	const float3 worldPos = pose * camera.screenToCam(x,y,d);
-	if (d < camera.minDepth || d > camera.maxDepth) return;
-
-    // Find the virtual screen position of current point
-	const float3 camPos = params.m_viewMatrix * worldPos;
-    screenPos = params.camera.camToScreen<uint2>(camPos);
-
-    if (camPos.z < params.camera.minDepth || camPos.z > params.camera.maxDepth || screenPos.x >= params.camera.width || screenPos.y >= params.camera.height)
-        screenPos = make_uint2(30000,30000);
-    screen_out(x,y) = make_short2(screenPos.x, screenPos.y);
-    depth_out(x,y) = camPos.z;
+	if (x >= 0 && y >= 0 && x < depth.width() && y < depth.height()) {
+		uint2 screenPos = make_uint2(30000,30000);
+		//screen_out(x,y) = make_short2(screenPos.x, screenPos.y);
+
+		const float d = depth.tex2D(x, y);
+		//const float3 worldPos = pose * camera.screenToCam(x,y,d);
+		//if (d < camera.minDepth || d > camera.maxDepth) return;
+
+		// Find the virtual screen position of current point
+		const float3 camPos =  (d >= camera.minDepth && d <= camera.maxDepth) ? pose * camera.screenToCam(x,y,d) : make_float3(0.0f,0.0f,0.0f); // worldPos;  // params.m_viewMatrix *
+		screenPos = vcamera.camToScreen<uint2>(camPos);
+
+		if (	camPos.z < vcamera.minDepth ||
+				camPos.z > vcamera.maxDepth ||
+				screenPos.x >= vcamera.width ||
+				screenPos.y >= vcamera.height)
+			screenPos = make_uint2(30000,30000);
+		screen_out(x,y) = make_short2(screenPos.x, screenPos.y);
+		depth_out(x,y) = camPos.z;
+	}
 }
 
 void ftl::cuda::screen_coord(TextureObject<float> &depth, TextureObject<float> &depth_out, TextureObject<short2> &screen_out, const SplatParams &params, const float4x4 &pose, const Camera &camera, cudaStream_t stream) {
     const dim3 gridSize((depth.width() + T_PER_BLOCK - 1)/T_PER_BLOCK, (depth.height() + T_PER_BLOCK - 1)/T_PER_BLOCK);
     const dim3 blockSize(T_PER_BLOCK, T_PER_BLOCK);
 
-	screen_coord_kernel<<<gridSize, blockSize, 0, stream>>>(depth, depth_out, screen_out, params, pose, camera);
+	screen_coord_kernel<<<gridSize, blockSize, 0, stream>>>(depth, depth_out, screen_out, params.camera, pose, camera);
     cudaSafeCall( cudaGetLastError() );
 }
diff --git a/components/renderers/cpp/src/tri_render.cpp b/components/renderers/cpp/src/tri_render.cpp
index 23934f5f2bd8a06291d6ba9a3834d518ec15b1ce..3f0f2cbd8778a9887ab1ca34840a4d1eac87ba3e 100644
--- a/components/renderers/cpp/src/tri_render.cpp
+++ b/components/renderers/cpp/src/tri_render.cpp
@@ -339,14 +339,15 @@ void Triangular::_mesh(ftl::rgbd::Frame &out, const Eigen::Matrix4d &t, cudaStre
 			continue;
 		}
 
-		auto pose = MatrixConversion::toCUDA(t.cast<float>() * f.getPose().cast<float>());
+		//auto pose = MatrixConversion::toCUDA(t.cast<float>() * f.getPose().cast<float>());
+		auto transform = params_.m_viewMatrix * MatrixConversion::toCUDA(t.cast<float>() * f.getPose().cast<float>());
 
 		// Calculate and save virtual view screen position of each source pixel
 		ftl::cuda::screen_coord(
 			f.createTexture<float>(Channel::Depth),
 			f.createTexture<float>(Channel::Depth2, Format<float>(f.get<GpuMat>(Channel::Depth).size())),
 			f.createTexture<short2>(Channel::Screen, Format<short2>(f.get<GpuMat>(Channel::Depth).size())),
-			params_, pose, f.getLeftCamera(), stream
+			params_, transform, f.getLeftCamera(), stream
 		);
 
 		// Must reset depth channel if blending
diff --git a/components/streams/src/filestream.cpp b/components/streams/src/filestream.cpp
index 784b14fb686d49ff22f30f7ee133b63b3997f1b8..0d7004dd7e3b63f33ea0d1b9939a9a156418f2dd 100644
--- a/components/streams/src/filestream.cpp
+++ b/components/streams/src/filestream.cpp
@@ -225,7 +225,7 @@ bool File::begin(bool dorun) {
 		// Capture current time to adjust timestamps
 		timestart_ = (ftl::timer::get_time() / ftl::timer::getInterval()) * ftl::timer::getInterval();
 		active_ = true;
-		interval_ = 50; //ftl::timer::getInterval();
+		interval_ = 50; //ftl::timer::getInterval();  // FIXME: Use correct interval!!
 		timestamp_ = timestart_;
 
 		tick(timestart_); // Do some now!
diff --git a/components/streams/src/receiver.cpp b/components/streams/src/receiver.cpp
index fbe2321f35dea3f37194889c72662208355785a5..4cb7329462a7271ba4a53714216fb362999c804e 100644
--- a/components/streams/src/receiver.cpp
+++ b/components/streams/src/receiver.cpp
@@ -61,6 +61,7 @@ void Receiver::_createDecoder(InternalVideoStates &frame, int chan, const ftl::c
 
 Receiver::InternalVideoStates::InternalVideoStates() {
 	for (int i=0; i<32; ++i) decoders[i] = nullptr;
+	timestamp = -1;
 }
 
 Receiver::InternalVideoStates &Receiver::_getVideoFrame(const StreamPacket &spkt, int ix) {
@@ -140,11 +141,6 @@ void Receiver::_processAudio(const StreamPacket &spkt, const Packet &pkt) {
 }
 
 void Receiver::_processVideo(const StreamPacket &spkt, const Packet &pkt) {
-	//ftl::cuda::setDevice();
-	//int dev;
-	//cudaSafeCall(cudaGetDevice(&dev));
-	//LOG(INFO) << "Cuda device = " << dev;
-
 	const ftl::codecs::Channel rchan = spkt.channel;
 	const unsigned int channum = (unsigned int)spkt.channel;
 	InternalVideoStates &iframe = _getVideoFrame(spkt);
@@ -153,41 +149,15 @@ void Receiver::_processVideo(const StreamPacket &spkt, const Packet &pkt) {
 	int width = ftl::codecs::getWidth(pkt.definition);
 	int height = ftl::codecs::getHeight(pkt.definition);
 
-	for (int i=0; i<pkt.frame_count; ++i) {
-		InternalVideoStates &frame = _getVideoFrame(spkt,i);
-
-		// Packets are for unwanted channel.
-		//if (rchan != Channel::Colour && rchan != chan) return;
-
-		if (frame.frame.hasChannel(spkt.channel)) {
-			UNIQUE_LOCK(frame.mutex, lk);
-			//if (frame.frame.hasChannel(spkt.channel)) {
-				// FIXME: Is this a corruption in recording or in playback?
-				// Seems to occur in same place in ftl file, one channel is missing
-				LOG(WARNING) << "Previous frame not complete: " << frame.timestamp;
-				//LOG(ERROR) << " --- " << (string)spkt;
-				//frame.frame.reset();
-				//frame.completed.clear();
-			//}
-		}
-		frame.timestamp = spkt.timestamp;
-
-		// Add channel to frame and allocate memory if required
-		const cv::Size size = cv::Size(width, height);
-		frame.frame.create<cv::cuda::GpuMat>(spkt.channel).create(size, (isFloatChannel(rchan) ? CV_32FC1 : CV_8UC4));
-	}
-
-	Packet tmppkt = pkt;
-	iframe.frame.pushPacket(spkt.channel, tmppkt);
-
 	//LOG(INFO) << " CODEC = " << (int)pkt.codec << " " << (int)pkt.flags << " " << (int)spkt.channel;
 	//LOG(INFO) << "Decode surface: " << (width*tx) << "x" << (height*ty);
 
 	auto &surface = iframe.surface[static_cast<int>(spkt.channel)];
 
+	// Allocate a decode surface, this is a tiled image to be split later
 	surface.create(height*ty, width*tx, ((isFloatChannel(spkt.channel)) ? ((pkt.flags & 0x2) ? CV_16UC4 : CV_16U) : CV_8UC4));
 
-	// Do the actual decode
+	// Find or create the decoder
 	_createDecoder(iframe, channum, pkt);
 	auto *decoder = iframe.decoders[channum];
 	if (!decoder) {
@@ -195,15 +165,15 @@ void Receiver::_processVideo(const StreamPacket &spkt, const Packet &pkt) {
 		return;
 	}
 
+	// Do the actual decode into the surface buffer
 	try {
-		//LOG(INFO) << "TYPE = " << frame.frame.get<cv::cuda::GpuMat>(spkt.channel).type();
 		if (!decoder->decode(pkt, surface)) {
 			LOG(ERROR) << "Decode failed";
-			//return;
+			return;
 		}
 	} catch (std::exception &e) {
 		LOG(ERROR) << "Decode failed for " << spkt.timestamp << ": " << e.what();
-		//return;
+		return;
 	}
 
 	auto cvstream = cv::cuda::StreamAccessor::wrapStream(decoder->stream());
@@ -215,15 +185,51 @@ void Receiver::_processVideo(const StreamPacket &spkt, const Packet &pkt) {
 	cv::waitKey(1);
 	}*/
 
+	bool apply_Y_filter = value("apply_Y_filter", true);
+
+	// Now split the tiles from surface into frames, doing colour conversions
+	// at the same time.
 	for (int i=0; i<pkt.frame_count; ++i) {
 		InternalVideoStates &frame = _getVideoFrame(spkt,i);
 
+		if (frame.frame.hasChannel(spkt.channel)) {
+			// FIXME: Is this a corruption in recording or in playback?
+			// Seems to occur in same place in ftl file, one channel is missing
+			LOG(WARNING) << "Previous frame not complete: " << frame.timestamp;
+		}
+
+		{
+			// This ensures that if previous frames are unfinished then they
+			// are discarded.
+			UNIQUE_LOCK(frame.mutex, lk);
+			if (frame.timestamp != spkt.timestamp && frame.timestamp != -1) {
+				frame.frame.reset();
+				frame.completed.clear();
+				LOG(WARNING) << "Frames out-of-phase by: " << spkt.timestamp - frame.timestamp;
+			}
+			frame.timestamp = spkt.timestamp;
+		}
+
+		// Add channel to frame and allocate memory if required
+		const cv::Size size = cv::Size(width, height);
+		frame.frame.create<cv::cuda::GpuMat>(spkt.channel).create(size, (isFloatChannel(rchan) ? CV_32FC1 : CV_8UC4));
+
 		cv::Rect roi((i % tx)*width, (i / tx)*height, width, height);
 		cv::cuda::GpuMat sroi = surface(roi);
 		
 		// Do colour conversion
 		if (isFloatChannel(rchan) && (pkt.flags & 0x2)) {
-			//LOG(INFO) << "VUYA Convert";
+			// Smooth Y channel around discontinuities
+			// Lerp the uv channels / smooth over a small kernal size.
+
+			if (value("apply_median", false)) {
+				cv::Mat tmp;
+				sroi.download(tmp);
+				cv::medianBlur(tmp, tmp, 5);
+				sroi.upload(tmp);
+			}
+
+			if (apply_Y_filter) ftl::cuda::smooth_y(sroi, cvstream);
 			ftl::cuda::vuya_to_depth(frame.frame.get<cv::cuda::GpuMat>(spkt.channel), sroi, 16.0f, cvstream);
 		} else if (isFloatChannel(rchan)) {
 			sroi.convertTo(frame.frame.get<cv::cuda::GpuMat>(spkt.channel), CV_32FC1, 1.0f/1000.0f, cvstream);
@@ -232,28 +238,29 @@ void Receiver::_processVideo(const StreamPacket &spkt, const Packet &pkt) {
 		}
 	}
 
+	Packet tmppkt = pkt;
+	iframe.frame.pushPacket(spkt.channel, tmppkt);
+
+	// Must ensure all processing is finished before completing a frame.
 	cudaSafeCall(cudaStreamSynchronize(decoder->stream()));
 
 	for (int i=0; i<pkt.frame_count; ++i) {
 		InternalVideoStates &frame = _getVideoFrame(spkt,i);
-		
-		frame.completed += spkt.channel;
-	
-		// Complete if all requested frames are found
-		auto sel = stream_->selected(spkt.frameSetID());
 
-		if ((frame.completed & sel) == sel) {
-			UNIQUE_LOCK(frame.mutex, lk);
+		auto sel = stream_->selected(spkt.frameSetID());
+		
+		UNIQUE_LOCK(frame.mutex, lk);
+		if (frame.timestamp == spkt.timestamp) {
+			frame.completed += spkt.channel;
+			
+			// Complete if all requested channels are found
 			if ((frame.completed & sel) == sel) {
 				timestamp_ = frame.timestamp;
 
 				//LOG(INFO) << "BUILDER PUSH: " << timestamp_ << ", " << spkt.frameNumber() << ", " << (int)pkt.frame_count;
 
 				if (frame.state.getLeft().width == 0) {
-					LOG(WARNING) << "Missing calibration, skipping frame";
-					//frame.frame.reset();
-					//frame.completed.clear();
-					//return;
+					LOG(WARNING) << "Missing calibration for frame";
 				}
 
 				// TODO: Have multiple builders for different framesets.
@@ -266,7 +273,10 @@ void Receiver::_processVideo(const StreamPacket &spkt, const Packet &pkt) {
 
 				frame.frame.reset();
 				frame.completed.clear();
+				frame.timestamp = -1;
 			}
+		} else {
+			LOG(ERROR) << "Frame timestamps mistmatch";
 		}
 	}
 }
diff --git a/components/streams/test/receiver_unit.cpp b/components/streams/test/receiver_unit.cpp
index faee6add4a30f4195a4d135db900413976913833..97e266d682c147347be18b36dd37c2ed42201a0c 100644
--- a/components/streams/test/receiver_unit.cpp
+++ b/components/streams/test/receiver_unit.cpp
@@ -219,3 +219,87 @@ TEST_CASE( "Receiver generating onFrameSet" ) {
 	delete receiver;
 	//ftl::config::cleanup();
 }
+
+TEST_CASE( "Receiver sync bugs" ) {
+	json_t global = json_t{{"$id","ftl://test"}};
+	ftl::config::configure(global);
+
+	json_t cfg = json_t{
+		{"$id","ftl://test/1"}
+	};
+	auto *receiver = ftl::create<Receiver>(cfg);
+
+	json_t cfg2 = json_t{
+		{"$id","ftl://test/2"}
+	};
+	TestStream stream(cfg2);
+	receiver->setStream(&stream);
+
+	ftl::codecs::NvPipeEncoder encoder(definition_t::HD1080, definition_t::SD480);
+
+	ftl::codecs::Packet pkt;
+	pkt.codec = codec_t::Any;
+	pkt.bitrate = 255;
+	pkt.definition = definition_t::Any;
+	pkt.flags = 0;
+	pkt.frame_count = 1;
+
+	ftl::codecs::StreamPacket spkt;
+	spkt.version = 4;
+	spkt.timestamp = 10;
+	spkt.frame_number = 0;
+	spkt.channel = Channel::Colour;
+	spkt.streamID = 0;
+
+	ftl::rgbd::Frame dummy;
+	ftl::rgbd::FrameState state;
+	state.getLeft().width = 1280;
+	state.getLeft().height = 720;
+	dummy.setOrigin(&state);
+	ftl::stream::injectCalibration(&stream, dummy, 0, 0);
+
+	ftl::timer::start(false);
+
+	stream.select(0, Channel::Colour + Channel::Colour2);
+
+	SECTION("out of phase packets") {
+		cv::cuda::GpuMat m(cv::Size(1280,720), CV_8UC4, cv::Scalar(0));
+
+		bool r = encoder.encode(m, pkt);
+		REQUIRE( r );
+
+		int count = 0;
+		int64_t ts = 0;
+		bool haswrongchan = false;
+		receiver->onFrameSet([&count,&ts,&haswrongchan](ftl::rgbd::FrameSet &fs) {
+			++count;
+
+			ts = fs.timestamp;
+			haswrongchan = fs.frames[0].hasChannel(Channel::ColourHighRes);
+
+			return true;
+		});
+
+		stream.post(spkt, pkt);
+		spkt.timestamp = 10;
+		spkt.channel = Channel::ColourHighRes;
+		stream.post(spkt, pkt);
+		spkt.timestamp = 20;
+		spkt.channel = Channel::Colour2;
+		stream.post(spkt, pkt);
+		spkt.timestamp = 20;
+		spkt.channel = Channel::Colour;
+		stream.post(spkt, pkt);
+
+		int i=10;
+		while (i-- > 0 && count < 1) std::this_thread::sleep_for(std::chrono::milliseconds(10));
+
+		REQUIRE( count == 1 );
+		REQUIRE( ts == 20 );
+		REQUIRE( !haswrongchan );
+	}
+
+	ftl::timer::stop(true);
+	//while (ftl::pool.n_idle() != ftl::pool.size()) std::this_thread::sleep_for(std::chrono::milliseconds(10));
+	delete receiver;
+}