diff --git a/applications/reconstruct/src/ilw.cpp b/applications/reconstruct/src/ilw.cpp index 12d1b32f1febb11b6e7a12d365e13627820a977c..e74c535fb9a1ac4eeb6964877429b6e3ddbad83b 100644 --- a/applications/reconstruct/src/ilw.cpp +++ b/applications/reconstruct/src/ilw.cpp @@ -1,7 +1,14 @@ #include "ilw.hpp" +#include <ftl/utility/matrix_conversion.hpp> +#include <ftl/rgbd/source.hpp> +#include <ftl/cuda/points.hpp> using ftl::ILW; using ftl::detail::ILWData; +using ftl::rgbd::Channel; +using ftl::rgbd::Channels; +using ftl::rgbd::Format; +using cv::cuda::GpuMat; ILW::ILW(nlohmann::json &config) : ftl::Configurable(config) { @@ -11,10 +18,8 @@ ILW::~ILW() { } -bool ILW::process(ftl::rgbd::FrameSet &fs) { - return true; - - _phase0(fs); +bool ILW::process(ftl::rgbd::FrameSet &fs, cudaStream_t stream) { + _phase0(fs, stream); for (int i=0; i<2; ++i) { _phase1(fs); @@ -28,8 +33,21 @@ bool ILW::process(ftl::rgbd::FrameSet &fs) { return true; } -bool ILW::_phase0(ftl::rgbd::FrameSet &fs) { - // Clear points channel... +bool ILW::_phase0(ftl::rgbd::FrameSet &fs, cudaStream_t stream) { + // Make points channel... + for (size_t i=0; i<fs.frames.size(); ++i) { + auto &f = fs.frames[i]; + auto *s = fs.sources[i]; + + if (f.empty(Channel::Depth + Channel::Colour)) { + LOG(ERROR) << "Missing required channel"; + continue; + } + + auto &t = f.createTexture<float4>(Channel::Points, Format<float4>(f.get<GpuMat>(Channel::Colour).size())); + auto pose = MatrixConversion::toCUDA(s->getPose().cast<float>()); //.inverse()); + ftl::cuda::point_cloud(t, f.createTexture<float>(Channel::Depth), s->parameters(), pose, stream); + } // Upload camera data? return true; diff --git a/applications/reconstruct/src/ilw.hpp b/applications/reconstruct/src/ilw.hpp index 1d269b83a9fc8efdd8d7a49e8e62b7b564059f66..5bab3387316c47c3f1489acde7990d583364f523 100644 --- a/applications/reconstruct/src/ilw.hpp +++ b/applications/reconstruct/src/ilw.hpp @@ -40,13 +40,13 @@ class ILW : public ftl::Configurable { /** * Take a frameset and perform the iterative lattice warping. */ - bool process(ftl::rgbd::FrameSet &fs); + bool process(ftl::rgbd::FrameSet &fs, cudaStream_t stream=0); private: /* * Initialise data. */ - bool _phase0(ftl::rgbd::FrameSet &fs); + bool _phase0(ftl::rgbd::FrameSet &fs, cudaStream_t stream); /* * Find possible correspondences and a confidence value. diff --git a/applications/reconstruct/src/main.cpp b/applications/reconstruct/src/main.cpp index 328a9427aee97b5f428d8d4a4a9841062946ebc5..2b5da6d01022227be4abbf9b7fab9ac39fc4c0f4 100644 --- a/applications/reconstruct/src/main.cpp +++ b/applications/reconstruct/src/main.cpp @@ -121,6 +121,7 @@ static void run(ftl::Configurable *root) { bool busy = false; + group.setLatency(4); group.setName("ReconGroup"); group.sync([splat,virt,&busy,&slave,&scene_A,&scene_B,&align](ftl::rgbd::FrameSet &fs) -> bool { //cudaSetDevice(scene->getCUDADevice()); diff --git a/components/rgbd-sources/src/frame.cpp b/components/rgbd-sources/src/frame.cpp index aaa55c756bf8cb1b032d4c0d57e84ad5efe1dee1..a56a19355526d9cdcdf25faaecdc67c05d09469d 100644 --- a/components/rgbd-sources/src/frame.cpp +++ b/components/rgbd-sources/src/frame.cpp @@ -66,6 +66,10 @@ void Frame::swapTo(ftl::rgbd::Channels channels, Frame &f) { cv::swap(m1.host, m2.host); cv::cuda::swap(m1.gpu, m2.gpu); + + auto temptex = std::move(m2.tex); + m2.tex = std::move(m1.tex); + m1.tex = std::move(temptex); } } } diff --git a/components/rgbd-sources/src/frameset.cpp b/components/rgbd-sources/src/frameset.cpp index 72cb5cb50af72dc035d3b113269ed4af38c2d5b9..9b9a807d8599c23141b6c3806546bf8038ef30f9 100644 --- a/components/rgbd-sources/src/frameset.cpp +++ b/components/rgbd-sources/src/frameset.cpp @@ -19,12 +19,12 @@ void FrameSet::download(ftl::rgbd::Channels c, cudaStream_t stream) { void FrameSet::swapTo(ftl::rgbd::FrameSet &fs) { UNIQUE_LOCK(fs.mtx, lk); - if (fs.frames.size() != frames.size()) { + //if (fs.frames.size() != frames.size()) { // Assume "this" is correct and "fs" is not. fs.sources.clear(); for (auto s : sources) fs.sources.push_back(s); fs.frames.resize(frames.size()); - } + //} fs.timestamp = timestamp; fs.count = static_cast<int>(count);