diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt
index 5329ac9f2892c172af9cbc5f7fdc6b0e0e82360f..1e6c15dc339094c56fc4fd55cb23ebb165c32cf0 100644
--- a/components/rgbd-sources/CMakeLists.txt
+++ b/components/rgbd-sources/CMakeLists.txt
@@ -32,6 +32,7 @@ endif (LIBSGM_FOUND)
if (CUDA_FOUND)
list(APPEND RGBDSRC
+ src/algorithms/disp2depth.cu
# "src/algorithms/opencv_cuda_bm.cpp"
# "src/algorithms/opencv_cuda_bp.cpp"
# "src/algorithms/rtcensus.cu"
diff --git a/components/rgbd-sources/src/algorithms/disp2depth.cu b/components/rgbd-sources/src/algorithms/disp2depth.cu
new file mode 100644
index 0000000000000000000000000000000000000000..72298892dbbcfc60a602f2fd5c206f7c74287058
--- /dev/null
+++ b/components/rgbd-sources/src/algorithms/disp2depth.cu
@@ -0,0 +1,29 @@
+#include <ftl/cuda_common.hpp>
+#include <ftl/rgbd/camera.hpp>
+#include <opencv2/core/cuda_stream_accessor.hpp>
+
+__global__ void d2d_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 = disp(v,u);
+ depth(v,u) = (d == 0) ? 50.0f : ((cam.baseline*cam.fx) / (d + cam.doffs)) / 1000.0f;
+ }
+ }
+}
+
+namespace ftl {
+namespace cuda {
+ void disparity_to_depth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth,
+ const ftl::rgbd::Camera &c, cv::cuda::Stream &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, cv::cuda::StreamAccessor::getStream(stream)>>>(
+ disparity, depth, c);
+ cudaSafeCall( cudaGetLastError() );
+ }
+}
+}
diff --git a/components/rgbd-sources/src/cuda_algorithms.hpp b/components/rgbd-sources/src/cuda_algorithms.hpp
index 1a70018de8c2f0e9ad3b629ec03530453cc0dccf..7c7d510ffebbb6ab4a42c7a2540b17d10c9a34e2 100644
--- a/components/rgbd-sources/src/cuda_algorithms.hpp
+++ b/components/rgbd-sources/src/cuda_algorithms.hpp
@@ -6,6 +6,7 @@
#define _FTL_CUDA_ALGORITHMS_HPP_
#include <ftl/cuda_common.hpp>
+#include <ftl/rgbd/camera.hpp>
namespace ftl {
namespace cuda {
@@ -34,6 +35,10 @@ namespace cuda {
void texture_map(const TextureObject<uchar4> &t,
TextureObject<float> &f);
+ void disparity_to_depth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth,
+ const ftl::rgbd::Camera &c, cv::cuda::Stream &stream);
+
+
}
}
diff --git a/components/rgbd-sources/src/middlebury_source.cpp b/components/rgbd-sources/src/middlebury_source.cpp
index 296d7b6e8c95520df9824316a89f4bf5d74c489a..6f38f7bdcf9cd9a1de8812934e8da9c460face47 100644
--- a/components/rgbd-sources/src/middlebury_source.cpp
+++ b/components/rgbd-sources/src/middlebury_source.cpp
@@ -1,6 +1,7 @@
#include "middlebury_source.hpp"
#include "disparity.hpp"
+#include "cuda_algorithms.hpp"
using ftl::rgbd::detail::MiddleburySource;
using ftl::rgbd::detail::Disparity;
@@ -183,7 +184,8 @@ void MiddleburySource::_performDisparity() {
if (disp_tmp_.empty()) disp_tmp_ = cv::cuda::GpuMat(left_.size(), CV_32FC1);
//calib_->rectifyStereo(left_, right_, stream_);
disp_->compute(left_, right_, disp_tmp_, stream_);
- disparityToDepth(disp_tmp_, depth_tmp_, params_, stream_);
+ //disparityToDepth(disp_tmp_, depth_tmp_, params_, stream_);
+ ftl::cuda::disparity_to_depth(disp_tmp_, depth_tmp_, params_, stream_);
//left_.download(rgb_, stream_);
//rgb_ = lsrc_->cachedLeft();
depth_tmp_.download(depth_, stream_);
diff --git a/components/rgbd-sources/src/net.cpp b/components/rgbd-sources/src/net.cpp
index b172719e8e499b7c123329f03d72a9512cf068b7..25270084d577634c2e412f8e6f15c9fe847cf0b4 100644
--- a/components/rgbd-sources/src/net.cpp
+++ b/components/rgbd-sources/src/net.cpp
@@ -40,6 +40,7 @@ bool NetSource::_getCalibration(Universe &net, const UUID &peer, const string &s
host_->getConfig()["centre_x"] = p.cx;
host_->getConfig()["centre_y"] = p.cy;
host_->getConfig()["baseline"] = p.baseline;
+ host_->getConfig()["doffs"] = p.doffs;
return true;
} else {
@@ -79,6 +80,11 @@ NetSource::NetSource(ftl::rgbd::Source *host)
host_->getNet()->send(peer_, "update_cfg", host_->getURI() + "/focal", host_->getConfig()["focal"].dump());
});
+ host->on("doffs", [this,host](const ftl::config::Event&) {
+ params_.doffs = host_->value("doffs", params_.doffs);
+ host_->getNet()->send(peer_, "update_cfg", host_->getURI() + "/doffs", host_->getConfig()["doffs"].dump());
+ });
+
host->on("baseline", [this,host](const ftl::config::Event&) {
params_.baseline = host_->value("baseline", 400.0);
host_->getNet()->send(peer_, "update_cfg", host_->getURI() + "/baseline", host_->getConfig()["baseline"].dump());
diff --git a/components/rgbd-sources/src/realsense_source.cpp b/components/rgbd-sources/src/realsense_source.cpp
index 08967ae7814ed82377773a41550cba884ede89c6..d6c6f487be89c3eafd4e3e8e6020272dd93e8e9d 100644
--- a/components/rgbd-sources/src/realsense_source.cpp
+++ b/components/rgbd-sources/src/realsense_source.cpp
@@ -32,6 +32,7 @@ RealsenseSource::RealsenseSource(ftl::rgbd::Source *host)
params_.fy = intrin.fy;
params_.maxDepth = 11.0;
params_.minDepth = 0.1;
+ params_.doffs = 0.0;
LOG(INFO) << "Realsense Intrinsics: " << params_.fx << "," << params_.fy << " - " << params_.cx << "," << params_.cy << " - " << params_.width;
}
diff --git a/components/rgbd-sources/src/stereovideo.cpp b/components/rgbd-sources/src/stereovideo.cpp
index dfb8df8c41063452342fcf6fdbf56566cb23bace..ec040740e7d19e5b6ad5f47fe18eba3acae234f7 100644
--- a/components/rgbd-sources/src/stereovideo.cpp
+++ b/components/rgbd-sources/src/stereovideo.cpp
@@ -5,6 +5,7 @@
#include "calibrate.hpp"
#include "local.hpp"
#include "disparity.hpp"
+#include "cuda_algorithms.hpp"
using ftl::rgbd::detail::Calibrate;
using ftl::rgbd::detail::LocalSource;
@@ -69,14 +70,17 @@ void StereoVideoSource::init(const string &file) {
(unsigned int)lsrc_->height(),
0.0f, // 0m min
15.0f, // 15m max
- 1.0 / calib_->getQ().at<double>(3,2) // Baseline
+ 1.0 / calib_->getQ().at<double>(3,2), // Baseline
+ 0.0f // doffs
};
+ params_.doffs = -calib_->getQ().at<double>(3,3) * params_.baseline;
// Add calibration to config object
host_->getConfig()["focal"] = params_.fx;
host_->getConfig()["centre_x"] = params_.cx;
host_->getConfig()["centre_y"] = params_.cy;
host_->getConfig()["baseline"] = params_.baseline;
+ host_->getConfig()["doffs"] = params_.doffs;
// Add event handlers to allow calibration changes...
host_->on("baseline", [this](const ftl::config::Event &e) {
@@ -91,6 +95,10 @@ void StereoVideoSource::init(const string &file) {
UNIQUE_LOCK(host_->mutex(), lk);
calib_->updateCalibration(params_);
});
+
+ host_->on("doffs", [this](const ftl::config::Event &e) {
+ params_.doffs = host_->value("doffs", params_.doffs);
+ });
// left and right masks (areas outside rectified images)
// only left mask used
@@ -130,7 +138,7 @@ bool StereoVideoSource::grab(int n, int b) {
if (disp_tmp_.empty()) disp_tmp_ = cv::cuda::GpuMat(left_.size(), CV_32FC1);
calib_->rectifyStereo(left_, right_, stream_);
disp_->compute(left_, right_, disp_tmp_, stream_);
- disparityToDepth(disp_tmp_, depth_tmp_, calib_->getQ(), stream_);
+ ftl::cuda::disparity_to_depth(disp_tmp_, depth_tmp_, params_, stream_);
left_.download(rgb_, stream_);
//rgb_ = lsrc_->cachedLeft();
depth_tmp_.download(depth_, stream_);