parameters+template for disp2depth and depth2disp

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

@@ -3,11 +3,13 @@
 namespace ftl {
 namespace cuda {
 
-void disparity_to_depth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth,
-						const ftl::rgbd::Camera &c, cudaStream_t &stream);
+template<typename T_in, typename T_out>
+void disparity_to_depth(const cv::cuda::GpuMat &depth, cv::cuda::GpuMat &disparity,
+						const ftl::rgbd::Camera &c, const float scale=1.0f, cudaStream_t &stream=0);
 
-void depth_to_disparity(cv::cuda::GpuMat &disparity, const cv::cuda::GpuMat &depth,
-						const ftl::rgbd::Camera &c, cudaStream_t &stream);
+template<typename T_in, typename T_out>
+void depth_to_disparity(const cv::cuda::GpuMat &depth, cv::cuda::GpuMat &disparity,
+						const ftl::rgbd::Camera &c, const float scale=1.0f, cudaStream_t &stream=0);
 
 void remove_occlusions(cv::cuda::GpuMat &depth, const cv::cuda::GpuMat &depthR,
 						const ftl::rgbd::Camera &c, cudaStream_t stream);

components/operators/src/disparity/bilateral_filter.cpp

@@ -39,7 +39,7 @@ bool DisparityBilateralFilter::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out
 
 			//LOG(ERROR) << "Calculated disparity from depth";
 
-			ftl::cuda::depth_to_disparity(disp, depth, params, stream);
+			ftl::cuda::depth_to_disparity<float, short>(depth, disp, params, 16.0f, stream);
 		} else {
 			throw FTL_Error("Joint Bilateral Filter is missing Disparity and Depth");
 			return false;

components/operators/src/disparity/disp2depth.cu

@@ -8,58 +8,70 @@
 #define PINF __int_as_float(0x7f800000)
 #endif
 
-__global__ void d2d_kernel(cv::cuda::PtrStepSz<short> disp, cv::cuda::PtrStepSz<float> depth,
-		ftl::rgbd::Camera cam) {
+template<typename T_in, typename T_out>
+__global__ void d2d_kernel(cv::cuda::PtrStepSz<T_in> disp, cv::cuda::PtrStepSz<T_out> depth,
+		const ftl::rgbd::Camera cam, const float scale) {
 
 	for (STRIDE_Y(v,disp.rows)) {
 	for (STRIDE_X(u,disp.cols)) {
 		short d = disp(v,u);
-		depth(v,u) = (d == 0) ? 0.0f : ((cam.baseline*cam.fx) / ((float(d)/16.0f) - cam.doffs));
+		depth(v,u) = (d == 0) ? 0.0f : ((cam.baseline*cam.fx) / ((float(d)*scale) - cam.doffs));
 	}
 	}
 }
 
 namespace ftl {
 namespace cuda {
+
+	template<typename T_in, typename T_out>
 	void disparity_to_depth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth,
-				const ftl::rgbd::Camera &c, cudaStream_t &stream) {
+				const ftl::rgbd::Camera &c, const float scale, 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);
-		d2d_kernel<<<grid, threads, 0, stream>>>(
-			disparity, depth, c);
+		d2d_kernel<T_in, T_out><<<grid, threads, 0, stream>>>(
+			disparity, depth, c, scale);
 		cudaSafeCall( cudaGetLastError() );
 	}
+
+	template void disparity_to_depth<short, float>(const cv::cuda::GpuMat&, cv::cuda::GpuMat&, const ftl::rgbd::Camera&, const float, cudaStream_t&);
+	template void disparity_to_depth<float, float>(const cv::cuda::GpuMat&, cv::cuda::GpuMat&, const ftl::rgbd::Camera&, const float, cudaStream_t&);
 }
 }
 
 //==============================================================================
 
-__global__ void d2drev_kernel(cv::cuda::PtrStepSz<short> disp, cv::cuda::PtrStepSz<float> depth,
-	ftl::rgbd::Camera cam) {
+template<typename T_in, typename T_out>
+__global__ void d2drev_kernel(cv::cuda::PtrStepSz<T_in> disp, cv::cuda::PtrStepSz<T_out> depth,
+	const ftl::rgbd::Camera cam, const float scale) {
 
 	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) = short(disparity*16.0f);
-}
-}
+		disp(v,u) = T_out(disparity*scale);
+	}}
 }
 
 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) {
+
+	template<typename T_in, typename T_out>
+	void depth_to_disparity(const cv::cuda::GpuMat &depth, cv::cuda::GpuMat &disparity,
+				const ftl::rgbd::Camera &c, const float scale, 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);
+		d2drev_kernel<T_in, T_out><<<grid, threads, 0, stream>>>(
+			disparity, depth, c, scale);
 		cudaSafeCall( cudaGetLastError() );
 	}
+
+	template void depth_to_disparity<float, float>(const cv::cuda::GpuMat&, cv::cuda::GpuMat&, const ftl::rgbd::Camera&, const float, cudaStream_t&);
+	template void depth_to_disparity<float, short>(const cv::cuda::GpuMat&, cv::cuda::GpuMat&, const ftl::rgbd::Camera&, const float, cudaStream_t&);
+
 }
 }
 

components/operators/src/disparity/disparity_to_depth.cpp

@@ -19,6 +19,6 @@ bool DisparityToDepth::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out,
 	GpuMat &depth = out.create<GpuMat>(Channel::Depth);
 	depth.create(disp.size(), CV_32FC1);
 
-	ftl::cuda::disparity_to_depth(disp, depth, params, stream);
+	ftl::cuda::disparity_to_depth<short, float>(disp, depth, params, 1.0f/16.0f, stream);
 	return true;
 }
\ No newline at end of file