Implements #272 Disparity pipeline in reconstruct

components/operators/src/disparity/disp2depth.cu

@@ -27,3 +27,32 @@ namespace cuda {
 	}
 }
 }
+
+//==============================================================================
+
+__global__ void d2drev_kernel(cv::cuda::PtrStepSz<float> disp, cv::cuda::PtrStepSz<float> depth,
+	ftl::rgbd::Camera cam) {
+
+for (STRIDE_Y(v,disp.rows)) {
+for (STRIDE_X(u,disp.cols)) {
+	float d = depth(v,u);
+	float disparity = (d > cam.maxDepth || d < cam.minDepth) ? 0.0f : ((cam.baseline*cam.fx) / d) + cam.doffs;
+	disp(v,u) = disparity;
+}
+}
+}
+
+namespace ftl {
+namespace cuda {
+void depth_to_disparity(cv::cuda::GpuMat &disparity, const cv::cuda::GpuMat &depth,
+			const ftl::rgbd::Camera &c, cudaStream_t &stream) {
+	dim3 grid(1,1,1);
+	dim3 threads(128, 1, 1);
+	grid.x = cv::cuda::device::divUp(disparity.cols, 128);
+	grid.y = cv::cuda::device::divUp(disparity.rows, 1);
+	d2drev_kernel<<<grid, threads, 0, stream>>>(
+		disparity, depth, c);
+	cudaSafeCall( cudaGetLastError() );
+}
+}
+}

components/operators/src/disparity/disparity_to_depth.cpp

@@ -9,7 +9,10 @@ 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;  }
+	if (!in.hasChannel(Channel::Disparity)) {
+		LOG(ERROR) << "Missing disparity before convert to depth";
+		return false;
+	}
 
 	const auto params = src->parameters();
 	const GpuMat &disp = in.get<GpuMat>(Channel::Disparity);

components/operators/src/disparity/fixstars_sgm.cpp

