diff --git a/components/operators/CMakeLists.txt b/components/operators/CMakeLists.txt index e1b7569960118e923c18b0d759cbb3f1ebcad640..9d6793817d5d20c5312e4c94a01fca1fe4a1d374 100644 --- a/components/operators/CMakeLists.txt +++ b/components/operators/CMakeLists.txt @@ -27,6 +27,7 @@ set(OPERSRC src/fusion/correspondence_util.cu src/fusion/mls_aggr.cu src/fusion/smoothing/mls_multi_weighted.cu + src/fusion/fusion.cpp src/misc/clipping.cpp src/disparity/depth.cpp src/analysis/tracking/detectandtrack.cpp diff --git a/components/operators/include/ftl/operators/fusion.hpp b/components/operators/include/ftl/operators/fusion.hpp new file mode 100644 index 0000000000000000000000000000000000000000..ed0e5f9cd324f1e57979d9f20fc9d9a98f796ce0 --- /dev/null +++ b/components/operators/include/ftl/operators/fusion.hpp @@ -0,0 +1,29 @@ +#ifndef _FTL_OPERATORS_FUSION_HPP_ +#define _FTL_OPERATORS_FUSION_HPP_ + +#include <ftl/operators/operator.hpp> +#include <ftl/operators/cuda/mls/multi_intensity.hpp> +#include <vector> + +namespace ftl { +namespace operators { + +class Fusion : public ftl::operators::Operator { + public: + Fusion(ftl::operators::Graph *g, ftl::Configurable*); + ~Fusion(); + + inline Operator::Type type() const override { return Operator::Type::ManyToMany; } + + bool apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) override; + + private: + ftl::cuda::MLSMultiIntensity mls_; + std::vector<cv::cuda::GpuMat> weights_; + cv::cuda::GpuMat temp_; +}; + +} +} + +#endif diff --git a/components/operators/src/fusion/fusion.cpp b/components/operators/src/fusion/fusion.cpp new file mode 100644 index 0000000000000000000000000000000000000000..489edb4c2e704bd1f7a38ae86d036dbd4358fbd2 --- /dev/null +++ b/components/operators/src/fusion/fusion.cpp @@ -0,0 +1,92 @@ +#include <ftl/operators/fusion.hpp> +#include <ftl/utility/matrix_conversion.hpp> +#include <opencv2/core/cuda_stream_accessor.hpp> + +#include <opencv2/cudaimgproc.hpp> +#include <opencv2/cudawarping.hpp> + +using ftl::operators::Fusion; +using ftl::codecs::Channel; +using cv::cuda::GpuMat; + +Fusion::Fusion(ftl::operators::Graph *g, ftl::Configurable *cfg) : ftl::operators::Operator(g, cfg), mls_(3) { + +} + +Fusion::~Fusion() { + +} + +bool Fusion::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStream_t stream) { + float mls_smoothing = config()->value("mls_smoothing", 0.01f); + int mls_iters = config()->value("mls_iterations", 2); + + if (weights_.size() != in.frames.size()) weights_.resize(in.frames.size()); + + cv::cuda::Stream cvstream = cv::cuda::StreamAccessor::wrapStream(stream); + + for (size_t i=0; i<in.frames.size(); ++i) { + if (!in.hasFrame(i)) continue; + const GpuMat &col = in.frames[i].get<GpuMat>(Channel::Colour); + const GpuMat &d = in.frames[i].get<GpuMat>(Channel::Depth); + + cv::cuda::cvtColor(col, temp_, cv::COLOR_BGRA2GRAY, 0, cvstream); + cv::cuda::resize(temp_, weights_[i], d.size(), 0, 0, cv::INTER_LINEAR, cvstream); + } + + for (int iters=0; iters < mls_iters; ++iters) { + for (size_t i=0; i<in.frames.size(); ++i) { + if (!in.hasFrame(i)) continue; + + auto &f1 = in.frames[i].cast<ftl::rgbd::Frame>(); + + Eigen::Vector4d d1(0.0, 0.0, 1.0, 0.0); + d1 = f1.getPose() * d1; + + auto pose1 = MatrixConversion::toCUDA(f1.getPose().cast<float>()); + + mls_.prime( + f1.get<GpuMat>(Channel::Depth), + weights_[i], + f1.getLeft(), + pose1, + stream + ); + + for (size_t j=0; j<in.frames.size(); ++j) { + if (!in.hasFrame(j)) continue; + //if (i == j) continue; + + //LOG(INFO) << "Running phase1"; + + auto &f2 = in.frames[j].cast<ftl::rgbd::Frame>(); + + // Are cameras facing similar enough direction? + Eigen::Vector4d d2(0.0, 0.0, 1.0, 0.0); + d2 = f2.getPose() * d2; + // No, so skip this combination + if (d1.dot(d2) <= 0.0) continue; + + auto pose2 = MatrixConversion::toCUDA(f2.getPose().cast<float>()); + + mls_.gather( + f2.get<GpuMat>(Channel::Depth), + f2.get<GpuMat>(Channel::Normals), + weights_[j], + f2.getLeft(), + pose2, + mls_smoothing, + stream + ); + } + + mls_.adjust( + f1.create<GpuMat>(Channel::Depth), + f1.create<GpuMat>(Channel::Normals), + stream + ); + } + } + + return true; +} diff --git a/components/streams/src/feed.cpp b/components/streams/src/feed.cpp index d5d5533b6a23064f5efafda322b6675e47433f4d..f9b468605bcc336e3bf6d89f137eae7fe252a087 100644 --- a/components/streams/src/feed.cpp +++ b/components/streams/src/feed.cpp @@ -15,6 +15,7 @@ #include <ftl/operators/detectandtrack.hpp> #include <ftl/operators/weighting.hpp> #include <ftl/operators/mvmls.hpp> +#include <ftl/operators/fusion.hpp> #include <ftl/operators/clipping.hpp> #include <ftl/operators/poser.hpp> #include <ftl/operators/gt_analysis.hpp> @@ -533,6 +534,7 @@ void Feed::_createPipeline(uint32_t fsid) { p->append<ftl::operators::BorderMask>("border_mask"); p->append<ftl::operators::CullDiscontinuity>("remove_discontinuity"); p->append<ftl::operators::MultiViewMLS>("mvmls")->value("enabled", false); + p->append<ftl::operators::Fusion>("fusion")->value("enabled", false); p->append<ftl::operators::DisplayMask>("display_mask")->value("enabled", false); p->append<ftl::operators::Poser>("poser")->value("enabled", true); p->append<ftl::operators::GTAnalysis>("gtanalyse");