diff --git a/cv-node/CMakeLists.txt b/cv-node/CMakeLists.txt
index 9bc20a4f37da65253d8d07db333cd19181b9ce7e..1547e96cbdc9dad8171558f0f4e6fcaa7e816069 100644
--- a/cv-node/CMakeLists.txt
+++ b/cv-node/CMakeLists.txt
@@ -1,6 +1,7 @@
 cmake_minimum_required (VERSION 3.1.0)
 include (CheckIncludeFile)
 include (CheckFunctionExists)
+include(CheckLanguage)
 
 project (cv-node)
 
@@ -21,6 +22,15 @@ find_package( CUDA )
 #find_package(PkgConfig)
 #pkg_check_modules(GTKMM gtkmm-3.0)
 
+if (CUDA_FOUND)
+check_language(CUDA)
+enable_language(CUDA)
+set(CMAKE_CUDA_FLAGS "-Xcompiler -Wall")
+set(CMAKE_CUDA_FLAGS_DEBUG "-g -DDEBUG -D_DEBUG -Wall")
+set(CMAKE_CUDA_FLAGS_RELEASE "")
+add_definitions(-DHAVE_CUDA)
+endif (CUDA_FOUND)
+
 # Need to include staged files and libs
 include_directories(${PROJECT_SOURCE_DIR}/include)
 include_directories(${PROJECT_BINARY_DIR})
@@ -52,7 +62,7 @@ add_definitions("-DFTL_CONFIG_ROOT=${FTL_CONFIG_ROOT}")
 add_definitions(-DFTL_CACHE_ROOT=${FTL_CACHE_ROOT})
 add_definitions(-DFTL_DATA_ROOT=${FTL_DATA_ROOT})
 
-set(CMAKE_CXX_FLAGS "-pthread -std=c++17 -Wall")
+set(CMAKE_CXX_FLAGS "-std=c++17 -Wall")
 set(CMAKE_CXX_FLAGS_DEBUG "-D_DEBUG -pg -Wall")
 set(CMAKE_CXX_FLAGS_RELEASE "-O3 -msse3 -mfpmath=sse")
 
@@ -74,7 +84,20 @@ if (LIBSGM_FOUND)
 	message("Cuda libraries at ${CUDA_LIBRARIES}")
 endif (LIBSGM_FOUND)
 
+if (CUDA_FOUND)
+	list(APPEND CVNODESRC "src/algorithms/opencv_cuda_bm.cpp" "src/algorithms/opencv_cuda_bp.cpp" "src/algorithms/rtcensus.cu")
+endif (CUDA_FOUND)
+
 add_executable(cv-node ${CVNODESRC})
+
+if (CUDA_FOUND)
+set_property(TARGET Threads::Threads
+                 PROPERTY INTERFACE_COMPILE_OPTIONS $<$<COMPILE_LANGUAGE:CUDA>:-Xcompiler -pthread>
+                                                    "$<$<NOT:$<COMPILE_LANGUAGE:CUDA>>:-pthread>")
+
+set_property(TARGET cv-node PROPERTY CUDA_SEPARABLE_COMPILATION ON)
+endif (CUDA_FOUND)
+
 target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include)
 target_link_libraries(cv-node Threads::Threads ${OpenCV_LIBS} ${LIBSGM_LIBRARIES} ${CUDA_LIBRARIES} glog)
 
diff --git a/cv-node/config/config.json b/cv-node/config/config.json
index 257a38777bbc8e3e5a5e528cb37497712316fa04..f14b58abd4e80de53243bf15c71e8780e38a273d 100644
--- a/cv-node/config/config.json
+++ b/cv-node/config/config.json
@@ -32,13 +32,21 @@
 	},
 	"disparity": {
 		"algorithm": "rtcensus",
-		"minimum": 0,
+		"use_cuda": true,
+		"minimum": 10,
 		"maximum": 208,
 		"tau": 0.0,
 		"gamma": 0.0,
 		"window_size": 5,
 		"sigma": 1.5,
 		"lambda": 8000.0
+	},
+	"display": {
+		"disparity": false,
+		"points": true,
+		"depth": false,
+		"left": false,
+		"right": false
 	}
 }
 
