diff --git a/components/operators/include/ftl/operators/disparity.hpp b/components/operators/include/ftl/operators/disparity.hpp
index 5a9533a57f8d9e5ce328d5d8a7cd82a97625105f..efa3a5128142e0d7cb9b5d5deb1c2e7dc376a38c 100644
--- a/components/operators/include/ftl/operators/disparity.hpp
+++ b/components/operators/include/ftl/operators/disparity.hpp
@@ -49,6 +49,8 @@ class DisparityBilateralFilter : public::ftl::operators::Operator {
 	private:
 	cv::Ptr<cv::cuda::DisparityBilateralFilter> filter_;
 	cv::cuda::GpuMat disp_int_;
+	cv::cuda::GpuMat disp_int_result_;
+	double scale_;
 	int radius_;
 	int iter_;
 	int max_disp_;
diff --git a/components/operators/src/disparity/bilateral_filter.cpp b/components/operators/src/disparity/bilateral_filter.cpp
index 73ded1af88aad541fa313e438596d6fcec476cb8..8489e6a4b36b5e46b825e5a7323024d024e5b626 100644
--- a/components/operators/src/disparity/bilateral_filter.cpp
+++ b/components/operators/src/disparity/bilateral_filter.cpp
@@ -5,12 +5,32 @@
 using cv::cuda::GpuMat;
 using cv::Size;
 
+using ftl::codecs::Channel;
 using ftl::operators::DisparityBilateralFilter;
 
 DisparityBilateralFilter::DisparityBilateralFilter(ftl::Configurable* cfg) :
 		ftl::operators::Operator(cfg) {
 	
+	scale_ = 16.0;
 	radius_ = cfg->value("radius", 7);
 	iter_ = cfg->value("iter", 13);
 	filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_ << 4, radius_, iter_);
+}
+
+bool DisparityBilateralFilter::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out,
+									 ftl::rgbd::Source *src, cudaStream_t stream) {
+
+	if (!in.hasChannel(Channel::Disparity) || !in.hasChannel(Channel::Colour)) {
+		return false;
+	}
+	auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
+	const GpuMat &rgb = in.get<GpuMat>(Channel::Colour);
+	GpuMat &disp_in = in.get<GpuMat>(Channel::Disparity);
+	GpuMat &disp_out = out.create<GpuMat>(Channel::Disparity);
+	disp_out.create(disp_in.size(), disp_in.type());
+
+	disp_in.convertTo(disp_int_, CV_16SC1, scale_, cvstream);
+	filter_->apply(disp_int_, rgb, disp_int_result_, cvstream);
+	disp_int_result_.convertTo(disp_out, 1.0/scale_, cvstream);
+	return true;
 }
\ 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 9f793c23463a5113fb12aa47a4ef3ff4aa9e4b8b..5951802f0adc960d387a20a4f7f77abd264fb6e8 100644
--- a/components/operators/src/disparity/disparity_to_depth.cpp
+++ b/components/operators/src/disparity/disparity_to_depth.cpp
@@ -14,7 +14,7 @@ bool DisparityToDepth::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out,
 	const auto params = src->parameters();
 	const GpuMat &disp = in.get<GpuMat>(Channel::Disparity);
 
-	GpuMat depth = out.create<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);
diff --git a/components/operators/src/disparity/fixstars_sgm.cpp b/components/operators/src/disparity/fixstars_sgm.cpp
index 80d8761bad19740485b8d63eb8dbfae8a70b0bfc..831ee7a21f7eafcc70e7713bb968e1b642d5a5c1 100644
--- a/components/operators/src/disparity/fixstars_sgm.cpp
+++ b/components/operators/src/disparity/fixstars_sgm.cpp
@@ -32,7 +32,7 @@ FixstarsSGM::FixstarsSGM(ftl::Configurable* cfg) :
 		LOG(WARNING) << "Invalid value for P1, using default (10)";
 	}
 
-	if (P2_ > P1_) {
+	if (P2_ < P1_) {
 		P2_ = P1_;
 		LOG(WARNING) << "Invalid value for P2, using value of P1 (" << P1_ << ")";
 	}
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
index 58e374dcb8877119daf92ce38438e533c96c9234..aaff4ec7b6b610ca3d16f13c13e682c4110c7712 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -126,6 +126,7 @@ void StereoVideoSource::init(const string &file) {
 
 	pipeline_depth_ = ftl::config::create<ftl::operators::Graph>(host_, "pipeline_disparity");
 	pipeline_depth_->append<ftl::operators::FixstarsSGM>("algorithm");
+	pipeline_depth_->append<ftl::operators::DisparityBilateralFilter>("bilateral_filter");
 	pipeline_depth_->append<ftl::operators::DisparitySmoothingOF>("optflow_filter");
 	pipeline_depth_->append<ftl::operators::DisparityToDepth>("calculate_depth");