@@ -89,6 +89,8 @@ bool FixstarsSGM::init() {
 	rbw_.create(size_, CV_8UC1);
 	disp_int_.create(size_, CV_16SC1);
 
+	LOG(INFO) << "INIT FIXSTARS";
+
 	ssgm_ = new sgm::StereoSGM(size_.width, size_.height, max_disp_, 8, 16,
 		lbw_.step, disp_int_.step / sizeof(short),
 		sgm::EXECUTE_INOUT_CUDA2CUDA,
@@ -106,12 +108,13 @@ bool FixstarsSGM::updateParameters() {
 
 bool FixstarsSGM::apply(Frame &in, Frame &out, Source *src, cudaStream_t stream) {
 	if (!in.hasChannel(Channel::Left) || !in.hasChannel(Channel::Right)) {
+		LOG(ERROR) << "Fixstars is missing Left or Right channel";
 		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; }
@@ -120,8 +123,8 @@ bool FixstarsSGM::apply(Frame &in, Frame &out, Source *src, cudaStream_t stream)
 	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);
+	cv::cuda::cvtColor(l, lbw_, cv::COLOR_BGRA2GRAY, 0, cvstream);
+	cv::cuda::cvtColor(r, rbw_, cv::COLOR_BGRA2GRAY, 0, cvstream);
 
 	cvstream.waitForCompletion();
 	ssgm_->execute(lbw_.data, rbw_.data, disp_int_.data);

components/operators/src/disparity/opencv/disparity_bilateral_filter.cpp

+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include <ftl/cuda_common.hpp>
+#include <opencv2/cudastereo.hpp>
+#include <opencv2/core/cuda_stream_accessor.hpp>
+#include "joint_bilateral.hpp"
+
+using namespace cv;
+using namespace cv::cuda;
+
+#if !defined (HAVE_CUDA) || defined (CUDA_DISABLER)
+
+Ptr<cuda::DisparityBilateralFilter> ftl::cuda::createDisparityBilateralFilter(int, int, int) { throw_no_cuda(); return Ptr<cuda::DisparityBilateralFilter>(); }
+
+#else /* !defined (HAVE_CUDA) */
+
+#include "disparity_bilateral_filter.hpp"
+
+namespace
+{
+    class DispBilateralFilterImpl : public cv::cuda::DisparityBilateralFilter
+    {
+    public:
+        DispBilateralFilterImpl(int ndisp, int radius, int iters);
+
+        void apply(InputArray disparity, InputArray image, OutputArray dst, Stream& stream);
+
+        int getNumDisparities() const { return ndisp_; }
+        void setNumDisparities(int numDisparities) { ndisp_ = numDisparities; }
+
+        int getRadius() const { return radius_; }
+        void setRadius(int radius);
+
+        int getNumIters() const { return iters_; }
+        void setNumIters(int iters) { iters_ = iters; }
+
+        double getEdgeThreshold() const { return edge_threshold_; }
+        void setEdgeThreshold(double edge_threshold) { edge_threshold_ = (float) edge_threshold; }
+
+        double getMaxDiscThreshold() const { return max_disc_threshold_; }
+        void setMaxDiscThreshold(double max_disc_threshold) { max_disc_threshold_ = (float) max_disc_threshold; }
+
+        double getSigmaRange() const { return sigma_range_; }
+        void setSigmaRange(double sigma_range);
+
+    private:
+        int ndisp_;
+        int radius_;
+        int iters_;
+        float edge_threshold_;
+        float max_disc_threshold_;
+        float sigma_range_;
+
+        GpuMat table_color_;
+        GpuMat table_space_;
+    };
+
+    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);
+    }
+
+    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);
+    }
+
+    const float DEFAULT_EDGE_THRESHOLD = 0.1f;
+    const float DEFAULT_MAX_DISC_THRESHOLD = 0.2f;
+    const float DEFAULT_SIGMA_RANGE = 10.0f;
+
+    DispBilateralFilterImpl::DispBilateralFilterImpl(int ndisp, int radius, int iters) :
+        ndisp_(ndisp), radius_(radius), iters_(iters),
+        edge_threshold_(DEFAULT_EDGE_THRESHOLD), max_disc_threshold_(DEFAULT_MAX_DISC_THRESHOLD),
+        sigma_range_(DEFAULT_SIGMA_RANGE)
+    {
+        calc_color_weighted_table(table_color_, sigma_range_, 255);
+        calc_space_weighted_filter(table_space_, radius_ * 2 + 1, radius_ + 1.0f);
+    }
+
+    void DispBilateralFilterImpl::setRadius(int radius)
+    {
+        radius_ = radius;
+        calc_space_weighted_filter(table_space_, radius_ * 2 + 1, radius_ + 1.0f);
+    }
+
+    void DispBilateralFilterImpl::setSigmaRange(double sigma_range)
+    {
+        sigma_range_ = (float) sigma_range;
+        calc_color_weighted_table(table_color_, sigma_range_, 255);
+    }
+
+    template <typename T>
+    void disp_bilateral_filter_operator(int ndisp, int radius, int iters, float edge_threshold, float max_disc_threshold,
+                                        GpuMat& table_color, GpuMat& table_space,
+                                        const GpuMat& disp, const GpuMat& img,
+                                        OutputArray _dst, Stream& stream)
+    {
+        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);
+
+        size_t table_space_step = table_space.step / sizeof(float);
+
+        _dst.create(disp.size(), disp.type());
+        GpuMat dst = _dst.getGpuMat();
+
+        if (dst.data != disp.data)
+            disp.copyTo(dst, stream);
+
+        disp_bilateral_filter<T>(dst, img, img.channels(), iters, table_color.ptr<float>(), (float *)table_space.data, table_space_step, radius, edge_disc, max_disc, StreamAccessor::getStream(stream));
+    }
+
+    void DispBilateralFilterImpl::apply(InputArray _disp, InputArray _image, OutputArray dst, Stream& stream)
+    {
+        typedef void (*bilateral_filter_operator_t)(int ndisp, int radius, int iters, float edge_threshold, float max_disc_threshold,
+                                                    GpuMat& table_color, GpuMat& table_space,
+                                                    const GpuMat& disp, const GpuMat& img, OutputArray dst, Stream& stream);
+        const bilateral_filter_operator_t operators[] =
+            {disp_bilateral_filter_operator<unsigned char>, 0, 0, disp_bilateral_filter_operator<short>, 0, 0, 0, 0};
+
+        CV_Assert( 0 < ndisp_ && 0 < radius_ && 0 < iters_ );
+
+        GpuMat disp = _disp.getGpuMat();
+        GpuMat img = _image.getGpuMat();
+
+        CV_Assert( disp.type() == CV_8U || disp.type() == CV_16S );
+        CV_Assert( img.type() == CV_8UC1 || img.type() == CV_8UC3 || img.type() == CV_8UC4 );
+        CV_Assert( disp.size() == img.size() );
+
+        operators[disp.type()](ndisp_, radius_, iters_, edge_threshold_, max_disc_threshold_,
+                               table_color_, table_space_, disp, img, dst, stream);
+    }
+}
+
+Ptr<cuda::DisparityBilateralFilter> ftl::cuda::createDisparityBilateralFilter(int ndisp, int radius, int iters)
+{
+    return makePtr<DispBilateralFilterImpl>(ndisp, radius, iters);
+}
+
+#endif /* !defined (HAVE_CUDA) */

