diff --git a/applications/vision/src/main.cpp b/applications/vision/src/main.cpp
index 7128b93d30863ce9e6709c3d0fe9dd7e74391113..f99754cbf558bb3d0f53fde37763855fac7af128 100644
--- a/applications/vision/src/main.cpp
+++ b/applications/vision/src/main.cpp
@@ -48,32 +48,6 @@ using std::chrono::milliseconds;
 using cv::Mat;
 using json = nlohmann::json;
 
-namespace ftl {
-void disparityToDepth(const cv::Mat &disparity, cv::Mat &depth, const cv::Mat &q) {
-	cv::Matx44d _Q;
-    q.convertTo(_Q, CV_64F);
-
-	if (depth.empty()) depth = cv::Mat(disparity.size(), CV_32F);
-
-	for( int y = 0; y < disparity.rows; y++ ) {
-		const float *sptr = disparity.ptr<float>(y);
-		float *dptr = depth.ptr<float>(y);
-
-		for( int x = 0; x < disparity.cols; x++ ) {
-			double d = sptr[x];
-			cv::Vec4d homg_pt = _Q*cv::Vec4d(x, y, d, 1.0);
-			//dptr[x] = Vec3d(homg_pt.val);
-			//dptr[x] /= homg_pt[3];
-			dptr[x] = (homg_pt[2] / homg_pt[3]); // Depth in meters
-
-			if( fabs(d) <= FLT_EPSILON )
-				dptr[x] = 1000.0f;
-		}
-	}
-}
-};
-
-
 static void run(ftl::Configurable *root) {
 	Universe *net = ftl::create<Universe>(root, "net");
 	ftl::ctrl::Slave slave(net, root);
diff --git a/components/operators/CMakeLists.txt b/components/operators/CMakeLists.txt
index da52eac6a1e754a3dec32a8f10ed424ef0214760..bdb1f1ade8aecb2dd5c4d4516bb2ebcfffd4dadd 100644
--- a/components/operators/CMakeLists.txt
+++ b/components/operators/CMakeLists.txt
@@ -13,6 +13,8 @@ add_library(ftloperators
 	src/disparity/optflow_smoothing.cpp
 	src/disparity/disp2depth.cu
 	src/disparity/disparity_to_depth.cpp
+	src/disparity/fixstars_sgm.cpp
+	src/disparity/bilateral_filter.cpp
 )
 
 # These cause errors in CI build and are being removed from PCL in newer versions
diff --git a/components/operators/include/ftl/operators/disparity.hpp b/components/operators/include/ftl/operators/disparity.hpp
index 7588a883f056f344afa728a482e50a0ce5874338..5a9533a57f8d9e5ce328d5d8a7cd82a97625105f 100644
--- a/components/operators/include/ftl/operators/disparity.hpp
+++ b/components/operators/include/ftl/operators/disparity.hpp
@@ -3,9 +3,57 @@
 #include <ftl/operators/operator.hpp>
 #include <opencv2/cudaoptflow.hpp>
 
+#include <libsgm.h>
+
 namespace ftl {
 namespace operators {
 
+/*
+ * FixstarsSGM https://github.com/fixstars/libSGM
+ * 
+ * Requires modified version https://gitlab.utu.fi/joseha/libsgm
+ * 
+ */
+class FixstarsSGM : public ftl::operators::Operator {
+	public:
+	explicit FixstarsSGM(ftl::Configurable* cfg);
+
+	~FixstarsSGM();
+	inline Operator::Type type() const override { return Operator::Type::OneToOne; }
+	bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *src, cudaStream_t stream) override;
+	
+	private:
+	bool init();
+
+	sgm::StereoSGM *ssgm_;
+	cv::Size size_;
+	cv::cuda::GpuMat lbw_;
+	cv::cuda::GpuMat rbw_;
+	cv::cuda::GpuMat disp_int_;
+
+	int P1_;
+	int P2_;
+	int max_disp_;
+	float uniqueness_;
+};
+
+class DisparityBilateralFilter : public::ftl::operators::Operator {
+	public:
+	explicit DisparityBilateralFilter(ftl::Configurable*);
+
+	~DisparityBilateralFilter() {};
+
+	inline Operator::Type type() const override { return Operator::Type::OneToOne; }
+	bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *src, cudaStream_t stream) override;
+
+	private:
+	cv::Ptr<cv::cuda::DisparityBilateralFilter> filter_;
+	cv::cuda::GpuMat disp_int_;
+	int radius_;
+	int iter_;
+	int max_disp_;
+};
+
 /*
  * Calculate depth from disparity
  */
@@ -32,7 +80,7 @@ class DisparitySmoothingOF : public ftl::operators::Operator {
 	bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *src, cudaStream_t stream) override;
 
 	private:
-	void init();
+	bool init();
 
 	const ftl::codecs::Channel channel_ = ftl::codecs::Channel::Depth;
 	cv::cuda::GpuMat history_;
diff --git a/components/operators/include/ftl/operators/opticalflow.hpp b/components/operators/include/ftl/operators/opticalflow.hpp
index 70df5e49e0dd5ef52b3cccaba5f6017703e2fbbd..81afc3914b3a3d31975deca14383f319588209d5 100644
--- a/components/operators/include/ftl/operators/opticalflow.hpp
+++ b/components/operators/include/ftl/operators/opticalflow.hpp
@@ -20,7 +20,7 @@ class NVOpticalFlow : public ftl::operators::Operator {
 	bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *src, cudaStream_t stream) override;
 
 	protected:
-	void init();
+	bool init();
 
 	private:
 	cv::Size size_;
diff --git a/components/operators/src/disparity/bilateral_filter.cpp b/components/operators/src/disparity/bilateral_filter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..73ded1af88aad541fa313e438596d6fcec476cb8
--- /dev/null
+++ b/components/operators/src/disparity/bilateral_filter.cpp
@@ -0,0 +1,16 @@
+#pragma once
+
+#include <ftl/operators/disparity.hpp>
+
+using cv::cuda::GpuMat;
+using cv::Size;
+
+using ftl::operators::DisparityBilateralFilter;
+
+DisparityBilateralFilter::DisparityBilateralFilter(ftl::Configurable* cfg) :
+		ftl::operators::Operator(cfg) {
+	
+	radius_ = cfg->value("radius", 7);
+	iter_ = cfg->value("iter", 13);
+	filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_ << 4, radius_, iter_);
+}
\ No newline at end of file
diff --git a/components/operators/src/disparity/disparity_to_depth.cpp b/components/operators/src/disparity/disparity_to_depth.cpp
index baed8acfdc8689d695daaf044a471a29cd02221e..9f793c23463a5113fb12aa47a4ef3ff4aa9e4b8b 100644
--- a/components/operators/src/disparity/disparity_to_depth.cpp
+++ b/components/operators/src/disparity/disparity_to_depth.cpp
@@ -9,10 +9,14 @@ using cv::cuda::GpuMat;
 bool DisparityToDepth::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out,
 							 ftl::rgbd::Source *src, cudaStream_t stream) {
 	
+	if (!in.hasChannel(Channel::Disparity)) { return false;  }
+
 	const auto params = src->parameters();
 	const GpuMat &disp = in.get<GpuMat>(Channel::Disparity);
-	GpuMat depth = out.get<GpuMat>(Channel::Depth);
-	
+
+	GpuMat depth = out.create<GpuMat>(Channel::Depth);
+	depth.create(disp.size(), CV_32FC1);
+
 	ftl::cuda::disparity_to_depth(disp, depth, params, stream);
 	return true;
 }
