operator: bilateral filter

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_;

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

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);

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_ << ")";
 	}

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");