diff --git a/cv-node/include/ftl/algorithms/opencv_cuda_bm.hpp b/cv-node/include/ftl/algorithms/opencv_cuda_bm.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..87561c9217e79ed6aa26ae00e3fb16e4bf6e2ca3
--- /dev/null
+++ b/cv-node/include/ftl/algorithms/opencv_cuda_bm.hpp
@@ -0,0 +1,31 @@
+#ifndef _FTL_ALGORITHMS_OPENCV_CUDA_BM_HPP_
+#define _FTL_ALGORITHMS_OPENCV_CUDA_BM_HPP_
+
+#include <opencv2/core.hpp>
+#include <opencv2/opencv.hpp>
+#include <opencv2/cudastereo.hpp>
+#include <ftl/disparity.hpp>
+
+namespace ftl {
+namespace algorithms {
+class OpenCVCudaBM : public ftl::Disparity {
+	public:
+	OpenCVCudaBM(nlohmann::json &config);
+	
+	void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
+
+	static inline Disparity *create(nlohmann::json &config) { return new OpenCVCudaBM(config); }
+	
+	private:
+	cv::Ptr<cv::cuda::StereoBM> matcher_;
+	cv::Ptr<cv::cuda::DisparityBilateralFilter> filter_;
+	cv::cuda::GpuMat disp_;
+	cv::cuda::GpuMat filtered_;
+	cv::cuda::GpuMat left_;
+	cv::cuda::GpuMat right_;
+};
+};
+};
+
+#endif // _FTL_ALGORITHMS_OPENCV_CUDA_BM_HPP_
+
diff --git a/cv-node/include/ftl/algorithms/opencv_cuda_bp.hpp b/cv-node/include/ftl/algorithms/opencv_cuda_bp.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..190c3812a5270447a792b8897ea8cf5e464e37bf
--- /dev/null
+++ b/cv-node/include/ftl/algorithms/opencv_cuda_bp.hpp
@@ -0,0 +1,29 @@
+#ifndef _FTL_ALGORITHMS_OPENCV_CUDA_BP_HPP_
+#define _FTL_ALGORITHMS_OPENCV_CUDA_BP_HPP_
+
+#include <opencv2/core.hpp>
+#include <opencv2/opencv.hpp>
+#include <opencv2/cudastereo.hpp>
+#include <ftl/disparity.hpp>
+
+namespace ftl {
+namespace algorithms {
+class OpenCVCudaBP : public ftl::Disparity {
+	public:
+	OpenCVCudaBP(nlohmann::json &config);
+	
+	void compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
+
+	static inline Disparity *create(nlohmann::json &config) { return new OpenCVCudaBP(config); }
+	
+	private:
+	cv::Ptr<cv::cuda::StereoBeliefPropagation> matcher_;
+	cv::cuda::GpuMat disp_;
+	cv::cuda::GpuMat left_;
+	cv::cuda::GpuMat right_;
+};
+};
+};
+
+#endif // _FTL_ALGORITHMS_OPENCV_CUDA_BP_HPP_
+
diff --git a/cv-node/include/ftl/algorithms/opencv_cuda_csbp.hpp b/cv-node/include/ftl/algorithms/opencv_cuda_csbp.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/cv-node/include/ftl/algorithms/rtcensus.hpp b/cv-node/include/ftl/algorithms/rtcensus.hpp
index b71f1b25262acd624c6d46205147b822e77b1bbd..2981b8d2e1a6aac2456142acfd2d0fd82eebacfd 100644
--- a/cv-node/include/ftl/algorithms/rtcensus.hpp
+++ b/cv-node/include/ftl/algorithms/rtcensus.hpp
@@ -6,6 +6,10 @@
 #include <ftl/disparity.hpp>
 #include <nlohmann/json.hpp>
 
