Implements #272 Disparity pipeline in reconstruct

applications/gui/src/camera.cpp

@@ -14,6 +14,7 @@ using ftl::gui::GLTexture;
 using ftl::gui::PoseWindow;
 using ftl::codecs::Channel;
 using ftl::codecs::Channels;
+using cv::cuda::GpuMat;
 
 // TODO(Nick) MOVE
 class StatisticsImage {
@@ -156,20 +157,22 @@ ftl::gui::Camera::Camera(ftl::gui::Screen *screen, ftl::rgbd::Source *src) : scr
 	posewin_->setTheme(screen->windowtheme);
 	posewin_->setVisible(false);
 
-	src->setCallback([this](int64_t ts, cv::cuda::GpuMat &channel1, cv::cuda::GpuMat &channel2) {
+	src->setCallback([this](int64_t ts, ftl::rgbd::Frame &frame) {
 		UNIQUE_LOCK(mutex_, lk);
-		im1_.create(channel1.size(), channel1.type());
-		im2_.create(channel2.size(), channel2.type());
-
-		//cv::swap(channel1, im1_);
-		//cv::swap(channel2, im2_);
 
+		auto &channel1 = frame.get<GpuMat>(Channel::Colour);
+		im1_.create(channel1.size(), channel1.type());
 		channel1.download(im1_);
-		channel2.download(im2_);
-		
+
 		// OpenGL (0,0) bottom left
 		cv::flip(im1_, im1_, 0);
-		cv::flip(im2_, im2_, 0);
+
+		if (channel_ != Channel::Colour && channel_ != Channel::None && frame.hasChannel(channel_)) {
+			auto &channel2 = frame.get<GpuMat>(channel_);
+			im2_.create(channel2.size(), channel2.type());
+			channel2.download(im2_);
+			cv::flip(im2_, im2_, 0);
+		}
 	});
 }
 
@@ -330,6 +333,7 @@ static void visualizeDepthMap(	const cv::Mat &depth, cv::Mat &out,
 	
 	applyColorMap(out, out, cv::COLORMAP_JET);
 	out.setTo(cv::Scalar(255, 255, 255), mask);
+	cv::cvtColor(out,out, cv::COLOR_BGR2BGRA);
 }
 
 static void visualizeEnergy(	const cv::Mat &depth, cv::Mat &out,
@@ -343,6 +347,7 @@ static void visualizeEnergy(	const cv::Mat &depth, cv::Mat &out,
 	
 	applyColorMap(out, out, cv::COLORMAP_JET);
 	//out.setTo(cv::Scalar(255, 255, 255), mask);
+	cv::cvtColor(out,out, cv::COLOR_BGR2BGRA);
 }
 
 static void drawEdges(	const cv::Mat &in, cv::Mat &out,
@@ -353,8 +358,8 @@ static void drawEdges(	const cv::Mat &in, cv::Mat &out,
 	cv::Laplacian(in, edges, 8, ksize);
 	cv::threshold(edges, edges, threshold, 255, threshold_type);
 
-	cv::Mat edges_color(in.size(), CV_8UC3);
-	cv::addWeighted(edges, weight, out, 1.0, 0.0, out, CV_8UC3);
+	cv::Mat edges_color(in.size(), CV_8UC4);
+	cv::addWeighted(edges, weight, out, 1.0, 0.0, out, CV_8UC4);
 }
 
 cv::Mat ftl::gui::Camera::visualizeActiveChannel() {
@@ -497,7 +502,7 @@ const GLTexture &ftl::gui::Camera::captureFrame() {
 			//case Channel::Flow:
 			case Channel::ColourNormals:
 			case Channel::Right:
-					if (im2_.rows == 0 || im2_.type() != CV_8UC3) { break; }
+					if (im2_.rows == 0 || im2_.type() != CV_8UC4) { break; }
 					texture2_.update(im2_);
 					break;
 

applications/gui/src/gltexture.cpp

@@ -19,7 +19,7 @@ void GLTexture::update(cv::Mat &m) {
 		glGenTextures(1, &glid_);
 		glBindTexture(GL_TEXTURE_2D, glid_);
 		//cv::Mat m(cv::Size(100,100), CV_8UC3);
-		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB8, m.cols, m.rows, 0, GL_BGR, GL_UNSIGNED_BYTE, m.data);
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, m.cols, m.rows, 0, GL_BGRA, GL_UNSIGNED_BYTE, m.data);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
@@ -27,7 +27,7 @@ void GLTexture::update(cv::Mat &m) {
 	} else {
 		glBindTexture(GL_TEXTURE_2D, glid_);
 		// TODO Allow for other formats
-		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB8, m.cols, m.rows, 0, GL_BGR, GL_UNSIGNED_BYTE, m.data);
+		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA8, m.cols, m.rows, 0, GL_BGRA, GL_UNSIGNED_BYTE, m.data);
 	}
 	auto err = glGetError();
 	if (err != 0) LOG(ERROR) << "OpenGL Texture error: " << err;

applications/gui/src/screen.cpp

@@ -54,6 +54,7 @@ namespace {
 		in vec2 uv;
 		void main() {
 			color = blendAmount * texture(image1, uv) + (1.0 - blendAmount) * texture(image2, uv);
+			color.w = 1.0f;
 		})";
 }
 

applications/reconstruct/src/main.cpp

@@ -101,6 +101,8 @@ static void run(ftl::Configurable *root) {
 		sourcecounts.push_back(configuration_size);
 	}
 
+	ftl::codecs::Channels channels;
+
 	// Check paths for FTL files to load.
 	auto paths = (*root->get<nlohmann::json>("paths"));
 	for (auto &x : paths.items()) {
@@ -117,8 +119,9 @@ static void run(ftl::Configurable *root) {
 			reader.begin();
 
 			int max_stream = 0;
-			reader.read(reader.getStartTime()+100, [&max_stream](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
+			reader.read(reader.getStartTime()+100, [&max_stream,&channels](const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt) {
 				max_stream = max(max_stream, spkt.streamID);
+				if ((int)spkt.channel < 32) channels |= spkt.channel;
 			});
 			reader.end();
 
@@ -135,6 +138,8 @@ static void run(ftl::Configurable *root) {
 		}
 	}
 
+	if (channels.empty()) channels |= Channel::Depth;
+
 	// Create a vector of all input RGB-Depth sources
 	auto sources = ftl::createArray<Source>(root, "sources", net);
 
@@ -191,6 +196,11 @@ static void run(ftl::Configurable *root) {
 		std::string id = std::to_string(cumulative);
 		auto reconstr = ftl::create<ftl::Reconstruction>(root, id, id);
 		for (size_t i=cumulative; i<cumulative+c; i++) {
+			if (channels.has(Channel::Depth)) {
+				sources[i]->setChannel(Channel::Depth);
+			} else if (channels.has(Channel::Right)) {
+				sources[i]->setChannel(Channel::Right);
+			}
 			reconstr->addSource(sources[i]);
 		}
 		groups.push_back(reconstr);
@@ -202,10 +212,18 @@ static void run(ftl::Configurable *root) {
 	renderpipe->append<ftl::operators::FXAA>("antialiasing"); 
 
 	vs->onRender([vs, &groups, &renderpipe](ftl::rgbd::Frame &out) {
+		bool hasFrame = false;
 		for (auto &reconstr : groups) {
-			reconstr->render(vs, out);
+			hasFrame = reconstr->render(vs, out) || hasFrame;
+		}
+
+		if (hasFrame) {
+			renderpipe->apply(out, out, vs, 0);
+			return true;
+		} else {
+			LOG(INFO) << "NO FRAME";
+			return false;
 		}
-		renderpipe->apply(out, out, vs, 0);
 	});
 	stream->add(vs);
 
@@ -313,8 +331,7 @@ static void run(ftl::Configurable *root) {
 	});
 
 	// -------------------------------------------------------------------------
-
-	stream->setLatency(6);  // FIXME: This depends on source!?
+	
 	//stream->add(group);
 	stream->run();
 

applications/reconstruct/src/reconstruction.cpp

@@ -9,6 +9,7 @@
 #include "ftl/operators/antialiasing.hpp"
 #include "ftl/operators/mvmls.hpp"
 #include "ftl/operators/clipping.hpp"
+#include <ftl/operators/disparity.hpp>
 
 using ftl::Reconstruction;
 using ftl::codecs::Channel;
@@ -24,14 +25,17 @@ static Eigen::Affine3d create_rotation_matrix(float ax, float ay, float az) {
 }
 
 Reconstruction::Reconstruction(nlohmann::json &config, const std::string name) :
-	ftl::Configurable(config), busy_(false), fs_render_(), fs_align_() {
+	ftl::Configurable(config), busy_(false), rbusy_(false), new_frame_(false), fs_render_(), fs_align_() {
 	group_ = new ftl::rgbd::Group;
-	group_->setName("ReconGroup-" + name);
+	group_->setName("Reconstruction-" + name);
 	group_->setLatency(4);
 
 	renderer_ = ftl::create<ftl::render::Triangular>(this, "renderer", &fs_render_);
 
 	pipeline_ = ftl::config::create<ftl::operators::Graph>(this, "pre_filters");
+	pipeline_->append<ftl::operators::DepthChannel>("depth");  // Ensure there is a depth channel
+	pipeline_->append<ftl::operators::DisparityBilateralFilter>("bilateral_filter")->set("enabled", false);
+	pipeline_->append<ftl::operators::DisparityToDepth>("calculate_depth")->set("enabled", false);
 	pipeline_->append<ftl::operators::ClipScene>("clipping")->set("enabled", false);
 	pipeline_->append<ftl::operators::ColourChannels>("colour");  // Convert BGR to BGRA
 	//pipeline_->append<ftl::operators::HFSmoother>("hfnoise");  // Remove high-frequency noise
@@ -49,52 +53,46 @@ Reconstruction::Reconstruction(nlohmann::json &config, const std::string name) :
 	group_->sync([this](ftl::rgbd::FrameSet &fs) -> bool {
 		// TODO: pause
 		
-		if (busy_) {
-			LOG(INFO) << "Group frameset dropped: " << fs.timestamp;
+		/*if (busy_) {
+			LOG(WARNING) << "Group frameset dropped: " << fs.timestamp;
 			return true;
 		}
-		busy_ = true;
+		busy_ = true;*/
 
 		// Swap the entire frameset to allow rapid return
-		fs.swapTo(fs_align_);
+		// FIXME: This gets stuck on a mutex
+		//fs.swapTo(fs_align_);
 
-		ftl::pool.push([this](int id) {
-			UNIQUE_LOCK(fs_align_.mtx, lk);
-			
-			/*rgb_.resize(fs_align_.frames.size());
-			for (size_t i = 0; i < rgb_.size(); i++) {
-				auto &depth = fs_align_.frames[i].get<cv::cuda::GpuMat>(ftl::codecs::Channel::Depth);
-				auto &color = fs_align_.frames[i].get<cv::cuda::GpuMat>(ftl::codecs::Channel::Colour);
-
-				if (depth.size() != color.size()) {
-					std::swap(rgb_[i], color);
-					cv::cuda::resize(rgb_[i], color, depth.size(), 0.0, 0.0, cv::INTER_LINEAR);
-				}
-			}*/
-
-			pipeline_->apply(fs_align_, fs_align_, 0);
+		//ftl::pool.push([this](int id) {
+			//UNIQUE_LOCK(fs.mtx, lk);
+
+			pipeline_->apply(fs, fs, 0);
 			
 			// TODO: To use second GPU, could do a download, swap, device change,
 			// then upload to other device. Or some direct device-2-device copy.
-			/*
-			for (size_t i = 0; i < rgb_.size(); i++) {
-				auto &depth = fs_align_.frames[i].get<cv::cuda::GpuMat>(ftl::codecs::Channel::Depth);
-				auto &color = fs_align_.frames[i].get<cv::cuda::GpuMat>(ftl::codecs::Channel::Colour);
-				auto &tmp = rgb_[i];
-
-				// TODO doesn't always work correctly if resolution changes
-				if (!tmp.empty() && (depth.size() != tmp.size())) {
-					std::swap(tmp, color);
-					fs_align_.frames[i].resetTexture(ftl::codecs::Channel::Colour);
-					fs_align_.frames[i].createTexture<uchar4>(ftl::codecs::Channel::Colour, true);
-				}
-			}*/
 
-			fs_align_.swapTo(fs_render_);
+			// FIXME: Need a buffer of framesets
+			/*if (rbusy_) {
+				LOG(WARNING) << "Render frameset dropped: " << fs_align_.timestamp;
+				busy_ = false;
+				//return;
+			}
+			rbusy_ = true;*/
+
+			//LOG(INFO) << "Align complete... " << fs.timestamp;
+
+			// TODO: Reduce mutex locking problem here...
+			{
+				UNIQUE_LOCK(exchange_mtx_, lk);
+				if (new_frame_ == true) LOG(WARNING) << "Frame lost";
+				fs.swapTo(fs_align_);
+				new_frame_ = true;
+			}
 
-			LOG(INFO) << "Align complete... " << fs_align_.timestamp;
-			busy_ = false;
-		});
+			//fs.resetFull();
+
+			//busy_ = false;
+		//});
 		return true;
 	});
 }
@@ -104,7 +102,7 @@ Reconstruction::~Reconstruction() {
 }
 
 void Reconstruction::addSource(ftl::rgbd::Source *src) {
-	src->setChannel(Channel::Depth);
+	//src->setChannel(Channel::Depth);
 	group_->addSource(src); // TODO: check if source is already in group?
 }
 
@@ -112,7 +110,20 @@ void Reconstruction::addRawCallback(const std::function<void(ftl::rgbd::Source *
 	group_->addRawCallback(cb);
 }
 
-void Reconstruction::render(ftl::rgbd::VirtualSource *vs, ftl::rgbd::Frame &out) {
+bool Reconstruction::render(ftl::rgbd::VirtualSource *vs, ftl::rgbd::Frame &out) {
+	{
+		UNIQUE_LOCK(exchange_mtx_, lk);
+		if (new_frame_) {
+			new_frame_ = false;
+			fs_align_.swapTo(fs_render_);
+		}
+	}
+	/*if (fs_render_.stale || fs_render_.timestamp <= 0) {
+		LOG(ERROR) << "STALE FRAME TO RENDER";
+		return false;
+	}
+	fs_render_.stale = true;*/
+
 	// Create scene transform, intended for axis aligning the walls and floor
 	Eigen::Matrix4d transform;
 	//if (getConfig()["transform"].is_object()) {
@@ -134,5 +145,7 @@ void Reconstruction::render(ftl::rgbd::VirtualSource *vs, ftl::rgbd::Frame &out)
 	//}
 
 	Eigen::Affine3d sm = Eigen::Affine3d(Eigen::Scaling(double(value("scale", 1.0f))));
-	renderer_->render(vs, out, sm.matrix() * transform);
+	bool res = renderer_->render(vs, out, sm.matrix() * transform);
+	//fs_render_.resetFull();
+	return res;
 }
\ No newline at end of file

applications/reconstruct/src/reconstruction.hpp

@@ -23,10 +23,13 @@ class Reconstruction : public ftl::Configurable {
 	/**
 	 * Do the render for a specified virtual camera.
 	 */
-	void render(ftl::rgbd::VirtualSource *vs, ftl::rgbd::Frame &out);
+	bool render(ftl::rgbd::VirtualSource *vs, ftl::rgbd::Frame &out);
 
 	private:
 	bool busy_;
+	bool rbusy_;
+	bool new_frame_;
+	MUTEX exchange_mtx_;
 	
 	ftl::rgbd::FrameSet fs_render_;
 	ftl::rgbd::FrameSet fs_align_;

applications/vision/src/main.cpp

@@ -24,6 +24,7 @@
 #include <ftl/net/universe.hpp>
 #include <ftl/master.hpp>
 #include <nlohmann/json.hpp>
+#include <ftl/operators/disparity.hpp>
 
 #include "opencv2/imgproc.hpp"
 #include "opencv2/imgcodecs.hpp"
@@ -54,7 +55,7 @@ static void run(ftl::Configurable *root) {
 
 	auto paths = root->get<vector<string>>("paths");
 	string file = "";
-	if (paths && (*paths).size() > 0) file = (*paths)[0];
+	if (paths && (*paths).size() > 0) file = (*paths)[(*paths).size()-1];
 
 	Source *source = nullptr;
 	source = ftl::create<Source>(root, "source");
@@ -65,6 +66,10 @@ static void run(ftl::Configurable *root) {
 	
 	Streamer *stream = ftl::create<Streamer>(root, "stream", net);
 	stream->add(source);
+
+	auto pipeline = ftl::config::create<ftl::operators::Graph>(root, "pipeline");
+	pipeline->append<ftl::operators::DepthChannel>("depth");  // Ensure there is a depth channel
+	stream->group()->addPipeline(pipeline);
 	
 	net->start();
 

components/codecs/include/ftl/codecs/channels.hpp

@@ -14,7 +14,6 @@ enum struct Channel : int {
 	Depth			= 1,	// 32S or 32F
 	Right			= 2,	// 8UC3 or 8UC4
 	Colour2			= 2,
-	Disparity		= 3,
 	Depth2			= 3,
 	Deviation		= 4,
 	Screen			= 4,
@@ -24,7 +23,6 @@ enum struct Channel : int {
 	Contribution	= 7,	// 32F
 	EnergyVector	= 8,	// 32FC4
 	Flow			= 9,	// 32F
-	Smoothing		= 9,	// 32F
 	Energy			= 10,	// 32F
 	Mask			= 11,	// 32U
 	Density			= 12,	// 32F
@@ -32,7 +30,9 @@ enum struct Channel : int {
 	Support2		= 14,	// 8UC4 (currently)
 	Segmentation	= 15,	// 32S?
 	ColourNormals	= 16,	// 8UC4
-	ColourHighRes	= 20,	// 8UC3 or 8UC4
+	ColourHighRes	= 17,	// 8UC3 or 8UC4
+	Disparity		= 18,
+	Smoothing		= 19,	// 32F
 
 	AudioLeft		= 32,
 	AudioRight		= 33,
@@ -102,6 +102,7 @@ class Channels {
 	}
 	
 	inline size_t count() { return std::bitset<32>(mask).count(); }
+	inline bool empty() { return mask == 0; }
 	inline void clear() { mask = 0; }
 
 	static const size_t kMax = 32;

components/codecs/include/ftl/codecs/opencv_decoder.hpp

@@ -16,7 +16,7 @@ class OpenCVDecoder : public ftl::codecs::Decoder {
 	bool accepts(const ftl::codecs::Packet &pkt);
 
 	private:
-	cv::Mat tmp_;
+	//cv::Mat tmp_;
 };
 
 }

components/codecs/src/channels.cpp

@@ -8,9 +8,9 @@ struct ChannelInfo {
 };
 
 static ChannelInfo info[] = {
-    "Colour", CV_8UC3,
+    "Colour", CV_8UC4,
     "Depth", CV_32F,
-    "Right", CV_8UC3,
+    "Right", CV_8UC4,
     "DepthRight", CV_32F,
     "Deviation", CV_32F,
     "Normals", CV_32FC4,

components/codecs/src/nvpipe_decoder.cpp

@@ -94,9 +94,9 @@ bool NvPipeDecoder::decode(const ftl::codecs::Packet &pkt, cv::cuda::GpuMat &out
 		//if (out.rows == ftl::codecs::getHeight(pkt.definition)) {
 			// Flag 0x1 means frame is in RGB so needs conversion to BGR
 			if (pkt.flags & 0x1) {
-				cv::cuda::cvtColor(tmp_, out, cv::COLOR_RGBA2BGR, 0, stream_);
+				cv::cuda::cvtColor(tmp_, out, cv::COLOR_RGBA2BGRA, 0, stream_);
 			} else {
-				cv::cuda::cvtColor(tmp_, out, cv::COLOR_BGRA2BGR, 0, stream_);
+				//cv::cuda::cvtColor(tmp_, out, cv::COLOR_BGRA2BGR, 0, stream_);
 			}
 		/*} else {
 			LOG(WARNING) << "Resizing decoded frame from " << tmp_.size() << " to " << out.size();

components/codecs/src/opencv_decoder.cpp

@@ -31,8 +31,15 @@ bool OpenCVDecoder::decode(const ftl::codecs::Packet &pkt, cv::cuda::GpuMat &out
 
 	//LOG(INFO) << "DECODE JPEG " << (int)pkt.block_number << "/" << chunk_dim;
 
+	cv::Mat tmp2_, tmp_;
 	// Decode in temporary buffers to prevent long locks
-	cv::imdecode(pkt.data, cv::IMREAD_UNCHANGED, &tmp_);
+	cv::imdecode(pkt.data, cv::IMREAD_UNCHANGED, &tmp2_);
+
+	if (tmp2_.type() == CV_8UC3) {
+		cv::cvtColor(tmp2_, tmp_, cv::COLOR_BGR2BGRA);
+	} else {
+		tmp_ = tmp2_;
+	}
 
 	// Apply colour correction to chunk
 	//ftl::rgbd::colourCorrection(tmp_rgb, gamma_, temperature_);
@@ -45,7 +52,7 @@ bool OpenCVDecoder::decode(const ftl::codecs::Packet &pkt, cv::cuda::GpuMat &out
 		if (!tmp_.empty() && tmp_.type() == CV_16U && chunkHead.type() == CV_32F) {
 			tmp_.convertTo(tmp_, CV_32FC1, 1.0f/1000.0f);
 			chunkHead.upload(tmp_);
-		} else if (!tmp_.empty() && tmp_.type() == CV_8UC3 && chunkHead.type() == CV_8UC3) {
+		} else if (!tmp_.empty() && tmp_.type() == CV_8UC4 && chunkHead.type() == CV_8UC4) {
 			//tmp_.copyTo(chunkHead);
 			chunkHead.upload(tmp_);
 		} else {
@@ -57,7 +64,7 @@ bool OpenCVDecoder::decode(const ftl::codecs::Packet &pkt, cv::cuda::GpuMat &out
 			tmp_.convertTo(tmp_, CV_32FC1, 1.0f/1000.0f); //(16.0f*10.0f));
 			cv::resize(tmp_, tmp_, chunkHead.size(), 0, 0, cv::INTER_NEAREST);
 			chunkHead.upload(tmp_);
-		} else if (!tmp_.empty() && tmp_.type() == CV_8UC3 && chunkHead.type() == CV_8UC3) {
+		} else if (!tmp_.empty() && tmp_.type() == CV_8UC4 && chunkHead.type() == CV_8UC4) {
 			cv::resize(tmp_, tmp_, chunkHead.size());
 			chunkHead.upload(tmp_);
 		} else {

components/codecs/test/opencv_codec_unit.cpp

@@ -84,8 +84,8 @@ TEST_CASE( "OpenCVEncoder::encode() - A depth test image at preset 0" ) {
 TEST_CASE( "OpenCVDecoder::decode() - A colour test image no resolution change" ) {
 	ftl::codecs::OpenCVEncoder encoder(definition_t::HD1080, definition_t::SD480);
 	ftl::codecs::OpenCVDecoder decoder;
-	cv::cuda::GpuMat in(cv::Size(1024,576), CV_8UC3, cv::Scalar(255,0,0));
-	cv::cuda::GpuMat out(cv::Size(1024,576), CV_8UC3, cv::Scalar(0,0,0));
+	cv::cuda::GpuMat in(cv::Size(1024,576), CV_8UC4, cv::Scalar(255,0,0,0));
+	cv::cuda::GpuMat out(cv::Size(1024,576), CV_8UC4, cv::Scalar(0,0,0,0));
 
 	std::mutex mtx;
 

components/operators/CMakeLists.txt

@@ -11,6 +11,8 @@ set(OPERSRC
 	src/disparity/disp2depth.cu
 	src/disparity/disparity_to_depth.cpp
 	src/disparity/bilateral_filter.cpp
+	src/disparity/opencv/disparity_bilateral_filter.cpp
+	src/disparity/opencv/disparity_bilateral_filter.cu
 	src/segmentation.cu
 	src/segmentation.cpp
 	src/mask.cu
@@ -20,6 +22,7 @@ set(OPERSRC
 	src/mvmls.cpp
 	src/correspondence.cu
 	src/clipping.cpp
+	src/depth.cpp
 )
 
 

components/operators/include/ftl/operators/colours.hpp

@@ -17,6 +17,7 @@ class ColourChannels : public ftl::operators::Operator {
 
     private:
     cv::cuda::GpuMat temp_;
+	cv::cuda::GpuMat rbuf_;
 };
 
 }

components/operators/include/ftl/operators/disparity.hpp

@@ -77,6 +77,28 @@ class DisparityToDepth : public ftl::operators::Operator {
 	bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *src, cudaStream_t stream) override;
 };
 
+/**
+ * Create a missing depth channel using Left and Right colour with a complete
+ * disparity pipeline. This is a wrapper operator that combines all of the
+ * disparity to depth steps.
+ */
+class DepthChannel : public ftl::operators::Operator {
+    public:
+    explicit DepthChannel(ftl::Configurable *cfg);
+    ~DepthChannel();
+
+	inline Operator::Type type() const override { return Operator::Type::ManyToMany; }
+
+    bool apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) override;
+
+    private:
+    ftl::operators::Graph *pipe_;
+	std::vector<cv::cuda::GpuMat> rbuf_;
+	cv::Size depth_size_;
+
+	void _createPipeline();
+};
+
 /*
  * Optical flow smoothing for depth
  */

components/operators/src/colours.cpp

@@ -25,15 +25,39 @@ bool ColourChannels::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgb
 		cv::cuda::cvtColor(temp_,col, cv::COLOR_BGR2BGRA, 0, cvstream);
 	}
 
+	if (in.hasChannel(Channel::Right)) {
+		auto &col = in.get<cv::cuda::GpuMat>(Channel::Right);
+
+		// Convert colour from BGR to BGRA if needed
+		if (col.type() == CV_8UC3) {
+			//cv::cuda::Stream cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
+			// Convert to 4 channel colour
+			temp_.create(col.size(), CV_8UC4);
+			cv::cuda::swap(col, temp_);
+			cv::cuda::cvtColor(temp_,col, cv::COLOR_BGR2BGRA, 0, cvstream);
+		}
+	}
+
 	//in.resetTexture(Channel::Colour);
 	in.createTexture<uchar4>(Channel::Colour, true);
 
-	auto &depth = in.get<cv::cuda::GpuMat>(Channel::Depth);
-	if (depth.size() != col.size()) {
-		auto &col2 = in.create<cv::cuda::GpuMat>(Channel::ColourHighRes);
-		cv::cuda::resize(col, col2, depth.size(), 0.0, 0.0, cv::INTER_LINEAR, cvstream);
-		in.createTexture<uchar4>(Channel::ColourHighRes, true);
-		in.swapChannels(Channel::Colour, Channel::ColourHighRes);
+	if (in.hasChannel(Channel::Depth)) {
+		auto &depth = in.get<cv::cuda::GpuMat>(Channel::Depth);
+		if (depth.size() != col.size()) {
+			auto &col2 = in.create<cv::cuda::GpuMat>(Channel::ColourHighRes);
+			cv::cuda::resize(col, col2, depth.size(), 0.0, 0.0, cv::INTER_LINEAR, cvstream);
+			in.createTexture<uchar4>(Channel::ColourHighRes, true);
+			in.swapChannels(Channel::Colour, Channel::ColourHighRes);
+		}
+
+		// Ensure right channel is also downsized
+		if (in.hasChannel(Channel::Right)) {
+			auto &right = in.get<cv::cuda::GpuMat>(Channel::Right);
+			if (depth.size() != right.size()) {
+				cv::cuda::resize(right, rbuf_, depth.size(), 0.0, 0.0, cv::INTER_CUBIC, cvstream);
+				cv::cuda::swap(right, rbuf_);
+			}
+		}
 	}
 
 	return true;

components/operators/src/depth.cpp

+#include <ftl/operators/disparity.hpp>
+
+#include "ftl/operators/smoothing.hpp"
+#include "ftl/operators/colours.hpp"
+#include "ftl/operators/normals.hpp"
+#include "ftl/operators/filling.hpp"
+#include "ftl/operators/segmentation.hpp"
+#include "ftl/operators/disparity.hpp"
+#include "ftl/operators/mask.hpp"
+
+using ftl::operators::DepthChannel;
+using ftl::codecs::Channel;
+
+DepthChannel::DepthChannel(ftl::Configurable *cfg) : ftl::operators::Operator(cfg) {
+	pipe_ = nullptr;
+}
+
+DepthChannel::~DepthChannel() {
+
+}
+
+void DepthChannel::_createPipeline() {
+	if (pipe_ != nullptr) return;
+
+	pipe_ = ftl::config::create<ftl::operators::Graph>(config(), "depth");
+	depth_size_ = cv::Size(	pipe_->value("width", 1280),
+							pipe_->value("height", 720));
+
+	pipe_->append<ftl::operators::ColourChannels>("colour");  // Convert BGR to BGRA
+	pipe_->append<ftl::operators::FixstarsSGM>("algorithm");
+	#ifdef HAVE_OPTFLOW
+	pipe_->append<ftl::operators::OpticalFlowTemporalSmoothing>("optflow_filter");
+	#endif
+	pipe_->append<ftl::operators::DisparityBilateralFilter>("bilateral_filter");
+	pipe_->append<ftl::operators::DisparityToDepth>("calculate_depth");
+	pipe_->append<ftl::operators::Normals>("normals");  // Estimate surface normals
+	pipe_->append<ftl::operators::CrossSupport>("cross");
+	pipe_->append<ftl::operators::DiscontinuityMask>("discontinuity_mask");
+	pipe_->append<ftl::operators::AggreMLS>("mls");  // Perform MLS (using smoothing channel)
+}
+
+bool DepthChannel::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) {
+	auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
+
+	rbuf_.resize(in.frames.size());
+
+	for (int i=0; i<in.frames.size(); ++i) {
+		auto &f = in.frames[i];
+		if (!f.hasChannel(Channel::Depth) && f.hasChannel(Channel::Right)) {
+			_createPipeline();
+
+			cv::cuda::GpuMat& left = f.get<cv::cuda::GpuMat>(Channel::Left);
+			cv::cuda::GpuMat& right = f.get<cv::cuda::GpuMat>(Channel::Right);
+			cv::cuda::GpuMat& depth = f.create<cv::cuda::GpuMat>(Channel::Depth);
+			depth.create(left.size(), CV_32FC1);
+
+			if (left.empty() || right.empty()) continue;
+
+			/*if (depth_size_ != left.size()) {
+				auto &col2 = f.create<cv::cuda::GpuMat>(Channel::ColourHighRes);
+				cv::cuda::resize(left, col2, depth_size_, 0.0, 0.0, cv::INTER_CUBIC, cvstream);
+				f.createTexture<uchar4>(Channel::ColourHighRes, true);
+				f.swapChannels(Channel::Colour, Channel::ColourHighRes);
+			}
+
+			if (depth_size_ != right.size()) {
+				cv::cuda::resize(right, rbuf_[i], depth_size_, 0.0, 0.0, cv::INTER_CUBIC, cvstream);
+				cv::cuda::swap(right, rbuf_[i]);
+			}*/
+
+			pipe_->apply(f, f, in.sources[i], stream);
+		}
+	}
+
+	return true;
+}

components/operators/src/disparity/bilateral_filter.cpp

 #include <ftl/operators/disparity.hpp>
 
+#include "opencv/joint_bilateral.hpp"
+#include "cuda.hpp"
+
 using cv::cuda::GpuMat;
 using cv::Size;
 
@@ -13,15 +16,36 @@ DisparityBilateralFilter::DisparityBilateralFilter(ftl::Configurable* cfg) :
 	n_disp_ = cfg->value("n_disp", 256);
 	radius_ = cfg->value("radius", 7);
 	iter_ = cfg->value("iter", 13);
-	filter_ = cv::cuda::createDisparityBilateralFilter(n_disp_ * scale_, radius_, iter_);
+	filter_ = nullptr;
 }
 
 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)) {
+	if (!in.hasChannel(Channel::Colour)) {
+		LOG(ERROR) << "Joint Bilateral Filter is missing Colour";
 		return false;
+	} else if (!in.hasChannel(Channel::Disparity)) {
+		// Have depth, so calculate disparity...
+		if (in.hasChannel(Channel::Depth)) {
+			// No disparity, so create it.
+			const auto params = src->parameters();
+			const GpuMat &depth = in.get<GpuMat>(Channel::Depth);
+
+			GpuMat &disp = out.create<GpuMat>(Channel::Disparity);
+			disp.create(depth.size(), CV_32FC1);
+
+			LOG(ERROR) << "Calculated disparity from depth";
+
+			ftl::cuda::depth_to_disparity(disp, depth, params, stream);
+		} else {
+			LOG(ERROR) << "Joint Bilateral Filter is missing Disparity and Depth";
+			return false;
+		}
 	}
+
+	if (!filter_) filter_ = ftl::cuda::createDisparityBilateralFilter(n_disp_ * scale_, radius_, iter_);
+
 	auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
 	const GpuMat &rgb = in.get<GpuMat>(Channel::Colour);
 	GpuMat &disp_in = in.get<GpuMat>(Channel::Disparity);

components/operators/src/disparity/cuda.hpp

@@ -6,6 +6,9 @@ namespace cuda {
 void disparity_to_depth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth,
 						const ftl::rgbd::Camera &c, cudaStream_t &stream);
 
+void depth_to_disparity(cv::cuda::GpuMat &disparity, const cv::cuda::GpuMat &depth,
+						const ftl::rgbd::Camera &c, cudaStream_t &stream);
+
 
 void optflow_filter(cv::cuda::GpuMat &disp, const cv::cuda::GpuMat &optflow,
 					cv::cuda::GpuMat &history, int n_max, float threshold,