components/operators/src/disparity/opencv/disparity_bilateral_filter.cu

+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#if !defined CUDA_DISABLER
+
+#include "opencv2/core/cuda/common.hpp"
+#include "opencv2/core/cuda/limits.hpp"
+
+#include "disparity_bilateral_filter.hpp"
+
+using namespace cv::cuda::device;
+using namespace cv::cuda;
+using namespace cv;
+
+namespace ftl { namespace cuda { namespace device
+{
+    namespace disp_bilateral_filter
+    {
+        template <int channels>
+        struct DistRgbMax
+        {
+            static __device__ __forceinline__ uchar calc(const uchar* a, const uchar* b)
+            {
+				// TODO: (Nick) Is this the best way to read for performance?
+                uchar x = ::abs(a[0] - b[0]);
+                uchar y = ::abs(a[1] - b[1]);
+                uchar z = ::abs(a[2] - b[2]);
+                return (::max(::max(x, y), z));
+            }
+        };
+
+        template <>
+        struct DistRgbMax<1>
+        {
+            static __device__ __forceinline__ uchar calc(const uchar* a, const uchar* b)
+            {
+                return ::abs(a[0] - b[0]);
+            }
+		};
+
+        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)
+        {
+            const int y = blockIdx.y * blockDim.y + threadIdx.y;
+            const int x = ((blockIdx.x * blockDim.x + threadIdx.x) << 1) + ((y + t) & 1);
+
+            T dp[5];
+
+            if (y > 0 && y < h - 1 && x > 0 && x < w - 1)
+            {
+                dp[0] = *(disp + (y  ) * disp_step + x + 0);
+                dp[1] = *(disp + (y-1) * disp_step + x + 0);
+                dp[2] = *(disp + (y  ) * disp_step + x - 1);
+                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)
+                {
+                    const int ymin = ::max(0, y - cradius);
+                    const int xmin = ::max(0, x - cradius);
+                    const int ymax = ::min(h - 1, y + cradius);
+                    const int xmax = ::min(w - 1, x + cradius);
+
+                    float cost[] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+                    const uchar* ic = img + y * img_step + channels * x;
+
+                    for(int yi = ymin; yi <= ymax; yi++)
+                    {
+                        const T* disp_y = disp + yi * disp_step;
+
+                        for(int xi = xmin; xi <= xmax; xi++)
+                        {
+                            const uchar* in = img + yi * img_step + channels * xi;
+
+                            uchar dist_rgb = DistRgbMax<channels>::calc(in, ic);
+
+                            const float weight = ctable_color[dist_rgb] * (ctable_space + ::abs(y-yi)* ctable_space_step)[::abs(x-xi)];
+
+                            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;
+                        }
+                    }
+
+                    float minimum = numeric_limits<float>::max();
+                    int id = 0;
+
+                    if (cost[0] < minimum)
+                    {
+                        minimum = cost[0];
+                        id = 0;
+                    }
+                    if (cost[1] < minimum)
+                    {
+                        minimum = cost[1];
+                        id = 1;
+                    }
+                    if (cost[2] < minimum)
+                    {
+                        minimum = cost[2];
+                        id = 2;
+                    }
+                    if (cost[3] < minimum)
+                    {
+                        minimum = cost[3];
+                        id = 3;
+                    }
+                    if (cost[4] < minimum)
+                    {
+                        minimum = cost[4];
+                        id = 4;
+                    }
+
+                    *(disp + y * disp_step + x) = dp[id];
+                }
+            }
+        }
+
+        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)
+        {
+            dim3 threads(32, 8, 1);
+            dim3 grid(1, 1, 1);
+            grid.x = divUp(disp.cols, threads.x << 1);
+            grid.y = divUp(disp.rows, threads.y);
+
+            switch (channels)
+            {
+            case 1:
+                for (int i = 0; i < iters; ++i)
+                {
+                    disp_bilateral_filter<1><<<grid, threads, 0, stream>>>(0, disp.data, disp.step/sizeof(T), img.data, img.step, disp.rows, disp.cols, table_color, table_space, table_step, radius, edge_disc, max_disc);
+                    cudaSafeCall( cudaGetLastError() );
+
+                    disp_bilateral_filter<1><<<grid, threads, 0, stream>>>(1, disp.data, disp.step/sizeof(T), img.data, img.step, disp.rows, disp.cols, table_color, table_space, table_step, radius, edge_disc, max_disc);
+                    cudaSafeCall( cudaGetLastError() );
+                }
+                break;
+            case 3:
+                for (int i = 0; i < iters; ++i)
+                {
+                    disp_bilateral_filter<3><<<grid, threads, 0, stream>>>(0, disp.data, disp.step/sizeof(T), img.data, img.step, disp.rows, disp.cols, table_color, table_space, table_step, radius, edge_disc, max_disc);
+                    cudaSafeCall( cudaGetLastError() );
+
+                    disp_bilateral_filter<3><<<grid, threads, 0, stream>>>(1, disp.data, disp.step/sizeof(T), img.data, img.step, disp.rows, disp.cols, table_color, table_space, table_step, radius, edge_disc, max_disc);
+                    cudaSafeCall( cudaGetLastError() );
+                }
+				break;
+			case 4:  // Nick: Support 4 channel
+                for (int i = 0; i < iters; ++i)
+                {
+                    disp_bilateral_filter<4><<<grid, threads, 0, stream>>>(0, disp.data, disp.step/sizeof(T), img.data, img.step, disp.rows, disp.cols, table_color, table_space, table_step, radius, edge_disc, max_disc);
+                    cudaSafeCall( cudaGetLastError() );
+
+                    disp_bilateral_filter<4><<<grid, threads, 0, stream>>>(1, disp.data, disp.step/sizeof(T), img.data, img.step, disp.rows, disp.cols, table_color, table_space, table_step, radius, edge_disc, max_disc);
+                    cudaSafeCall( cudaGetLastError() );
+                }
+                break;
+            default:
+                CV_Error(cv::Error::BadNumChannels, "Unsupported channels count");
+            }
+
+            if (stream == 0)
+                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);
+    } // namespace bilateral_filter
+}}} // namespace ftl { namespace cuda { namespace cudev
+
+#endif /* CUDA_DISABLER */