+#if defined HAVE_CUDA
+#include <opencv2/core/cuda.hpp>
+#endif
+
 namespace ftl {
 namespace algorithms {
 class RTCensus : public ftl::Disparity {
@@ -22,6 +26,21 @@ class RTCensus : public ftl::Disparity {
 	private:
 	float gamma_;
 	float tau_;
+	bool use_cuda_;
+	
+	#if defined HAVE_CUDA
+	cv::cuda::GpuMat disp_;
+	cv::cuda::GpuMat filtered_;
+	cv::cuda::GpuMat left_;
+	cv::cuda::GpuMat right_;
+	#endif
+	
+	private:
+	void computeCPU(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
+	
+	#if defined HAVE_CUDA
+	void computeCUDA(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp);
+	#endif
 };
 };
 };
diff --git a/cv-node/src/algorithms/opencv_cuda_bm.cpp b/cv-node/src/algorithms/opencv_cuda_bm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5957792fe530e10e9b5c8213887a253ecb85de98
--- /dev/null
+++ b/cv-node/src/algorithms/opencv_cuda_bm.cpp
@@ -0,0 +1,31 @@
+#include <ftl/algorithms/opencv_cuda_bm.hpp>
+
+using ftl::algorithms::OpenCVCudaBM;
+using namespace cv;
+
+static ftl::Disparity::Register opencvcudabm("cuda_bm", OpenCVCudaBM::create);
+
+OpenCVCudaBM::OpenCVCudaBM(nlohmann::json &config) : Disparity(config) {
+	matcher_ = cuda::createStereoBM(max_disp_);
+	
+	// TODO Add filter
+	filter_ = cv::cuda::createDisparityBilateralFilter(max_disp_, 5, 5);
+}
+
+void OpenCVCudaBM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
+	if (disp_.empty()) disp_ = cuda::GpuMat(l.size(), CV_8U);
+	if (filtered_.empty()) filtered_ = cuda::GpuMat(l.size(), CV_8U);
+	if (left_.empty()) left_ = cuda::GpuMat(l.size(), CV_8U);
+	if (right_.empty()) right_ = cuda::GpuMat(l.size(), CV_8U);
+	
+	left_.upload(l);
+	right_.upload(r);
+	
+	matcher_->compute(left_, right_, disp_);
+	filter_->apply(disp_, left_, filtered_);
+	
+	filtered_.download(disp);
+}
+
+
+
diff --git a/cv-node/src/algorithms/opencv_cuda_bp.cpp b/cv-node/src/algorithms/opencv_cuda_bp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..51f1e58d0a1f836d4b6e412060fbdf7d06e3c9b8
--- /dev/null
+++ b/cv-node/src/algorithms/opencv_cuda_bp.cpp
@@ -0,0 +1,26 @@
+#include <ftl/algorithms/opencv_cuda_bp.hpp>
+
+using ftl::algorithms::OpenCVCudaBP;
+using namespace cv;
+
+static ftl::Disparity::Register opencvcudabp("cuda_bp", OpenCVCudaBP::create);
+
+OpenCVCudaBP::OpenCVCudaBP(nlohmann::json &config) : Disparity(config) {
+	matcher_ = cuda::createStereoBeliefPropagation(max_disp_);
+	
+	// TODO Add filter
+}
+
+void OpenCVCudaBP::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
+	if (disp_.empty()) disp_ = cuda::GpuMat(l.size(), CV_8U);
+	if (left_.empty()) left_ = cuda::GpuMat(l.size(), CV_8U);
+	if (right_.empty()) right_ = cuda::GpuMat(l.size(), CV_8U);
+	
+	left_.upload(l);
+	right_.upload(r);
+	
+	matcher_->compute(left_, right_, disp_);
+	
+	disp_.download(disp);
+}
+
diff --git a/cv-node/src/algorithms/opencv_cuda_csbp.cpp b/cv-node/src/algorithms/opencv_cuda_csbp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/cv-node/src/algorithms/opencv_sgbm.cpp b/cv-node/src/algorithms/opencv_sgbm.cpp
index 9853e4f01f995dfd886a9cfde89d777ed4cd19a5..3e003bdb1ccd75eec7c7e6e6f76280e240a84c98 100644
--- a/cv-node/src/algorithms/opencv_sgbm.cpp
+++ b/cv-node/src/algorithms/opencv_sgbm.cpp
@@ -16,6 +16,7 @@ OpenCVSGBM::OpenCVSGBM(nlohmann::json &config) : Disparity(config) {
             left_matcher_->setP2(96*wsize*wsize);
             left_matcher_->setPreFilterCap(63);
             left_matcher_->setMode(StereoSGBM::MODE_SGBM_3WAY);
+            left_matcher_->setMinDisparity(config.value("minimum",0));
     wls_filter_ = createDisparityWLSFilter(left_matcher_);
     right_matcher_ = createRightMatcher(left_matcher_);
     
@@ -29,6 +30,9 @@ void OpenCVSGBM::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
 	left_matcher_-> compute(l, r,left_disp);
     right_matcher_->compute(r,l, right_disp);
     wls_filter_->filter(left_disp,l,disp,right_disp);
+    
+    //float f = 255.0f / static_cast<float>(max_disp_);
+    //disp *= f;
 }
 
 
