diff --git a/components/operators/CMakeLists.txt b/components/operators/CMakeLists.txt
index e4f7fa533fb9b36d3fcda6851cad6628e6292ee4..da52eac6a1e754a3dec32a8f10ed424ef0214760 100644
--- a/components/operators/CMakeLists.txt
+++ b/components/operators/CMakeLists.txt
@@ -9,8 +9,10 @@ add_library(ftloperators
src/filling.cpp
src/filling.cu
src/nvopticalflow.cpp
- src/disparity/optflow_smoothing.cpp
src/disparity/optflow_smoothing.cu
+ src/disparity/optflow_smoothing.cpp
+ src/disparity/disp2depth.cu
+ src/disparity/disparity_to_depth.cpp
)
# These cause errors in CI build and are being removed from PCL in newer versions
diff --git a/components/operators/include/ftl/operators/disparity.hpp b/components/operators/include/ftl/operators/disparity.hpp
index 8e7758107124fe54cd7a3d46f9ddd6b30be00cad..7588a883f056f344afa728a482e50a0ce5874338 100644
--- a/components/operators/include/ftl/operators/disparity.hpp
+++ b/components/operators/include/ftl/operators/disparity.hpp
@@ -1,6 +1,5 @@
#pragma once
-
#include <ftl/operators/operator.hpp>
#include <opencv2/cudaoptflow.hpp>
@@ -8,7 +7,20 @@ namespace ftl {
namespace operators {
/*
- * Optical flow smoothing for disparity (or depth)
+ * Calculate depth from disparity
+ */
+class DisparityToDepth : public ftl::operators::Operator {
+ public:
+ explicit DisparityToDepth(ftl::Configurable* cfg) :
+ ftl::operators::Operator(cfg) {}
+
+ ~DisparityToDepth() {};
+ inline Operator::Type type() const override { return Operator::Type::OneToOne; }
+ bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *src, cudaStream_t stream) override;
+};
+
+/*
+ * Optical flow smoothing for depth
*/
class DisparitySmoothingOF : public ftl::operators::Operator {
public:
@@ -20,9 +32,14 @@ class DisparitySmoothingOF : public ftl::operators::Operator {
bool apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out, ftl::rgbd::Source *src, cudaStream_t stream) override;
private:
+ void init();
+
+ const ftl::codecs::Channel channel_ = ftl::codecs::Channel::Depth;
cv::cuda::GpuMat history_;
+ cv::Size size_;
+ int n_max_;
+ float threshold_;
};
-
}
}
diff --git a/components/operators/src/disparity/optflow_smoothing.hpp b/components/operators/src/disparity/cuda.hpp
similarity index 62%
rename from components/operators/src/disparity/optflow_smoothing.hpp
rename to components/operators/src/disparity/cuda.hpp
index 8deb30f4ce593a03c0ee1093c25e504f900d5d7f..d4cf06aaec772869ee3e8f2e8fafab9157115f57 100644
--- a/components/operators/src/disparity/optflow_smoothing.hpp
+++ b/components/operators/src/disparity/cuda.hpp
@@ -3,10 +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);
+
+
void optflow_filter(cv::cuda::GpuMat &disp, const cv::cuda::GpuMat &optflow,
cv::cuda::GpuMat &history, int n_max, float threshold,
cv::cuda::Stream &stream);
-
}
}
diff --git a/components/rgbd-sources/src/algorithms/disp2depth.cu b/components/operators/src/disparity/disp2depth.cu
similarity index 74%
rename from components/rgbd-sources/src/algorithms/disp2depth.cu
rename to components/operators/src/disparity/disp2depth.cu
index 4b707c4795017cdf38999cd6606d53ff4870ccb5..86a3fc290967fc0a64dd1a61df61badaef0aa833 100644
--- a/components/rgbd-sources/src/algorithms/disp2depth.cu
+++ b/components/operators/src/disparity/disp2depth.cu
@@ -16,12 +16,12 @@ __global__ void d2d_kernel(cv::cuda::PtrStepSz<float> disp, cv::cuda::PtrStepSz<
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) {
+ 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);
+ 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)>>>(
+ d2d_kernel<<<grid, threads, 0, stream>>>(
disparity, depth, c);
cudaSafeCall( cudaGetLastError() );
}
diff --git a/components/operators/src/disparity/disparity_to_depth.cpp b/components/operators/src/disparity/disparity_to_depth.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..baed8acfdc8689d695daaf044a471a29cd02221e
--- /dev/null
+++ b/components/operators/src/disparity/disparity_to_depth.cpp
@@ -0,0 +1,18 @@
+#include "ftl/operators/disparity.hpp"
+#include "disparity/cuda.hpp"
+
+using ftl::operators::DisparityToDepth;
+using ftl::codecs::Channel;
+
+using cv::cuda::GpuMat;
+
+bool DisparityToDepth::apply(ftl::rgbd::Frame &in, ftl::rgbd::Frame &out,
+ ftl::rgbd::Source *src, cudaStream_t stream) {
+
+ const auto params = src->parameters();
+ const GpuMat &disp = in.get<GpuMat>(Channel::Disparity);
+ GpuMat depth = out.get<GpuMat>(Channel::Depth);
+
+ ftl::cuda::disparity_to_depth(disp, depth, params, stream);
+ return true;
+}
\ No newline at end of file
diff --git a/components/operators/src/disparity/optflow_smoothing.cpp b/components/operators/src/disparity/optflow_smoothing.cpp
index fcfa1aa50c461e2e3101839b7d251aaf5a7c7fa9..8dffdaa4cf3c9e288faa38b4eb1e6e9a38a37a75 100644
--- a/components/operators/src/disparity/optflow_smoothing.cpp
+++ b/components/operators/src/disparity/optflow_smoothing.cpp
@@ -2,7 +2,7 @@
#include "ftl/operators/disparity.hpp"
#include "ftl/offilter.hpp"
-#include "disparity/optflow_smoothing.hpp"
+#include "disparity/cuda.hpp"
#ifdef HAVE_OPTFLOW
@@ -21,29 +21,73 @@ template<typename T> static bool inline isValidDisparity(T d) { return (0.0 < d)
DisparitySmoothingOF::DisparitySmoothingOF(ftl::Configurable* cfg) :
ftl::operators::Operator(cfg) {
- // TODO read params
- /*
- CHECK((n_max_ > 1) && (n_max_ <= 32)) << "History length must be between 0 and 31!";
- disp_old_ = cv::cuda::GpuMat(cv::Size(size.width * n_max_, size.height), CV_32FC1);
- */
+
+ size_ = Size(0, 0);
+
+ n_max_ = cfg->value("history_size", 7);
+ if (n_max_ < 1) {
+ LOG(WARNING) << "History length must be larger than 0";
+ n_max_ = 7;
+ }
+
+ if (n_max_ > 32) {
+ // TODO: cuda kernel uses fixed size buffer
+ LOG(WARNING) << "History length can't be larger than 32 (TODO)";
+ n_max_ = 32;
+ }
+
+ threshold_ = cfg->value("threshold", 1.0);
+
+ cfg->on("threshold", [this, &cfg](const ftl::config::Event&) {
+ float threshold = cfg->value("threshold", 1.0);
+ if (threshold < 0.0) {
+ LOG(WARNING) << "invalid threshold " << threshold << ", value must be positive";
+ }
+ else {
+ threshold_ = threshold;
+ init();
+ }
+ });
+
+ cfg->on("history_size", [this, &cfg](const ftl::config::Event&) {
+ int n_max = cfg->value("history_size", 1.0);
+
+ if (n_max < 1) {
+ LOG(WARNING) << "History length must be larger than 0";
+ }
+ else if (n_max_ > 32) {
+ // TODO: cuda kernel uses fixed size buffer
+ LOG(WARNING) << "History length can't be larger than 32 (TODO)";
+ }
+ else {
+ n_max_ = n_max;
+ init();
+ }
+ });
}
DisparitySmoothingOF::~DisparitySmoothingOF() {}
-bool DisparitySmoothingOF::apply(Frame &in, Frame &out, Source *src, cudaStream_t stream) {
-/*
-void OFDisparityFilter::filter(ftl::rgbd::Frame &frame, cv::cuda::Stream &stream)
-{
- frame.upload(Channel::Flow, stream);
- const cv::cuda::GpuMat &optflow = frame.get<cv::cuda::GpuMat>(Channel::Flow);
- //frame.get<cv::cuda::GpuMat>(Channel::Disparity);
- stream.waitForCompletion();
- if (optflow.empty()) { return; }
-
- cv::cuda::GpuMat &disp = frame.create<cv::cuda::GpuMat>(Channel::Disparity);
- ftl::cuda::optflow_filter(disp, optflow, disp_old_, n_max_, threshold_, stream);
+void DisparitySmoothingOF::init() {
+ if (size_ == Size(0, 0)) { return; }
+ history_.create(cv::Size(size_.width * n_max_, size_.height), CV_32FC1);
+ history_.setTo(0.0);
}
-*/
+
+bool DisparitySmoothingOF::apply(Frame &in, Frame &out, Source *src, cudaStream_t stream) {
+ if (!out.hasChannel(channel_) || !in.hasChannel(Channel::Flow)) { return true; }
+
+ auto cvstream = cv::cuda::StreamAccessor::wrapStream(stream);
+ const cv::cuda::GpuMat &optflow = in.get<cv::cuda::GpuMat>(Channel::Flow);
+ cv::cuda::GpuMat &data = out.get<cv::cuda::GpuMat>(channel_);
+
+ if (data.size() != size_) {
+ size_ = data.size();
+ init();
+ }
+
+ ftl::cuda::optflow_filter(data, optflow, history_, n_max_, threshold_, cvstream);
+
return true;
}
diff --git a/components/operators/src/disparity/optflow_smoothing.cu b/components/operators/src/disparity/optflow_smoothing.cu
index b703a62aac386a535b83f23a1aface414a238465..a7633036fae1e9d20c6a52ae5617466f1b234202 100644
--- a/components/operators/src/disparity/optflow_smoothing.cu
+++ b/components/operators/src/disparity/optflow_smoothing.cu
@@ -3,7 +3,7 @@
#include <opencv2/core/cuda_stream_accessor.hpp>
#include "disparity/qsort.h"
-#include "optflow_smoothing.hpp"
+#include "disparity/cuda.hpp"
__device__ void quicksort(float A[], size_t n)
{
diff --git a/components/rgbd-sources/CMakeLists.txt b/components/rgbd-sources/CMakeLists.txt
index 70b5f7afb0c8f63a9518a80063afa6bf9f3a9123..1e0f0de201a91b01289e888510b655152e3af311 100644
--- a/components/rgbd-sources/CMakeLists.txt
+++ b/components/rgbd-sources/CMakeLists.txt
@@ -39,7 +39,6 @@ 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/cuda_algorithms.hpp b/components/rgbd-sources/src/cuda_algorithms.hpp
index f07c698f14d172aff96a7b59726b64cd9b98000d..43fff52d0c1d00aa2ff7960456db1fb827ad5cc6 100644
--- a/components/rgbd-sources/src/cuda_algorithms.hpp
+++ b/components/rgbd-sources/src/cuda_algorithms.hpp
@@ -38,9 +38,6 @@ 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/sources/middlebury/middlebury_source.cpp b/components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp
index d152d25e5a7d8830e8b555851f94d97b70463e29..229895049061f2308f1a982eab46005a26940548 100644
--- a/components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp
+++ b/components/rgbd-sources/src/sources/middlebury/middlebury_source.cpp
@@ -149,7 +149,7 @@ void MiddleburySource::_performDisparity() {
//calib_->rectifyStereo(left_, right_, stream_);
disp_->compute(rgb_, right_, disp_tmp_, stream_);
//disparityToDepth(disp_tmp_, depth_tmp_, params_, stream_);
- ftl::cuda::disparity_to_depth(disp_tmp_, depth_, params_, stream_);
+ //ftl::cuda::disparity_to_depth(disp_tmp_, depth_, params_, stream_);
//left_.download(rgb_, stream_);
//rgb_ = lsrc_->cachedLeft();
//depth_tmp_.download(depth_, stream_);
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
index a018859c87e1a1141b102135b38077e18e2c739f..473e66a49e59a7b5d810bb797169dac06c18a03a 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -1,19 +1,19 @@
#include <loguru.hpp>
+
#include "stereovideo.hpp"
-#include <ftl/configuration.hpp>
+#include "ftl/configuration.hpp"
#ifdef HAVE_OPTFLOW
-#include <ftl/operators/opticalflow.hpp>
+#include "ftl/operators/opticalflow.hpp"
#endif
-#include <ftl/threads.hpp>
+#include "ftl/operators/disparity.hpp"
+
+#include "ftl/threads.hpp"
#include "calibrate.hpp"
#include "local.hpp"
#include "disparity.hpp"
-#include "cuda_algorithms.hpp"
-
-#include "cuda_algorithms.hpp"
using ftl::rgbd::detail::Calibrate;
using ftl::rgbd::detail::LocalSource;
@@ -38,8 +38,7 @@ StereoVideoSource::~StereoVideoSource() {
delete lsrc_;
}
-void StereoVideoSource::init(const string &file)
-{
+void StereoVideoSource::init(const string &file) {
capabilities_ = kCapVideo | kCapStereo;
if (ftl::is_video(file)) {
@@ -127,6 +126,7 @@ void StereoVideoSource::init(const string &file)
#endif
pipeline_depth_ = ftl::config::create<ftl::operators::Graph>(host_, "pipeline_disparity");
+ pipeline_depth_->append<ftl::operators::DisparityToDepth>("calculate_depth");
disp_ = Disparity::create(host_, "disparity");
if (!disp_) LOG(FATAL) << "Unknown disparity algorithm : " << *host_->get<ftl::config::json_t>("disparity");
@@ -158,15 +158,6 @@ ftl::rgbd::Camera StereoVideoSource::parameters(Channel chan) {
}
}
-static void disparityToDepth(const cv::cuda::GpuMat &disparity, cv::cuda::GpuMat &depth,
- const cv::Mat &Q, cv::cuda::Stream &stream) {
- // Q(3, 2) = -1/Tx
- // Q(2, 3) = f
-
- double val = (1.0f / Q.at<double>(3, 2)) * Q.at<double>(2, 3);
- cv::cuda::divide(val, disparity, depth, 1.0f / 1000.0f, -1, stream);
-}
-
bool StereoVideoSource::capture(int64_t ts) {
timestamp_ = ts;
lsrc_->grab();
@@ -180,7 +171,7 @@ bool StereoVideoSource::retrieve() {
auto &right = frame.create<cv::cuda::GpuMat>(Channel::Right);
lsrc_->get(left, right, calib_, stream2_);
- pipeline_input_->apply(frame, frame, (ftl::rgbd::Source*) lsrc_, cv::cuda::StreamAccessor::getStream(stream2_));
+ pipeline_input_->apply(frame, frame, (ftl::rgbd::Source*) this, cv::cuda::StreamAccessor::getStream(stream2_));
stream2_.waitForCompletion();
return true;
@@ -209,8 +200,8 @@ bool StereoVideoSource::compute(int n, int b) {
auto &disp = frame.get<cv::cuda::GpuMat>(Channel::Disparity);
auto &depth = frame.create<cv::cuda::GpuMat>(Channel::Depth);
if (depth.empty()) depth = cv::cuda::GpuMat(left.size(), CV_32FC1);
-
- ftl::cuda::disparity_to_depth(disp, depth, params_, stream_);
+ pipeline_depth_->apply(frame, frame, (ftl::rgbd::Source*) this, cv::cuda::StreamAccessor::getStream(stream_));
+ //ftl::cuda::disparity_to_depth(disp, depth, params_, stream_);
//left.download(rgb_, stream_);
//depth.download(depth_, stream_);