components/operators/src/disparity/opencv/disparity_bilateral_filter.hpp

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

components/operators/src/disparity/opencv/joint_bilateral.hpp

+#ifndef _FTL_CUDA_OPENCV_BILATERAL_HPP_
+#define _FTL_CUDA_OPENCV_BILATERAL_HPP_
+
+#include <ftl/cuda_common.hpp>
+
+namespace ftl {
+namespace cuda {
+	cv::Ptr<cv::cuda::DisparityBilateralFilter> createDisparityBilateralFilter(int ndisp, int radius, int iters);
+}
+}
+
+#endif  // _FTL_CUDA_OPENCV_BILATERAL_HPP_

components/operators/src/mask.cpp

@@ -15,9 +15,13 @@ DiscontinuityMask::~DiscontinuityMask() {
 }
 
 bool DiscontinuityMask::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *s, cudaStream_t stream) {
+	if (in.hasChannel(Channel::Mask)) return true;
+	
 	int radius = config()->value("radius", 2);
 	float threshold = config()->value("threshold", 0.1f);
 
+	if (!in.hasChannel(Channel::Depth) || !in.hasChannel(Channel::Support1)) return false;
+
 	ftl::cuda::discontinuity(
 		out.createTexture<int>(Channel::Mask, ftl::rgbd::Format<int>(in.get<cv::cuda::GpuMat>(Channel::Depth).size())),
 		in.createTexture<uchar4>(Channel::Support1),

components/operators/src/normals.cpp

@@ -22,7 +22,8 @@ bool Normals::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Sour
 	}
 
 	if (out.hasChannel(Channel::Normals)) {
-		LOG(WARNING) << "Output already has normals";
+		//LOG(WARNING) << "Output already has normals";
+		return true;
 	}
 
 	ftl::cuda::normals(
@@ -53,7 +54,8 @@ bool SmoothNormals::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd
 	}
 
 	if (out.hasChannel(Channel::Normals)) {
-		LOG(WARNING) << "Output already has normals";
+		//LOG(WARNING) << "Output already has normals";
+		return true;
 	}
 
 	auto &depth = in.get<cv::cuda::GpuMat>(Channel::Depth);

components/operators/src/operator.cpp

@@ -52,15 +52,22 @@ bool Graph::apply(FrameSet &in, FrameSet &out, cudaStream_t stream) {
 
 		if (i.instances[0]->type() == Operator::Type::OneToOne) {
 			// Make sure there are enough instances
-			while (i.instances.size() < in.frames.size()) {
+			//while (i.instances.size() < in.frames.size()) {
+				//i.instances.push_back(i.maker->make());
+			//}
+			if (in.frames.size() > 1) {
 				i.instances.push_back(i.maker->make());
 			}
 
 			for (int j=0; j<in.frames.size(); ++j) {
-				auto *instance = i.instances[j];
+				auto *instance = i.instances[j&0x1];
 
 				if (instance->enabled()) {
-					instance->apply(in.frames[j], out.frames[j], in.sources[j], stream_actual);
+					try {
+						if (!instance->apply(in.frames[j], out.frames[j], in.sources[j], stream_actual)) return false;
+					} catch (const std::exception &e) {
+						LOG(ERROR) << "Operator exception: " << e.what();
+					}
 				}
 			}
 		} else if (i.instances[0]->type() == Operator::Type::ManyToMany) {
@@ -68,7 +75,7 @@ bool Graph::apply(FrameSet &in, FrameSet &out, cudaStream_t stream) {
 
 			if (instance->enabled()) {
 				try {
-					instance->apply(in, out, stream_actual);
+					if (!instance->apply(in, out, stream_actual)) return false;
 				} catch (const std::exception &e) {
 					LOG(ERROR) << "Operator exception: " << e.what();
 				}
@@ -96,7 +103,7 @@ bool Graph::apply(Frame &in, Frame &out, Source *s, cudaStream_t stream) {
 		auto *instance = i.instances[0];
 
 		if (instance->enabled()) {
-			instance->apply(in, out, s, stream);
+			if (!instance->apply(in, out, s, stream)) return false;
 		}
 	}
 

components/operators/src/segmentation.cpp

@@ -16,7 +16,9 @@ CrossSupport::~CrossSupport() {
 bool CrossSupport::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *s, cudaStream_t stream) {
 	bool use_mask = config()->value("discon_support", false);
 
-	if (use_mask) {
+
+	if (use_mask && !in.hasChannel(Channel::Support2)) {
+		if (!in.hasChannel(Channel::Mask)) return false;
 		ftl::cuda::support_region(
 			in.createTexture<int>(Channel::Mask),
 			out.createTexture<uchar4>(Channel::Support2, ftl::rgbd::Format<uchar4>(in.get<cv::cuda::GpuMat>(Channel::Colour).size())),
@@ -24,7 +26,7 @@ bool CrossSupport::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd:
 			config()->value("h_max", 5),
 			config()->value("symmetric", false), stream
 		);
-	} else {
+	} else if (!in.hasChannel(Channel::Support1)) {
 		ftl::cuda::support_region(
 			in.createTexture<uchar4>(Channel::Colour),
 			out.createTexture<uchar4>(Channel::Support1, ftl::rgbd::Format<uchar4>(in.get<cv::cuda::GpuMat>(Channel::Colour).size())),

components/renderers/cpp/include/ftl/render/tri_render.hpp

@@ -44,6 +44,7 @@ class Triangular : public ftl::render::Renderer {
 	float3 light_pos_;
 	Eigen::Matrix4d transform_;
 	float scale_;
+	int64_t last_frame_;
 
 	cv::cuda::GpuMat env_image_;
 	ftl::cuda::TextureObject<uchar4> env_tex_;
@@ -55,6 +56,8 @@ class Triangular : public ftl::render::Renderer {
 	void _reprojectChannel(ftl::rgbd::Frame &, ftl::codecs::Channel in, ftl::codecs::Channel out, const Eigen::Matrix4d &t, cudaStream_t);
 	void _dibr(ftl::rgbd::Frame &, const Eigen::Matrix4d &t, cudaStream_t);
 	void _mesh(ftl::rgbd::Frame &, ftl::rgbd::Source *, const Eigen::Matrix4d &t, cudaStream_t);
+
+	bool _alreadySeen() const { return last_frame_ == scene_->timestamp; }
 };
 
 }

components/renderers/cpp/src/tri_render.cpp

@@ -147,6 +147,7 @@ Triangular::Triangular(nlohmann::json &config, ftl::rgbd::FrameSet *fs) : ftl::r
 
 	//filters_ = ftl::create<ftl::Filters>(this, "filters");
 	//filters_->create<ftl::filters::DepthSmoother>("hfnoise");
+	last_frame_ = -1;
 }
 
 Triangular::~Triangular() {
@@ -579,6 +580,13 @@ bool Triangular::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, co
 
 	int aligned_source = value("aligned_source",-1);
 	if (aligned_source >= 0 && aligned_source < scene_->frames.size()) {
+		// Can only send at originally received frame rate due to reuse of
+		// encodings that can't be sent twice.
+		if (_alreadySeen()) {
+			out.reset();
+			return false;
+		}
+
 		// FIXME: Output may not be same resolution as source!
 		cudaSafeCall(cudaStreamSynchronize(stream_));
 		scene_->frames[aligned_source].copyTo(Channel::Depth + Channel::Colour + Channel::Smoothing + Channel::Confidence, out);
@@ -596,6 +604,7 @@ bool Triangular::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, co
 			//out.resetTexture(Channel::Normals);
 		}
 
+		last_frame_ = scene_->timestamp;
 		return true;
 	}
 
@@ -704,5 +713,6 @@ bool Triangular::render(ftl::rgbd::VirtualSource *src, ftl::rgbd::Frame &out, co
 	}
 
 	cudaSafeCall(cudaStreamSynchronize(stream_));
+	last_frame_ = scene_->timestamp;
 	return true;
 }

components/rgbd-sources/include/ftl/rgbd/detail/source.hpp

@@ -62,8 +62,7 @@ class Source {
 	capability_t capabilities_;
 	ftl::rgbd::Source *host_;
 	ftl::rgbd::Camera params_;
-	cv::cuda::GpuMat rgb_;
-	cv::cuda::GpuMat depth_;
+	ftl::rgbd::Frame frame_;
 	int64_t timestamp_;
 	//Eigen::Matrix4f pose_;
 };

components/rgbd-sources/include/ftl/rgbd/frame.hpp

@@ -11,11 +11,13 @@
 #include <ftl/codecs/channels.hpp>
 #include <ftl/rgbd/format.hpp>
 #include <ftl/codecs/bitrates.hpp>
+#include <ftl/codecs/packet.hpp>
 
 #include <ftl/cuda_common.hpp>
 
 #include <type_traits>
 #include <array>
+#include <list>
 
 namespace ftl {
 namespace rgbd {
@@ -90,6 +92,18 @@ public:
 	template <typename T>
 	ftl::cuda::TextureObject<T> &createTexture(ftl::codecs::Channel c, bool interpolated=false);
 
+	/**
+	 * Append encoded data for a channel. This will move the data, invalidating
+	 * the original packet structure. It is to be used to allow data that is
+	 * already encoded to be transmitted or saved again without re-encoding.
+	 * A called to `create` will clear all encoded data for that channel.
+	 */
+	void pushPacket(ftl::codecs::Channel c, ftl::codecs::Packet &pkt);
+
+	const std::list<ftl::codecs::Packet> &getPackets(ftl::codecs::Channel c) const;
+
+	void mergeEncoding(ftl::rgbd::Frame &f);
+
 	void resetTexture(ftl::codecs::Channel c);
 
 	/**
@@ -97,6 +111,11 @@ public:
 	 */
 	void reset();
 
+	/**
+	 * Reset all channels and release memory.
+	 */
+	void resetFull();
+
 	bool empty(ftl::codecs::Channels c);
 
 	inline bool empty(ftl::codecs::Channel c) {
@@ -159,6 +178,7 @@ private:
 		ftl::cuda::TextureObjectBase tex;
 		cv::Mat host;
 		cv::cuda::GpuMat gpu;
+		std::list<ftl::codecs::Packet> encoded;
 	};
 
 	std::array<ChannelData, ftl::codecs::Channels::kMax> data_;

components/rgbd-sources/include/ftl/rgbd/frameset.hpp

@@ -29,6 +29,8 @@ struct FrameSet {
 	void upload(ftl::codecs::Channels, cudaStream_t stream=0);
 	void download(ftl::codecs::Channels, cudaStream_t stream=0);
 	void swapTo(ftl::rgbd::FrameSet &);
+
+	void resetFull();
 };
 
 }

components/rgbd-sources/include/ftl/rgbd/group.hpp

@@ -12,12 +12,16 @@
 #include <vector>
 
 namespace ftl {
+namespace operators {
+	class Graph;
+}
+
 namespace rgbd {
 
 class Source;
 
 // Allows a latency of 20 frames maximum
-static const size_t kFrameBufferSize = 20;
+static const size_t kMaxFramesets = 15;
 
 /**
  * Manage a group of RGB-D sources to obtain synchronised sets of frames from
@@ -55,6 +59,12 @@ class Group {
 	 */
 	void addGroup(ftl::rgbd::Group *);
 
+	/**
+	 * Add a pipeline to be run after each frame is received from source but
+	 * before it as added to a synchronised frameset.
+	 */
+	void addPipeline(ftl::operators::Graph *g) { pipeline_ = g; };
+
 	/**
 	 * Provide a function to be called once per frame with a valid frameset
 	 * at the specified latency. The function may not be called under certain
@@ -95,20 +105,25 @@ class Group {
 	 * the reference point, this may already be several frames old. Latency
 	 * does not correspond to actual current time.
 	 */
-	void setLatency(int frames) { latency_ = frames; }
+	void setLatency(int frames) { }
 
 	void stop() {}
 
 	int streamID(const ftl::rgbd::Source *s) const;
 
 	private:
-	std::vector<FrameSet> framesets_;
+	std::list<FrameSet*> framesets_;  // Active framesets
+	std::list<FrameSet*> allocated_;  // Keep memory allocations
+
 	std::vector<Source*> sources_;
+	ftl::operators::Graph *pipeline_;
 	size_t head_;
 	std::function<bool(FrameSet &)> callback_;
 	MUTEX mutex_;
 	int mspf_;
-	int latency_;
+	float latency_;
+	float fps_;
+	int stats_count_;
 	int64_t last_ts_;
 	std::atomic<int> jobs_;
 	volatile bool skip_;
@@ -120,13 +135,19 @@ class Group {
 	/* Insert a new frameset into the buffer, along with all intermediate
 	 * framesets between the last in buffer and the new one.
 	 */
-	void _addFrameset(int64_t timestamp);
+	ftl::rgbd::FrameSet *_addFrameset(int64_t timestamp);
 
 	void _retrieveJob(ftl::rgbd::Source *);
 	void _computeJob(ftl::rgbd::Source *);
 
 	/* Find a frameset with given latency in frames. */
-	ftl::rgbd::FrameSet *_getFrameset(int f);
+	ftl::rgbd::FrameSet *_getFrameset();
+
+	/* Search for a matching frameset. */
+	ftl::rgbd::FrameSet *_findFrameset(int64_t ts);
+	void _freeFrameset(ftl::rgbd::FrameSet *);
+
+	void _recordStats(float fps, float latency);
 };
 
 }

components/rgbd-sources/include/ftl/rgbd/source.hpp

@@ -32,6 +32,7 @@ class VirtualSource;
 class Player;
 
 typedef std::function<void(ftl::rgbd::Source*, const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt)> RawCallback;
+typedef std::function<void(int64_t,ftl::rgbd::Frame&)> FrameCallback;
 
 /**
  * RGBD Generic data source configurable entity. This class hides the
@@ -176,14 +177,14 @@ class Source : public ftl::Configurable {
 
 	SHARED_MUTEX &mutex() { return mutex_; }
 
-	std::function<void(int64_t, cv::cuda::GpuMat &, cv::cuda::GpuMat &)> &callback() { return callback_; }
+	const FrameCallback &callback() { return callback_; }
 
 	/**
 	 * Set the callback that receives decoded frames as they are generated.
 	 * There can be only a single such callback as the buffers can be swapped
 	 * by the callback.
 	 */
-	void setCallback(std::function<void(int64_t, cv::cuda::GpuMat &, cv::cuda::GpuMat &)> cb);
+	void setCallback(const FrameCallback &cb);
 	void removeCallback() { callback_ = nullptr; }
 
 	/**
@@ -207,7 +208,8 @@ class Source : public ftl::Configurable {
 	 * Notify of a decoded or available pair of frames. This calls the source
 	 * callback after having verified the correct resolution of the frames.
 	 */
-	void notify(int64_t ts, cv::cuda::GpuMat &c1, cv::cuda::GpuMat &c2);
+	//void notify(int64_t ts, cv::cuda::GpuMat &c1, cv::cuda::GpuMat &c2);
+	void notify(int64_t ts, ftl::rgbd::Frame &f);
 
 	// ==== Inject Data into stream ============================================
 
@@ -227,8 +229,8 @@ class Source : public ftl::Configurable {
 	SHARED_MUTEX mutex_;
 	ftl::codecs::Channel channel_;
 	cudaStream_t stream_;
-	std::function<void(int64_t, cv::cuda::GpuMat &, cv::cuda::GpuMat &)> callback_;
-	std::list<std::function<void(ftl::rgbd::Source*, const ftl::codecs::StreamPacket &spkt, const ftl::codecs::Packet &pkt)>> rawcallbacks_;
+	FrameCallback callback_;
+	std::list<RawCallback> rawcallbacks_;
 
 	detail::Source *_createImplementation();
 	detail::Source *_createFileImpl(const ftl::URI &uri);

components/rgbd-sources/include/ftl/rgbd/streamer.hpp

@@ -107,6 +107,8 @@ class Streamer : public ftl::Configurable {
 	 */
 	void add(ftl::rgbd::Group *grp);
 
+	ftl::rgbd::Group *group() { return &group_; }
+
 	void remove(Source *);
 	void remove(const std::string &);
 

components/rgbd-sources/include/ftl/rgbd/virtual.hpp

@@ -11,7 +11,7 @@ class VirtualSource : public ftl::rgbd::Source {
     explicit VirtualSource(ftl::config::json_t &cfg);
 	~VirtualSource();
 
-	void onRender(const std::function<void(ftl::rgbd::Frame &)> &);
+	void onRender(const std::function<bool(ftl::rgbd::Frame &)> &);
 
     /**
 	 * Write frames into source buffers from an external renderer. Virtual