diff --git a/cv-node/src/algorithms/rtcensus.cpp b/cv-node/src/algorithms/rtcensus.cpp
index 23939ed0021d5f4009da4db3be805c39e7be11dd..93a5a68e4302ba6fee0567951aa6242caf4a06c0 100644
--- a/cv-node/src/algorithms/rtcensus.cpp
+++ b/cv-node/src/algorithms/rtcensus.cpp
@@ -151,9 +151,25 @@ static cv::Mat consistency(cv::Mat &d_sub_r, cv::Mat &d_sub_l) {
 	return result;
 }
 
-RTCensus::RTCensus(nlohmann::json &config) : Disparity(config), gamma_(0.0f), tau_(0.0f) {}
+RTCensus::RTCensus(nlohmann::json &config)
+	:	Disparity(config),
+		gamma_(0.0f),
+		tau_(0.0f),
+		use_cuda_(config.value("use_cuda",true)) {}
 
 void RTCensus::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
+	#if defined HAVE_CUDA
+	if (use_cuda_) {
+		computeCUDA(l,r,disp);
+	} else {
+		computeCPU(l,r,disp);
+	}
+	#else // !HAVE_CUDA
+	computeCPU(l,r,disp);
+	#endif
+}
+
+void RTCensus::computeCPU(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
 	size_t d_min = min_disp_;
 	size_t d_max = max_disp_;
 
@@ -179,3 +195,32 @@ void RTCensus::compute(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
 	// TODO confidence and texture filtering
 }
 
+#if defined HAVE_CUDA
+
+using namespace cv::cuda;
+using namespace cv;
+
+namespace ftl { namespace gpu {
+void rtcensus_call(const PtrStepSzb &l, const PtrStepSzb &r, const PtrStepSz<float> &disp, size_t num_disp, const int &s=0);
+}}
+
+void RTCensus::computeCUDA(const cv::Mat &l, const cv::Mat &r, cv::Mat &disp) {
+	if (disp_.empty()) disp_ = cuda::GpuMat(l.size(), CV_32FC1);
+	//if (filtered_.empty()) filtered_ = cuda::GpuMat(l.size(), CV_8U);
+	if (left_.empty()) left_ = cuda::GpuMat(l.size(), CV_8U);
+	if (right_.empty()) right_ = cuda::GpuMat(l.size(), CV_8U);
+	
+	left_.upload(l);
+	right_.upload(r);
+	
+	auto start = std::chrono::high_resolution_clock::now();
+	ftl::gpu::rtcensus_call(left_, right_, disp_, max_disp_);
+	std::chrono::duration<double> elapsed = std::chrono::high_resolution_clock::now() - start;
+	LOG(INFO) << "CUDA census in " << elapsed.count() << "s";
+	//filter_->apply(disp_, left_, filtered_);
+	
+	disp_.download(disp);
+}
+
+#endif
+
diff --git a/cv-node/src/algorithms/rtcensus.cu b/cv-node/src/algorithms/rtcensus.cu
new file mode 100644
index 0000000000000000000000000000000000000000..4b7fffe77a2841ecd8e21a340c4a4cb6c042f6f7
--- /dev/null
+++ b/cv-node/src/algorithms/rtcensus.cu
@@ -0,0 +1,237 @@
+/*
+ * Author: Nicolas Pope and Sebastian Hahta (2019)
+ * Implementation of algorithm presented in article(s):
+ *
+ * [1] Humenberger, Engelke, Kubinger: A fast stereo matching algorithm suitable
+ *     for embedded real-time systems
+ * [2] Humenberger, Zinner, Kubinger: Performance Evaluation of Census-Based
+ *     Stereo Matching Algorithm on Embedded and Multi-Core Hardware
+ *
+ * Equation numbering uses [1] unless otherwise stated
+ *
+ */
+ 
+#include <opencv2/core/cuda/common.hpp>
+
+using namespace cv::cuda;
+using namespace cv;
+
+#define BLOCK_W 128
+#define RADIUS 7
+#define RADIUS2 2
+#define ROWSperTHREAD 20
+
+#define XHI(P1,P2) ((P1 <= P2) ? 0 : 1)
+
+namespace ftl {
+namespace gpu {
+
+__device__ uint64_t sparse_census(unsigned char *arr, size_t u, size_t v, size_t w) {
+	uint64_t r = 0;
+
+	unsigned char t = arr[v*w+u];
+
+	for (int n=-7; n<=7; n+=2) {
+	auto u_ = u + n;
+	for (int m=-7; m<=7; m+=2) {
+		auto v_ = v + m;
+		r <<= 1;
+		r |= XHI(t, arr[v_*w+u_]);
+	}
+	}
+
+	return r;
+}
+
+__device__ float fit_parabola(size_t pi, uint16_t p, uint16_t pl, uint16_t pr) {
+	float a = pr - pl;
+	float b = 2 * (2 * p - pl - pr);
+	return static_cast<float>(pi) + (a / b);
+}
+
+__global__ void census_kernel(PtrStepSzb l, PtrStepSzb r, uint64_t *census) {	
+	//extern __shared__ uint64_t census[];
+	
+	size_t u = (blockIdx.x * BLOCK_W + threadIdx.x + RADIUS);
+	size_t v_start = blockIdx.y * ROWSperTHREAD + RADIUS;
+	size_t v_end = v_start + ROWSperTHREAD;
+	
+	if (v_end >= l.rows) v_end = l.rows;
+	if (u >= l.cols) return;
+	
+	size_t width = l.cols;
+	
+	for (size_t v=v_start; v<v_end; v++) {
+	//for (size_t u=7; u<width-7; u++) {
+		size_t ix = (u + v*width) * 2;
+		uint64_t cenL = sparse_census(l.data, u, v, l.step);
+		uint64_t cenR = sparse_census(r.data, u, v, r.step);
+		
+		census[ix] = cenL;
+		census[ix + 1] = cenR;
+		
+		//disp(v,u) = (float)cenL;
+	//}
+	}
+	
+	//__syncthreads();
+	
+	return;
+}
+	
+__global__ void disp_kernel(PtrStepSz<float> disp, size_t width, size_t height, uint64_t *census, size_t ds) {	
+	//extern __shared__ uint64_t census[];
+	
+	size_t u = (blockIdx.x * BLOCK_W) + threadIdx.x + RADIUS2;
+	size_t v_start = (blockIdx.y * ROWSperTHREAD) + RADIUS2;
+	size_t v_end = v_start + ROWSperTHREAD;
+	
+	if (v_end >= height) v_end = height;
+	if (u >= width-ds) return;
+	
+	for (size_t v=v_start; v<v_end; v++) {
+	//for (size_t u=7; u<width-7; u++) {
+	//const size_t eu = (sign>0) ? w-2-ds : w-2;
+
+	//for (size_t v=7; v<height-7; v++) {
+	//for (size_t u=7; u<width-7; u++) {
+		//const size_t ix = v*w*ds+u*ds;
+		
+		uint16_t last_ham[2] = {65535,65535};
+		uint16_t min_disp[2] = {65535,65535};
+		uint16_t min_before[2] = {0,0};
+		uint16_t min_after[2] = {0,0};
+		size_t dix[2] = {0,0};
+		
+		for (size_t d=0; d<ds; d++) {
+			uint16_t hamming1 = 0;
+			uint16_t hamming2 = 0;
+			
+			//if (u+2+ds >= width) break;
+		
+			for (int n=-2; n<=2; n++) {
+				const auto u_ = u + n;
+
+				for (int m=-2; m<=2; m++) {
+					const auto v_ = (v + m)*width;
+					//const auto d_ = d; // * sign;
+					auto l1 = census[(u_+v_)*2 + 1];
+					auto r1 = census[(v_+(u_+d))*2];
+					auto l2 = census[(u_+ds+v_)*2];
+					auto r2 = census[(v_+(u_+ds-d))*2 + 1];
+					//auto l2 = census[((u_+ds+v_)*2)+1];
+					//auto r2 = census[(v_+(u_+ds-d))*2];
+					hamming1 += __popcll(r1^l1);
+					hamming2 += __popcll(r2^l2);
+				}
+			}
+			
+			if (hamming1 < min_disp[0]) {
+				min_before[0] = last_ham[0];
+				min_disp[0] = hamming1;
+				dix[0] = d;
+			}
+			if (dix[0] == d) min_after[0] = hamming1;
+			last_ham[0] = hamming1;
+			
+			if (hamming2 < min_disp[1]) {
+				min_before[1] = last_ham[1];
+				min_disp[1] = hamming2;
+				dix[1] = d;
+			}
+			if (dix[1] == d) min_after[1] = hamming2;
+			last_ham[1] = hamming2;
+		
+		}
+		
+		float d1 = (dix[0] == 0 || dix[0] == ds-1) ? (float)dix[0] : fit_parabola(dix[0], min_disp[0], min_before[0], min_after[0]);
+		float d2 = (dix[1] == 0 || dix[1] == ds-1) ? (float)dix[1] : fit_parabola(dix[1], min_disp[1], min_before[1], min_after[1]);
+	
+		if (abs(d1-d2) <= 1.0) disp(v,u) = abs((d1+d2)/2);
+		else disp(v,u) = 0.0f;
+		
+		//disp(v,u) = d2;
+	
+		//disp_l[v*width+u] = d2;
+		//disp_r[v*width+u] = d1;
+	}
+}
+
+/*__global__ void consistency_kernel(float *d_sub_l, float *d_sub_r, PtrStepSz<float> disp) {
+	size_t w = disp.cols;
+	size_t h = disp.rows;
+	//Mat result = Mat::zeros(Size(w,h), CV_32FC1);
+	
+	size_t u = (blockIdx.x * BLOCK_W) + threadIdx.x + RADIUS;
+	size_t v_start = (blockIdx.y * ROWSperTHREAD) + RADIUS;
+	size_t v_end = v_start + ROWSperTHREAD;
+	
+	if (v_end >= disp.rows) v_end = disp.rows;
+	if (u >= w) return;
+	
+	for (size_t v=v_start; v<v_end; v++) {
+	
+		int a = (int)(d_sub_l[v*w+u]);
+		if ((int)u-a < 0) continue;
+		
+		auto b = d_sub_r[v*w+u-a];
+		
+		if (abs(a-b) <= 1.0) disp(v,u) = abs((a+b)/2);
+		else disp(v,u) = 0.0f;
+	//}
+	}
+
+}*/
+
+/*__global__ void test_kernel(const PtrStepSzb l, const PtrStepSzb r, PtrStepSz<float> disp)
+{
+	int x = threadIdx.x + blockIdx.x * blockDim.x;
+	int y = threadIdx.y + blockIdx.y * blockDim.y;
+	if (x < l.cols && y < l.rows) {
+		const unsigned char lv = l(y, x);
+		const unsigned char rv = r(y, x);
+		disp(y, x) = (float)lv - (float)rv; //make_uchar1(v.z, v.y, v.x);
+	}
+}*/
+
+void rtcensus_call(const PtrStepSzb &l, const PtrStepSzb &r, const PtrStepSz<float> &disp, size_t num_disp, const int &stream) {
+	dim3 grid(1,1,1);
+    dim3 threads(BLOCK_W, 1, 1);
+
+	grid.x = cv::cuda::device::divUp(l.cols - 2 * RADIUS, BLOCK_W);
+	grid.y = cv::cuda::device::divUp(l.rows - 2 * RADIUS, ROWSperTHREAD);
+	
+	// TODO, reduce allocations
+	uint64_t *census;
+	//float *disp_l;
+	//float *disp_r;
+	cudaMalloc(&census, sizeof(uint64_t)*l.cols*l.rows*2);
+	//cudaMemset(census, 0, sizeof(uint64_t)*l.cols*l.rows*2);
+	//cudaMalloc(&disp_l, sizeof(float)*l.cols*l.rows);
+	//cudaMalloc(&disp_r, sizeof(float)*l.cols*l.rows);
+	
+	//size_t smem_size = (2 * l.cols * l.rows) * sizeof(uint64_t);
+	
+	census_kernel<<<grid, threads>>>(l, r, census);
+	cudaSafeCall( cudaGetLastError() );
+	
+	grid.x = cv::cuda::device::divUp(l.cols - 2 * RADIUS2, BLOCK_W);
+	grid.y = cv::cuda::device::divUp(l.rows - 2 * RADIUS2, ROWSperTHREAD);
+	
+	//grid.x = cv::cuda::device::divUp(l.cols - 2 * RADIUS - num_disp, BLOCK_W) - 1;
+	disp_kernel<<<grid, threads>>>(disp, l.cols, l.rows, census, num_disp);
+	cudaSafeCall( cudaGetLastError() );
+	
+	//consistency_kernel<<<grid, threads>>>(disp_l, disp_r, disp);
+	//cudaSafeCall( cudaGetLastError() );
+	
+	//cudaFree(disp_r);
+	//cudaFree(disp_l);
+	cudaFree(census);
+	
+	//if (&stream == Stream::Null())
+		cudaSafeCall( cudaDeviceSynchronize() );
+}
+
+};
+};
diff --git a/cv-node/src/calibrate.cpp b/cv-node/src/calibrate.cpp
index ca5bea249353b1028b94b8732128c70bfe876534..c4e07f071bc334a724fe775777500751a758d17d 100644
--- a/cv-node/src/calibrate.cpp
+++ b/cv-node/src/calibrate.cpp
@@ -344,7 +344,6 @@ bool Calibrate::recalibrate() {
 
 bool Calibrate::_recalibrate(vector<vector<Point2f>> *imagePoints,
 		Mat *cameraMatrix, Mat *distCoeffs, Size *imageSize) {
-	// TODO WHAT IS WINSIZE!!
 	int winSize = 11; //parser.get<int>("winSize");
 
     float grid_width = settings_.squareSize * (settings_.boardSize.width - 1);
diff --git a/cv-node/src/local.cpp b/cv-node/src/local.cpp
index 4342d9581fde66909fb9c1105d645d0d037c03d4..3bd14954716a0e28ef9e27e016a7cf5c5ca8ad90 100644
--- a/cv-node/src/local.cpp
+++ b/cv-node/src/local.cpp
@@ -71,8 +71,10 @@ LocalSource::LocalSource(const string &vid, nlohmann::json &config)
 	}
 
 	if (frame.cols >= 2*frame.rows) {
+		LOG(INFO) << "Video size : " << frame.cols/2 << "x" << frame.rows;
 		stereo_ = true;
 	} else {
+		LOG(INFO) << "Video size : " << frame.cols << "x" << frame.rows;
 		stereo_ = false;
 	}
 }
diff --git a/cv-node/src/main.cpp b/cv-node/src/main.cpp
index afa9be6571abb2da57fbf75cd750b5973ef4ad60..bf89661252df17e10b2aa05bd38b96068b53624b 100644
--- a/cv-node/src/main.cpp
+++ b/cv-node/src/main.cpp
@@ -121,11 +121,12 @@ int main(int argc, char **argv) {
     // Choose and configure disparity algorithm
     auto disparity = Disparity::create(config["disparity"]);
 	
-	Mat l, r, disparity32F, lbw, rbw;
+	Mat l, r, disparity32F, depth32F, lbw, rbw;
 	
 	cv::viz::Viz3d myWindow("FTL");
 	
-	float fact = (float)(config["camera"]["focal_length"]) / (float)(config["camera"]["sensor_width"]);
+	float base_line = (float)config["camera"]["base_line"];
+	float focal = (float)(config["camera"]["focal_length"]) / (float)(config["camera"]["sensor_width"]);
 	Mat rot_vec = Mat::zeros(1,3,CV_32F);
 	
 	while (!myWindow.wasStopped()) {
@@ -145,52 +146,69 @@ int main(int argc, char **argv) {
         cvtColor(r, rbw, COLOR_BGR2GRAY);
         
         disparity->compute(lbw,rbw,disparity32F);
-		LOG(INFO) << "Disparity complete ";
+		//LOG(INFO) << "Disparity complete ";
 		
 		disparity32F.convertTo(disparity32F, CV_32F);
+		disparity32F += 10.0f;
+		Rect rect((int)config["disparity"]["maximum"],7,disparity32F.cols-(int)config["disparity"]["maximum"],disparity32F.rows-14);
+		disparity32F = disparity32F(rect);
+		l = l(rect);
 		
 		// TODO Send RGB+D data somewhere
 		
 		// Convert disparity to depth
-		disparity32F = (fact * (float)l.cols * 0.1f) / disparity32F;
 
-		//normalize(filtered_disp, filtered_disp, 0, 255, NORM_MINMAX, CV_8U);
+		//normalize(disparity32F, disparity32F, 0, 255, NORM_MINMAX, CV_8U);
 		
 		//cv::imshow("Disparity",filtered_disp);
 		//Mat i3d;
 		//const Mat &Q = calibrate.getQ();
-		cv::Mat Q_32F;
-		calibrate.getQ().convertTo(Q_32F,CV_32F);
-		cv::Mat_<cv::Vec3f> XYZ(disparity32F.rows,disparity32F.cols);   // Output point cloud
-		reprojectImageTo3D(disparity32F, XYZ, Q_32F, true);
 		
-		//cv::imshow("Points",XYZ);
-		
-		cv::viz::WCloud cloud_widget = cv::viz::WCloud( XYZ, l );
-    	cloud_widget.setRenderingProperty( cv::viz::POINT_SIZE, 2 );
-    	
-    	/* Rotation using rodrigues */
-        /// Rotate around (1,1,1)
-        rot_vec.at<float>(0,0) = 0.0f;
-        rot_vec.at<float>(0,1) = 0.0f;
-        rot_vec.at<float>(0,2) = CV_PI * 1.0f;
 
-        Mat rot_mat;
-        Rodrigues(rot_vec, rot_mat);
+		if (config["display"]["points"]) {
+			cv::Mat Q_32F; // = (Mat_<double>(4,4) << 1, 0, 0, 0,  0, 1, 0, 0,  0, 0, 1, 0,  0, 0, 0, 1); //(4,4,CV_32F);
+			calibrate.getQ().convertTo(Q_32F,CV_32F);
+			cv::Mat_<cv::Vec3f> XYZ(disparity32F.rows,disparity32F.cols);   // Output point cloud
+			reprojectImageTo3D(disparity32F, XYZ, Q_32F, false);
+			
+			//cv::imshow("Points",XYZ);
+			
+			cv::viz::WCloud cloud_widget = cv::viz::WCloud( XYZ, l );
+			cloud_widget.setRenderingProperty( cv::viz::POINT_SIZE, 2 );
+			
+			/* Rotation using rodrigues */
+		    /// Rotate around (1,1,1)
+		    rot_vec.at<float>(0,0) = 0.0f;
+		    rot_vec.at<float>(0,1) = 0.0f;
+		    rot_vec.at<float>(0,2) = CV_PI * 1.0f;
 
-        /// Construct pose
-        Affine3f pose(rot_mat, Vec3f(0, 0, 0));
+		    Mat rot_mat;
+		    Rodrigues(rot_vec, rot_mat);
 
-		//myWindow.showWidget( "coosys", viz::WCoordinateSystem() );
-		myWindow.showWidget( "Depth", cloud_widget );
-		myWindow.setWidgetPose("Depth", pose);
+		    /// Construct pose
+		    Affine3f pose(rot_mat, Vec3f(0, 20.0, 0));
+			myWindow.showWidget( "coosys", viz::WCoordinateSystem() );
+			myWindow.showWidget( "Depth", cloud_widget );
+			myWindow.setWidgetPose("Depth", pose);
 
-		myWindow.spinOnce( 30, true );
+			myWindow.spinOnce( 30, true );
+		}
 		
-		//if(cv::waitKey(10) == 27){
-            //exit if ESC is pressed
-        //    break;
-        //}
+		if (config["display"]["depth"]) {
+			depth32F = (focal * (float)l.cols * base_line) / disparity32F;
+			cv::imshow("Depth", depth32F);
+			if(cv::waitKey(10) == 27){
+		        //exit if ESC is pressed
+		        break;
+		    }
+        } else if (config["display"]["disparity"]) {
+        	normalize(disparity32F, disparity32F, 0, 255, NORM_MINMAX, CV_8U);
+			cv::imshow("Disparity", disparity32F);
+			if(cv::waitKey(10) == 27){
+		        //exit if ESC is pressed
+		        break;
+		    }
+        }
 	}
 }