\ No newline at end of file
diff --git a/components/operators/src/disparity/fixstars_sgm.cpp b/components/operators/src/disparity/fixstars_sgm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..80d8761bad19740485b8d63eb8dbfae8a70b0bfc
--- /dev/null
+++ b/components/operators/src/disparity/fixstars_sgm.cpp
@@ -0,0 +1,97 @@
+#include <loguru.hpp>
+
+#include "ftl/operators/disparity.hpp"
+
+using cv::Size;
+using cv::cuda::GpuMat;
+
+using ftl::rgbd::Format;
+using ftl::codecs::Channel;
+using ftl::rgbd::Frame;
+using ftl::rgbd::Source;
+using ftl::operators::FixstarsSGM;
+
+FixstarsSGM::FixstarsSGM(ftl::Configurable* cfg) :
+		ftl::operators::Operator(cfg) {
+	
+	ssgm_ = nullptr;
+	size_ = Size(0, 0);
+
+	uniqueness_ = cfg->value("uniqueness", 0.95f);
+	P1_ = cfg->value("P1", 10);
+	P2_ = cfg->value("P2", 120);
+	max_disp_ = cfg->value("max_disp", 256);
+
+	if (uniqueness_ < 0.0 || uniqueness_ > 1.0) {
+		uniqueness_ = 1.0;
+		LOG(WARNING) << "Invalid uniqueness, using default (1.0)";
+	}
+
+	if (P1_ <= 0 ) {
+		P1_ = 10;
+		LOG(WARNING) << "Invalid value for P1, using default (10)";
+	}
+
+	if (P2_ > P1_) {
+		P2_ = P1_;
+		LOG(WARNING) << "Invalid value for P2, using value of P1 (" << P1_ << ")";
+	}
+
+	if (!(max_disp_ == 256 || max_disp_ == 128)) {
+		max_disp_ = 256;
+		LOG(WARNING) << "Invalid value for max_disp, using default value (256)";
+	}
+}
+
+FixstarsSGM::~FixstarsSGM() {
+	if (ssgm_) {
+		delete ssgm_;
+	}
+}
+
+bool FixstarsSGM::init() {
+	if (size_ == Size(0, 0)) { return false; }
+	if (ssgm_) { delete ssgm_; }
+	lbw_.create(size_, CV_8UC1);
+	rbw_.create(size_, CV_8UC1);
+	disp_int_.create(size_, CV_16SC1);
+
+	ssgm_ = new sgm::StereoSGM(size_.width, size_.height, max_disp_, 8, 16,
+		lbw_.step, disp_int_.step / sizeof(short),
+		sgm::EXECUTE_INOUT_CUDA2CUDA,
+		sgm::StereoSGM::Parameters(P1_, P2_, uniqueness_, true)
+	);
+
+	return true;
+}
+
+bool FixstarsSGM::apply(Frame &in, Frame &out, Source *src, cudaStream_t stream) {
+	if (!in.hasChannel(Channel::Left) || !in.hasChannel(Channel::Right)) {
+		return false;
+	}
+
+	const auto &l = in.get<GpuMat>(Channel::Left);
+	const auto &r = in.get<GpuMat>(Channel::Right);
+	
+	if (l.size() != size_) {
+		size_ = l.size();
+		if (!init()) { return false; }
+	}
+
+	auto &disp = out.create<GpuMat>(Channel::Disparity, Format<float>(l.size()));
+
+	auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
+	cv::cuda::cvtColor(l, lbw_, cv::COLOR_BGR2GRAY, 0, cvstream);
+	cv::cuda::cvtColor(r, rbw_, cv::COLOR_BGR2GRAY, 0, cvstream);
+
+	cvstream.waitForCompletion();
+	ssgm_->execute(lbw_.data, rbw_.data, disp_int_.data);
+
+	// GpuMat left_pixels(dispt_, cv::Rect(0, 0, max_disp_, dispt_.rows));
+	// left_pixels.setTo(0);
+
+	cv::cuda::threshold(disp_int_, disp_int_, 4096.0f, 0.0f, cv::THRESH_TOZERO_INV, cvstream);
+	
+	disp_int_.convertTo(disp, CV_32F, 1.0f / 16.0f, cvstream);
+	return true;
+}
diff --git a/components/operators/src/disparity/optflow_smoothing.cpp b/components/operators/src/disparity/optflow_smoothing.cpp
index 8dffdaa4cf3c9e288faa38b4eb1e6e9a38a37a75..6dd9ff11e2ac62c045f731fc43b3d61399bf376d 100644
--- a/components/operators/src/disparity/optflow_smoothing.cpp
+++ b/components/operators/src/disparity/optflow_smoothing.cpp
@@ -68,14 +68,15 @@ DisparitySmoothingOF::DisparitySmoothingOF(ftl::Configurable* cfg) :
 
 DisparitySmoothingOF::~DisparitySmoothingOF() {}
 
-void DisparitySmoothingOF::init() {
-	if (size_ == Size(0, 0)) { return; }
+bool DisparitySmoothingOF::init() {
+	if (size_ == Size(0, 0)) { return false; }
 	history_.create(cv::Size(size_.width * n_max_, size_.height), CV_32FC1);
 	history_.setTo(0.0);
+	return true;
 }
 
 bool DisparitySmoothingOF::apply(Frame &in, Frame &out, Source *src, cudaStream_t stream) {
-	if (!out.hasChannel(channel_) || !in.hasChannel(Channel::Flow)) { return true; }
+	if (!out.hasChannel(channel_) || !in.hasChannel(Channel::Flow)) { return false; }
 
 	auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
 	const cv::cuda::GpuMat &optflow = in.get<cv::cuda::GpuMat>(Channel::Flow);
@@ -83,7 +84,7 @@ bool DisparitySmoothingOF::apply(Frame &in, Frame &out, Source *src, cudaStream_
 	
 	if (data.size() != size_) {
 		size_ = data.size();
-		init();
+		if (!init()) { return false; }
 	}
 
 	ftl::cuda::optflow_filter(data, optflow, history_, n_max_, threshold_, cvstream);
diff --git a/components/operators/src/nvopticalflow.cpp b/components/operators/src/nvopticalflow.cpp
index 5d8239365b11d052069e26de93de9cca2a244435..a8b6d406bc7421d48d0cfb15de40d0eb48cafebe 100644
--- a/components/operators/src/nvopticalflow.cpp
+++ b/components/operators/src/nvopticalflow.cpp
@@ -17,7 +17,7 @@ NVOpticalFlow::NVOpticalFlow(ftl::Configurable* cfg) :
 NVOpticalFlow::~NVOpticalFlow() {
 }
 
-void NVOpticalFlow::init() {
+bool NVOpticalFlow::init() {
 	nvof_ = cv::cuda::NvidiaOpticalFlow_1_0::create(
 				size_.width, size_.height, 
 				cv::cuda::NvidiaOpticalFlow_1_0::NV_OF_PERF_LEVEL_SLOW,
@@ -25,12 +25,11 @@ void NVOpticalFlow::init() {
 	
 	left_gray_.create(size_, CV_8UC1);
 	left_gray_prev_.create(size_, CV_8UC1);
+	return true;
 }
 
 bool NVOpticalFlow::apply(Frame &in, Frame &out, Source *src, cudaStream_t stream) {
-	if (!in.hasChannel(channel_in_)) {
-		return true; // false?
-	}
+	if (!in.hasChannel(channel_in_)) { return false; }
 
 	if (in.get<GpuMat>(channel_in_).size() != size_) {
 		size_ = in.get<GpuMat>(channel_in_).size();
diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt
index 1e0f0de201a91b01289e888510b655152e3af311..6027ab17a7d64110a72dd51e6cfde098c0b0abc7 100644
--- a/components/rgbd-sources/CMakeLists.txt
+++ b/components/rgbd-sources/CMakeLists.txt
@@ -1,12 +1,11 @@
 set(RGBDSRC
 	src/sources/stereovideo/calibrate.cpp
 	src/sources/stereovideo/local.cpp
-	src/disparity.cpp
 	src/source.cpp
 	src/frame.cpp
 	src/frameset.cpp
 	src/sources/stereovideo/stereovideo.cpp
-	src/sources/middlebury/middlebury_source.cpp
+#	src/sources/middlebury/middlebury_source.cpp
 	src/sources/net/net.cpp
 	src/streamer.cpp
 	src/colour.cpp
@@ -33,9 +32,9 @@ if (LibArchive_FOUND)
 	)
 endif (LibArchive_FOUND)
 
-if (LIBSGM_FOUND)
-	list(APPEND RGBDSRC "src/algorithms/fixstars_sgm.cpp")
-endif (LIBSGM_FOUND)
+#if (LIBSGM_FOUND)
+#	list(APPEND RGBDSRC "src/algorithms/fixstars_sgm.cpp")
+#endif (LIBSGM_FOUND)
 
 if (CUDA_FOUND)
 	list(APPEND RGBDSRC
diff --git a/components/rgbd-sources/src/source.cpp b/components/rgbd-sources/src/source.cpp
index b0ee77a449e33b82641c18aa0c7a039ed42e8626..13cdd5487edf0b7cbc99f7cd9dd7032b43d31185 100644
--- a/components/rgbd-sources/src/source.cpp
+++ b/components/rgbd-sources/src/source.cpp
@@ -104,8 +104,8 @@ ftl::rgbd::detail::Source *Source::_createFileImpl(const ftl::URI &uri) {
 		if (ftl::is_directory(path)) {
 			if (ftl::is_file(path + "/video.mp4")) {
 				return new StereoVideoSource(this, path);
-			} else if (ftl::is_file(path + "/im0.png")) {
-				return new MiddleburySource(this, path);
+//			} else if (ftl::is_file(path + "/im0.png")) {
+//				return new MiddleburySource(this, path);
 			} else {
 				LOG(ERROR) << "Directory is not a valid RGBD source: " << path;
 			}
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
index 473e66a49e59a7b5d810bb797169dac06c18a03a..58e374dcb8877119daf92ce38438e533c96c9234 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -33,7 +33,6 @@ StereoVideoSource::StereoVideoSource(ftl::rgbd::Source *host, const string &file
 }
 
 StereoVideoSource::~StereoVideoSource() {
-	delete disp_;
 	delete calib_;
 	delete lsrc_;
 }
@@ -126,12 +125,10 @@ void StereoVideoSource::init(const string &file) {
 	#endif
 
 	pipeline_depth_ = ftl::config::create<ftl::operators::Graph>(host_, "pipeline_disparity");
+	pipeline_depth_->append<ftl::operators::FixstarsSGM>("algorithm");
+	pipeline_depth_->append<ftl::operators::DisparitySmoothingOF>("optflow_filter");
 	pipeline_depth_->append<ftl::operators::DisparityToDepth>("calculate_depth");
 
-	disp_ = Disparity::create(host_, "disparity");
-	if (!disp_) LOG(FATAL) << "Unknown disparity algorithm : " << *host_->get<ftl::config::json_t>("disparity");
-	disp_->setMask(mask_l_);
-
 	LOG(INFO) << "StereoVideo source ready...";
 	ready_ = true;
 }
@@ -171,7 +168,7 @@ bool StereoVideoSource::retrieve() {
 	auto &right = frame.create<cv::cuda::GpuMat>(Channel::Right);
 	lsrc_->get(left, right, calib_, stream2_);
 
-	pipeline_input_->apply(frame, frame, (ftl::rgbd::Source*) this, cv::cuda::StreamAccessor::getStream(stream2_));
+	pipeline_input_->apply(frame, frame, host_, cv::cuda::StreamAccessor::getStream(stream2_));
 	stream2_.waitForCompletion();
 	
 	return true;
@@ -186,39 +183,31 @@ void StereoVideoSource::swap() {
 bool StereoVideoSource::compute(int n, int b) {
 	auto &frame = frames_[1];
 	
-	pipeline_depth_->apply(frame, frame, host_, cv::cuda::StreamAccessor::getStream(stream_));
-
-	auto &left = frame.get<cv::cuda::GpuMat>(Channel::Left);
-	auto &right = frame.get<cv::cuda::GpuMat>(Channel::Right);
-
 	const ftl::codecs::Channel chan = host_->getChannel();
-	if (left.empty() || right.empty()) return false;
+	if (!frame.hasChannel(Channel::Left) || !frame.hasChannel(Channel::Right)) {
+		return false;
+	}
 
 	if (chan == Channel::Depth) {
-		disp_->compute(frame, stream_);
-		
-		auto &disp = frame.get<cv::cuda::GpuMat>(Channel::Disparity);
-		auto &depth = frame.create<cv::cuda::GpuMat>(Channel::Depth);
-		if (depth.empty()) depth = cv::cuda::GpuMat(left.size(), CV_32FC1);
-		pipeline_depth_->apply(frame, frame, (ftl::rgbd::Source*) this, cv::cuda::StreamAccessor::getStream(stream_));
-		//ftl::cuda::disparity_to_depth(disp, depth, params_, stream_);
-		
-		//left.download(rgb_, stream_);
-		//depth.download(depth_, stream_);
-		//frame.download(Channel::Left + Channel::Depth);
+		pipeline_depth_->apply(frame, frame, host_, cv::cuda::StreamAccessor::getStream(stream_));	
 		stream_.waitForCompletion();
-		host_->notify(timestamp_, left, depth);
+		host_->notify(timestamp_,
+						frame.get<cv::cuda::GpuMat>(Channel::Left),
+						frame.get<cv::cuda::GpuMat>(Channel::Depth));
+
 	} else if (chan == Channel::Right) {
-		//left.download(rgb_, stream_);
-		//right.download(depth_, stream_);
-		stream_.waitForCompletion();  // TODO:(Nick) Move to getFrames
-		host_->notify(timestamp_, left, right);
+		stream_.waitForCompletion(); // TODO:(Nick) Move to getFrames
+		host_->notify(timestamp_,
+						frame.get<cv::cuda::GpuMat>(Channel::Left),
+						frame.get<cv::cuda::GpuMat>(Channel::Right));
+	
 	} else {
-		//left.download(rgb_, stream_);
-		stream_.waitForCompletion();  // TODO:(Nick) Move to getFrames
-		//LOG(INFO) << "NO SECOND CHANNEL: " << (bool)depth_.empty();
+		stream_.waitForCompletion(); // TODO:(Nick) Move to getFrames
+		auto &left = frame.get<cv::cuda::GpuMat>(Channel::Left);
 		depth_.create(left.size(), left.type());
-		host_->notify(timestamp_, left, depth_);
+		host_->notify(timestamp_,
+						frame.get<cv::cuda::GpuMat>(Channel::Left),
+						depth_);
 	}
 
 	return true;
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp
index 44014a88d5b33c47a94adb363cb039211faa87b9..78fcdbcf809eeae0d5de6da30b99ce3dc07f9214 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.hpp
@@ -37,7 +37,6 @@ class StereoVideoSource : public detail::Source {
 	private:
 	LocalSource *lsrc_;
 	Calibrate *calib_;
-	Disparity *disp_;
 
 	ftl::operators::Graph *pipeline_input_;
 	ftl::operators::Graph *pipeline_depth_;