diff --git a/applications/gui/CMakeLists.txt b/applications/gui/CMakeLists.txt index a30525ddc67d057d1a64ddb17c1e85901e5b6259..b2dbd004f25c03332a40efe988c664af0d7d225a 100644 --- a/applications/gui/CMakeLists.txt +++ b/applications/gui/CMakeLists.txt @@ -14,7 +14,6 @@ set(GUISRC src/media_panel.cpp src/thumbview.cpp src/record_window.cpp - src/overlay.cpp ) if (HAVE_OPENVR) diff --git a/applications/gui/src/camera.cpp b/applications/gui/src/camera.cpp index 53a0d0593a457921cc82dfe2759484a1967f18dc..ab94a7848ae05b7c1715530a9909bbf158975110 100644 --- a/applications/gui/src/camera.cpp +++ b/applications/gui/src/camera.cpp @@ -14,7 +14,7 @@ #include <ftl/codecs/faces.hpp> -#include "overlay.hpp" +#include <ftl/render/overlay.hpp> #include "statsimage.hpp" #define LOGURU_REPLACE_GLOG 1 @@ -83,6 +83,7 @@ ftl::gui::Camera::Camera(ftl::gui::Screen *screen, int fsmask, int fid, ftl::cod framesets_ = nullptr; colouriser_ = ftl::create<ftl::render::Colouriser>(screen->root(), "colouriser"); + overlayer_ = ftl::create<ftl::overlay::Overlay>(screen->root(), "overlay"); // Is virtual camera? if (fid == 255) { @@ -172,7 +173,7 @@ void ftl::gui::Camera::draw(std::vector<ftl::rgbd::FrameSet*> &fss) { auto &d = *data.rbegin(); cv::Mat over_depth; - over_depth.create(im1_.size(), CV_32F); + over_depth.create(overlay_.size(), CV_32F); auto cam = ftl::rgbd::Camera::from(intrinsics_); @@ -224,7 +225,7 @@ void ftl::gui::Camera::draw(std::vector<ftl::rgbd::FrameSet*> &fss) { squre_pts.push_back(cv::Point2f(cam.width,cam.height)); cv::Mat transmtx = cv::getPerspectiveTransform(quad_pts,squre_pts); - cv::Mat transformed = cv::Mat::zeros(im1_.rows, im1_.cols, CV_8UC4); + cv::Mat transformed = cv::Mat::zeros(overlay_.rows, overlay_.cols, CV_8UC4); //cv::warpPerspective(im1_, im1_, transmtx, im1_.size()); ftl::render::ViewPort vp; @@ -300,6 +301,12 @@ void ftl::gui::Camera::_draw(std::vector<ftl::rgbd::FrameSet*> &fss) { frame_.create<cv::cuda::GpuMat>(Channel::Colour) = texture1_.map(renderer_->getCUDAStream()); if (isStereo()) frame_.create<cv::cuda::GpuMat>(Channel::Colour2) = texture2_.map((renderer2_) ? renderer2_->getCUDAStream() : 0); + overlay_.create(state_.getLeft().height, state_.getLeft().width, CV_8UC4); + frame_.create<cv::Mat>(Channel::Overlay) = overlay_; + + overlay_.setTo(cv::Scalar(0,0,0,0)); + bool enable_overlay = overlayer_->value("enabled", false); + { FTL_Profile("Render",0.034); renderer_->begin(frame_, Channel::Colour); @@ -317,6 +324,12 @@ void ftl::gui::Camera::_draw(std::vector<ftl::rgbd::FrameSet*> &fss) { fs->mtx.lock(); renderer_->submit(fs, ftl::codecs::Channels<0>(Channel::Colour), transforms_[fs->id]); if (isStereo()) renderer2_->submit(fs, ftl::codecs::Channels<0>(Channel::Colour), transforms_[fs->id]); + + if (enable_overlay) { + // Generate and upload an overlay image. + overlayer_->apply(*fs, overlay_, state_); + frame_.upload(Channel::Overlay, renderer_->getCUDAStream()); + } } if (channel_ != Channel::Left && channel_ != Channel::Right && channel_ != Channel::None) { @@ -326,6 +339,10 @@ void ftl::gui::Camera::_draw(std::vector<ftl::rgbd::FrameSet*> &fss) { } } + if (enable_overlay) { + renderer_->blend(overlayer_->value("alpha", 0.8f), Channel::Overlay); + } + renderer_->end(); if (isStereo()) renderer2_->end(); } catch(std::exception &e) { @@ -354,31 +371,6 @@ void ftl::gui::Camera::_draw(std::vector<ftl::rgbd::FrameSet*> &fss) { width_ = texture1_.width(); height_ = texture1_.height(); - if (isStereo()) { - //_downloadFrames(frame_.getTexture<uchar4>(Channel::Colour), frame_.getTexture<uchar4>(Channel::Colour2)); - } else { - //_downloadFrame(frame_.getTexture<uchar4>(Channel::Colour)); - } - - if (screen_->root()->value("show_poses", false)) { - cv::Mat over_col, over_depth; - over_col.create(im1_.size(), CV_8UC4); - over_depth.create(im1_.size(), CV_32F); - - for (auto *fs : fss) { - if (!usesFrameset(fs->id)) continue; - for (size_t i=0; i<fs->frames.size(); ++i) { - auto pose = fs->frames[i].getPose().inverse() * state_.getPose(); - Eigen::Vector4d pos = pose.inverse() * Eigen::Vector4d(0,0,0,1); - pos /= pos[3]; - - auto name = fs->frames[i].get<std::string>("name"); - ftl::overlay::drawCamera(state_.getLeft(), im1_, over_depth, fs->frames[i].getLeftCamera(), pose, cv::Scalar(0,0,255,255), 0.2,screen_->root()->value("show_frustrum", false)); - if (name) ftl::overlay::drawText(state_.getLeft(), im1_, over_depth, *name, pos, 0.5, cv::Scalar(0,0,255,255)); - } - } - } - if (record_stream_ && record_stream_->active()) { // TODO: Allow custom channel selection ftl::rgbd::FrameSet fs2; @@ -579,16 +571,6 @@ void ftl::gui::Camera::setChannel(Channel c) { cv::addWeighted(edges, weight, out, 1.0, 0.0, out, CV_8UC4); }*/ -bool ftl::gui::Camera::thumbnail(cv::Mat &thumb) { - { - UNIQUE_LOCK(mutex_, lk); - if (im1_.empty()) return false; - // FIXME: Use correct aspect ratio? - cv::resize(im1_, thumb, cv::Size(320,180)); - } - cv::flip(thumb, thumb, 0); - return true; -} void ftl::gui::Camera::active(bool a) { if (a) { @@ -704,16 +686,6 @@ const void ftl::gui::Camera::captureFrame() { } if (framesets_) draw(*framesets_); - - { - UNIQUE_LOCK(mutex_, lk); - if (im1_.rows != 0) { - //texture1_.update(im1_); - } - if (isStereo() && im2_.rows != 0) { - //texture2_.update(im2_); - } - } } //return texture1_; @@ -724,7 +696,7 @@ void ftl::gui::Camera::snapshot(const std::string &filename) { { UNIQUE_LOCK(mutex_, lk); - cv::flip(im1_, flipped, 0); + //cv::flip(im1_, flipped, 0); } cv::cvtColor(flipped, flipped, cv::COLOR_BGRA2BGR); cv::imwrite(filename, flipped); diff --git a/applications/gui/src/camera.hpp b/applications/gui/src/camera.hpp index 8a62198d966686631a9fe299efc531bd50572b90..c64bdfcaa12596460b63c00d303f7df1cab8ada9 100644 --- a/applications/gui/src/camera.hpp +++ b/applications/gui/src/camera.hpp @@ -3,6 +3,7 @@ #include <ftl/rgbd/frameset.hpp> #include <ftl/render/CUDARender.hpp> +#include <ftl/render/overlay.hpp> #include <ftl/codecs/writer.hpp> #include "gltexture.hpp" @@ -86,8 +87,6 @@ class Camera { const GLTexture &getLeft() const { return texture1_; } const GLTexture &getRight() const { return texture2_; } - bool thumbnail(cv::Mat &thumb); - void snapshot(const std::string &filename); void startVideoRecording(const std::string &filename); @@ -134,8 +133,8 @@ class Camera { bool pause_; ftl::codecs::Channel channel_; ftl::codecs::Channels<0> channels_; - cv::Mat im1_; // first channel (left) - cv::Mat im2_; // second channel ("right") + cv::Mat overlay_; // first channel (left) + //cv::Mat im2_; // second channel ("right") bool stereo_; std::atomic_flag stale_frame_; int rx_; @@ -145,6 +144,7 @@ class Camera { ftl::render::CUDARender *renderer_; ftl::render::CUDARender *renderer2_; ftl::render::Colouriser *colouriser_; + ftl::overlay::Overlay *overlayer_; ftl::Configurable *intrinsics_; ftl::operators::Graph *post_pipe_; diff --git a/applications/gui/src/src_window.cpp b/applications/gui/src/src_window.cpp index 07fcb3ce3b2191151eda2518ac10b1a02c7f62b4..81dfcca0f91e5fd7e40a6dcda981b2ed7dc23cf1 100644 --- a/applications/gui/src/src_window.cpp +++ b/applications/gui/src/src_window.cpp @@ -31,6 +31,7 @@ #include <ftl/operators/detectandtrack.hpp> #include <ftl/operators/weighting.hpp> #include <ftl/operators/mvmls.hpp> +#include <ftl/operators/clipping.hpp> #include <nlohmann/json.hpp> @@ -230,6 +231,7 @@ void SourceWindow::_checkFrameSets(int id) { while (framesets_.size() <= id) { auto *p = ftl::config::create<ftl::operators::Graph>(screen_->root(), "pre_filters"); p->append<ftl::operators::DepthChannel>("depth")->value("enabled", false); + p->append<ftl::operators::ClipScene>("clipping")->value("enabled", false); //p->append<ftl::operators::ColourChannels>("colour"); // Convert BGR to BGRA p->append<ftl::operators::DetectAndTrack>("facedetection")->value("enabled", false); p->append<ftl::operators::ArUco>("aruco")->value("enabled", false); diff --git a/components/codecs/include/ftl/codecs/channels.hpp b/components/codecs/include/ftl/codecs/channels.hpp index 9b0aaa1f5374130f61fbe68a6f91622c66463455..f2f8a833bfb2f5515a407dbcaf135929ebce4e63 100644 --- a/components/codecs/include/ftl/codecs/channels.hpp +++ b/components/codecs/include/ftl/codecs/channels.hpp @@ -36,6 +36,7 @@ enum struct Channel : int { Smoothing = 19, // 32F RightHighRes = 20, // 8UC3 or 8UC4 Colour2HighRes = 20, + Overlay = 21, // 8UC4 Audio = 32, AudioMono = 32, @@ -53,7 +54,8 @@ enum struct Channel : int { Data = 2048, // Custom data, any codec. Faces = 2049, // Data about detected faces - Transforms = 2050 // Transformation matrices for framesets + Transforms = 2050, // Transformation matrices for framesets + Shapes3D = 2051 // Labeled 3D shapes }; inline bool isVideo(Channel c) { return (int)c < 32; }; diff --git a/components/codecs/include/ftl/codecs/shapes.hpp b/components/codecs/include/ftl/codecs/shapes.hpp new file mode 100644 index 0000000000000000000000000000000000000000..ff5b189b31fc3c9d3f94d524a97e40a12606b37e --- /dev/null +++ b/components/codecs/include/ftl/codecs/shapes.hpp @@ -0,0 +1,39 @@ +#ifndef _FTL_CODECS_SHAPES_HPP_ +#define _FTL_CODECS_SHAPES_HPP_ + +#include <opencv2/core/mat.hpp> + +#include <ftl/utility/msgpack.hpp> + +namespace ftl { +namespace codecs { + +enum class Shape3DType { + UNKNOWN = 0, + BOX, + SPHERE, + STAR, + HEAD, + CLIPPING, + CAMERA, + FEATURE +}; + +struct Shape3D { + Shape3D() {}; + + int id; + Shape3DType type; + Eigen::Vector3f size; + Eigen::Matrix4f pose; + std::string label; + + MSGPACK_DEFINE_ARRAY(id, type, size, pose, label); +}; + +} +} + +MSGPACK_ADD_ENUM(ftl::codecs::Shape3DType); + +#endif diff --git a/components/common/cpp/CMakeLists.txt b/components/common/cpp/CMakeLists.txt index 57ec51ace3ab13823105f2c5936e419e020809a6..821c2eee75fdd21e6fa4ff7a7911686c01a50787 100644 --- a/components/common/cpp/CMakeLists.txt +++ b/components/common/cpp/CMakeLists.txt @@ -23,7 +23,7 @@ target_include_directories(ftlcommon PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> $<INSTALL_INTERFACE:include> PRIVATE src) -target_link_libraries(ftlcommon Threads::Threads ${OS_LIBS} ${OpenCV_LIBS} ${URIPARSER_LIBRARIES} ${CUDA_LIBRARIES}) +target_link_libraries(ftlcommon Threads::Threads Eigen3::Eigen ${OS_LIBS} ${OpenCV_LIBS} ${URIPARSER_LIBRARIES} ${CUDA_LIBRARIES}) if (BUILD_TESTS) add_subdirectory(test) diff --git a/components/common/cpp/include/ftl/utility/msgpack.hpp b/components/common/cpp/include/ftl/utility/msgpack.hpp index b9ed96637a78697b219cd9b0506c01e6b76da934..30319d2256c0a32d324346637727e216a4620d1b 100644 --- a/components/common/cpp/include/ftl/utility/msgpack.hpp +++ b/components/common/cpp/include/ftl/utility/msgpack.hpp @@ -7,6 +7,7 @@ #include <msgpack.hpp> #include <opencv2/core/mat.hpp> +#include <Eigen/Eigen> namespace msgpack { MSGPACK_API_VERSION_NAMESPACE(MSGPACK_DEFAULT_API_NS) { @@ -210,6 +211,48 @@ struct object_with_zone<cv::Mat> { } }; +//////////////////////////////////////////////////////////////////////////////// +// Eigen::Matrix<> + +template <typename T, int X, int Y> +struct pack<Eigen::Matrix<T, X, Y>> { + template <typename Stream> + packer<Stream>& operator()(msgpack::packer<Stream>& o, Eigen::Matrix<T, X, Y> const& v) const { + + o.pack_array(X*Y); + for (int i = 0; i < X*Y; i++) { o.pack(v.data()[i]); } + + return o; + } +}; + +template<typename T, int X, int Y> +struct convert<Eigen::Matrix<T, X, Y>> { + msgpack::object const& operator()(msgpack::object const& o, Eigen::Matrix<T, X, Y> &v) const { + if (o.type != msgpack::type::ARRAY) { throw msgpack::type_error(); } + if (o.via.array.size != X*Y) { throw msgpack::type_error(); } + + for (int i = 0; i < X*Y; i++) { v.data()[i] = o.via.array.ptr[i].as<T>(); } + + return o; + } +}; + +template <typename T, int X, int Y> +struct object_with_zone<Eigen::Matrix<T, X, Y>> { + void operator()(msgpack::object::with_zone& o, Eigen::Matrix<T, X, Y> const& v) const { + o.type = type::ARRAY; + o.via.array.size = X*Y; + o.via.array.ptr = static_cast<msgpack::object*>( + o.zone.allocate_align( sizeof(msgpack::object) * o.via.array.size, + MSGPACK_ZONE_ALIGNOF(msgpack::object))); + + for (int i = 0; i < X*Y; i++) { + o.via.array.ptr[i] = msgpack::object(v.data()[i], o.zone); + } + } +}; + } } } diff --git a/components/common/cpp/test/CMakeLists.txt b/components/common/cpp/test/CMakeLists.txt index de71019fb70424cbdf6243c1dcf7e9f5071db504..9112234bfadbd4de3be225dce71937a38ed1f62a 100644 --- a/components/common/cpp/test/CMakeLists.txt +++ b/components/common/cpp/test/CMakeLists.txt @@ -34,7 +34,7 @@ add_executable(msgpack_unit $<TARGET_OBJECTS:Loguru> ./msgpack_unit.cpp) target_include_directories(msgpack_unit PUBLIC ${OpenCV_INCLUDE_DIRS} "${CMAKE_CURRENT_SOURCE_DIR}/../include") -target_link_libraries(msgpack_unit Threads::Threads ${OS_LIBS} ${OpenCV_LIBS}) +target_link_libraries(msgpack_unit Threads::Threads Eigen3::Eigen ${OS_LIBS} ${OpenCV_LIBS}) add_test(ConfigurableUnitTest configurable_unit) add_test(URIUnitTest uri_unit) diff --git a/components/common/cpp/test/msgpack_unit.cpp b/components/common/cpp/test/msgpack_unit.cpp index a321cd36bdbebe16959fdf558b3d3dffd16dc06a..c5e79d3bf91e34f2ac672ffcb53919b9a884a196 100644 --- a/components/common/cpp/test/msgpack_unit.cpp +++ b/components/common/cpp/test/msgpack_unit.cpp @@ -27,6 +27,26 @@ T msgpack_unpack(std::string str) { return obj.convert<T>(res); } +TEST_CASE( "msgpack Eigen::Matrix") { + SECTION("Matrix4f") { + Eigen::Matrix4f a; + a.setIdentity(); + + Eigen::Matrix4f b = msgpack_unpack<Eigen::Matrix4f>(msgpack_pack(a)); + + REQUIRE( (a == b) ); + } + + SECTION("Vector3f") { + Eigen::Vector3f a; + a.setIdentity(); + + Eigen::Vector3f b = msgpack_unpack<Eigen::Vector3f>(msgpack_pack(a)); + + REQUIRE( (a == b) ); + } +} + TEST_CASE( "msgpack cv::Mat" ) { SECTION( "Mat::ones(Size(5, 5), CV_64FC1)" ) { Mat A = Mat::ones(Size(5, 5), CV_64FC1); diff --git a/components/operators/src/clipping.cpp b/components/operators/src/clipping.cpp index 2d13f65f662973f0dc6c6a47508c97cc8e143a47..e6380fffcfa737ab430f18ee7ee681afa9c7fc07 100644 --- a/components/operators/src/clipping.cpp +++ b/components/operators/src/clipping.cpp @@ -1,6 +1,7 @@ #include <ftl/operators/clipping.hpp> #include <ftl/cuda/points.hpp> #include <ftl/utility/matrix_conversion.hpp> +#include <ftl/codecs/shapes.hpp> using ftl::operators::ClipScene; using ftl::codecs::Channel; @@ -42,8 +43,22 @@ bool ClipScene::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStr Eigen::Affine3f t(trans); ftl::cuda::ClipSpace clip; - clip.origin = MatrixConversion::toCUDA(r.matrix() * t.matrix()); + clip.origin = MatrixConversion::toCUDA(t.matrix() * r.matrix()); clip.size = make_float3(width, height, depth); + + ftl::codecs::Shape3D shape; + shape.id = 0; + shape.label = "Clipping"; + shape.pose = t.matrix() * r.matrix(); + shape.size = Eigen::Vector3f(width, height, depth); + shape.type = ftl::codecs::Shape3DType::CLIPPING; + + bool no_clip = config()->value("no_clip", false); + + std::vector<ftl::codecs::Shape3D> shapes; + if (in.hasChannel(Channel::Shapes3D)) in.get(Channel::Shapes3D, shapes); + shapes.push_back(shape); + in.create(Channel::Shapes3D, shapes); for (size_t i=0; i<in.frames.size(); ++i) { auto &f = in.frames[i]; @@ -52,8 +67,8 @@ bool ClipScene::apply(ftl::rgbd::FrameSet &in, ftl::rgbd::FrameSet &out, cudaStr auto pose = MatrixConversion::toCUDA(f.getPose().cast<float>()); auto sclip = clip; - sclip.origin = sclip.origin * pose; - ftl::cuda::clipping(f.createTexture<float>(Channel::Depth), f.getLeftCamera(), sclip, stream); + sclip.origin = sclip.origin.getInverse() * pose; + if (!no_clip) ftl::cuda::clipping(f.createTexture<float>(Channel::Depth), f.getLeftCamera(), sclip, stream); } return true; diff --git a/components/renderers/cpp/CMakeLists.txt b/components/renderers/cpp/CMakeLists.txt index 1541460635fab69c057b305a6df10eea9ae29114..516369fddf779c5949ca9ae1b2c79ce9fced866e 100644 --- a/components/renderers/cpp/CMakeLists.txt +++ b/components/renderers/cpp/CMakeLists.txt @@ -9,6 +9,7 @@ add_library(ftlrender src/CUDARender.cpp src/colouriser.cpp src/colour_util.cu + src/overlay.cpp ) target_include_directories(ftlrender PUBLIC diff --git a/applications/gui/src/overlay.hpp b/components/renderers/cpp/include/ftl/render/overlay.hpp similarity index 76% rename from applications/gui/src/overlay.hpp rename to components/renderers/cpp/include/ftl/render/overlay.hpp index 9aff7628ebe9ae9c49b7a6aa8328bb7f25e214a5..7843a60a3580027f3ccda802a5895602a4166838 100644 --- a/applications/gui/src/overlay.hpp +++ b/components/renderers/cpp/include/ftl/render/overlay.hpp @@ -3,11 +3,22 @@ #include <opencv2/core/mat.hpp> #include <Eigen/Eigen> -#include <ftl/rgbd/frame.hpp> +#include <ftl/rgbd/frameset.hpp> namespace ftl { namespace overlay { +class Overlay : public ftl::Configurable { + public: + explicit Overlay(nlohmann::json &config); + ~Overlay(); + + void apply(ftl::rgbd::FrameSet &fs, cv::Mat &out, ftl::rgbd::FrameState &state); + + private: + cv::Mat over_depth_; +}; + void draw3DLine( const ftl::rgbd::Camera &cam, cv::Mat &colour, @@ -36,6 +47,14 @@ void drawPoseBox( const cv::Scalar &linecolour, double size); +void drawBox( + const ftl::rgbd::Camera &cam, + cv::Mat &colour, + cv::Mat &depth, + const Eigen::Matrix4d &pose, + const cv::Scalar &linecolour, + const Eigen::Vector3d &size); + void drawRectangle( const ftl::rgbd::Camera &cam, cv::Mat &colour, diff --git a/components/renderers/cpp/src/CUDARender.cpp b/components/renderers/cpp/src/CUDARender.cpp index 9dec33bcbf46369f18ce5989f02bd5bd4bcc7ee5..557392d1ced004f232f7bcf5fef3dd6262468766 100644 --- a/components/renderers/cpp/src/CUDARender.cpp +++ b/components/renderers/cpp/src/CUDARender.cpp @@ -504,7 +504,11 @@ void CUDARender::blend(float alpha, Channel c) { //cv::cuda::addWeighted(buf.to_gpumat(), alpha, out_->get<GpuMat>(out_chan_), 1.0f-alpha, 0.0f, // out_->get<GpuMat>(out_chan_), -1, cvstream); - ftl::cuda::blend_alpha(buf, out_->getTexture<uchar4>(out_chan_), alpha, 1.0f-alpha, stream_); + if (alpha < 0.0f) { + ftl::cuda::composite(buf, out_->getTexture<uchar4>(out_chan_), stream_); + } else { + ftl::cuda::blend_alpha(buf, out_->getTexture<uchar4>(out_chan_), alpha, 1.0f-alpha, stream_); + } } void CUDARender::end() { diff --git a/components/renderers/cpp/src/clipping.cu b/components/renderers/cpp/src/clipping.cu index 354376365c5334e8d4bf0671f76f7aec07c441bb..0fae98e11617edb65f916fb08b372c4e0aefcb6d 100644 --- a/components/renderers/cpp/src/clipping.cu +++ b/components/renderers/cpp/src/clipping.cu @@ -4,7 +4,7 @@ __device__ bool isClipped(const float4 &p, const ftl::cuda::ClipSpace &clip) { const float3 tp = clip.origin * make_float3(p); - return fabs(tp.x) > clip.size.x || fabs(tp.y) > clip.size.y || fabs(tp.z) > clip.size.z; + return fabs(tp.x) > clip.size.x/2.0f || fabs(tp.y) > clip.size.y/2.0f || fabs(tp.z) > clip.size.z/2.0f; } __global__ void clipping_kernel(ftl::cuda::TextureObject<float> depth, ftl::rgbd::Camera camera, ftl::cuda::ClipSpace clip) @@ -17,7 +17,7 @@ __global__ void clipping_kernel(ftl::cuda::TextureObject<float> depth, ftl::rgbd float4 p = make_float4(camera.screenToCam(x,y,d), 0.0f); if (isClipped(p, clip)) { - depth(x,y) = MINF; + depth(x,y) = 0.0f; } } } diff --git a/components/renderers/cpp/src/colour_cuda.hpp b/components/renderers/cpp/src/colour_cuda.hpp index fe499af68e956c4b8c9d2fd7bab2b87264cd2df0..bad912c96f99e1d36091076f23da436dde3f8c26 100644 --- a/components/renderers/cpp/src/colour_cuda.hpp +++ b/components/renderers/cpp/src/colour_cuda.hpp @@ -17,6 +17,11 @@ void blend_alpha( float alpha, float beta, cudaStream_t stream); +void composite( + ftl::cuda::TextureObject<uchar4> &in, + ftl::cuda::TextureObject<uchar4> &out, + cudaStream_t stream); + } } diff --git a/components/renderers/cpp/src/colour_util.cu b/components/renderers/cpp/src/colour_util.cu index ae3d72f8cde48af1a615addd8782076a09e6ec3d..01939ebd37609c690ab4ec8f4cf873a357be3192 100644 --- a/components/renderers/cpp/src/colour_util.cu +++ b/components/renderers/cpp/src/colour_util.cu @@ -63,6 +63,10 @@ template void ftl::cuda::lut<short>(TextureObject<short> &in, TextureObject<ucha // ==== Blending =============================================================== +__device__ inline float clamp(float a, float c) { + return (a > c) ? c : a; +} + __global__ void blend_alpha_kernel( const uchar4* __restrict__ in, int in_pitch, @@ -77,12 +81,12 @@ __global__ void blend_alpha_kernel( const uchar4 c2 = out[x+y*out_pitch]; const float a = alpha*(float(c1.w)/255.0f); - const float b = 1.0f - a; + const float b = 1.0f - (float(c1.w)/255.0f); out[x+y*out_pitch] = make_uchar4( - float(c1.x)*a + float(c2.x)*b, - float(c1.y)*a + float(c2.y)*b, - float(c1.z)*a + float(c2.z)*b, + clamp(float(c1.x)*a + float(c2.x)*b, 255.0f), + clamp(float(c1.y)*a + float(c2.y)*b, 255.0f), + clamp(float(c1.z)*a + float(c2.z)*b, 255.0f), 255.0f ); } @@ -107,3 +111,48 @@ void ftl::cuda::blend_alpha( out.width(), out.height(), alpha); cudaSafeCall( cudaGetLastError() ); } + +// ==== Composite ============================================================== + +__global__ void composite_kernel( + const uchar4* __restrict__ in, + int in_pitch, + uchar4* __restrict__ out, + int out_pitch, + int width, int height) { + + for (STRIDE_Y(y, height)) { + for (STRIDE_X(x, width)) { + const uchar4 c1 = in[x+y*in_pitch]; + const uchar4 c2 = out[x+y*out_pitch]; + + const float a = (float(c1.w)/255.0f); + const float b = 1.0f - a; + + out[x+y*out_pitch] = make_uchar4( + clamp(float(c1.x)*a + float(c2.x)*b, 255.0f), + clamp(float(c1.y)*a + float(c2.y)*b, 255.0f), + clamp(float(c1.z)*a + float(c2.z)*b, 255.0f), + 255.0f + ); + } + } +} + +void ftl::cuda::composite( + TextureObject<uchar4> &in, + TextureObject<uchar4> &out, + cudaStream_t stream) { + + static constexpr int THREADS_X = 32; + static constexpr int THREADS_Y = 8; + + const dim3 gridSize(6,64); + const dim3 blockSize(THREADS_X, THREADS_Y); + + composite_kernel<<<gridSize, blockSize, 0, stream>>>( + in.devicePtr(), in.pixelPitch(), + out.devicePtr(), out.pixelPitch(), + out.width(), out.height()); + cudaSafeCall( cudaGetLastError() ); +} diff --git a/components/renderers/cpp/src/colouriser.cpp b/components/renderers/cpp/src/colouriser.cpp index 70521ba4104d6d457ab1e502170fbb22b740a60a..95e85c47116857799276e52c18b8812033b735c6 100644 --- a/components/renderers/cpp/src/colouriser.cpp +++ b/components/renderers/cpp/src/colouriser.cpp @@ -114,6 +114,7 @@ Colouriser::~Colouriser() { TextureObject<uchar4> &Colouriser::colourise(ftl::rgbd::Frame &f, Channel c, cudaStream_t stream) { switch (c) { + case Channel::Overlay : return f.createTexture<uchar4>(c); case Channel::ColourHighRes : case Channel::Colour : case Channel::Colour2 : return _processColour(f,c,stream); diff --git a/applications/gui/src/overlay.cpp b/components/renderers/cpp/src/overlay.cpp similarity index 68% rename from applications/gui/src/overlay.cpp rename to components/renderers/cpp/src/overlay.cpp index 341f2daf69c89ae2f9563c93d1fe98d02e7b9fad..a539c6724c3abebd140ac9c218f83943181a3c74 100644 --- a/applications/gui/src/overlay.cpp +++ b/components/renderers/cpp/src/overlay.cpp @@ -1,7 +1,73 @@ -#include "overlay.hpp" +#include <ftl/render/overlay.hpp> #include <opencv2/imgproc.hpp> +#include <ftl/codecs/shapes.hpp> + +#define LOGURU_REPLACE_GLOG 1 +#include <loguru.hpp> + +using ftl::overlay::Overlay; +using ftl::codecs::Channel; + +Overlay::Overlay(nlohmann::json &config) : ftl::Configurable(config) { + +} + +Overlay::~Overlay() { + +} + +void Overlay::apply(ftl::rgbd::FrameSet &fs, cv::Mat &out, ftl::rgbd::FrameState &state) { + over_depth_.create(out.size(), CV_32F); + + if (value("show_poses", false)) { + for (size_t i=0; i<fs.frames.size(); ++i) { + auto pose = fs.frames[i].getPose().inverse() * state.getPose(); + Eigen::Vector4d pos = pose.inverse() * Eigen::Vector4d(0,0,0,1); + pos /= pos[3]; + + auto name = fs.frames[i].get<std::string>("name"); + ftl::overlay::drawCamera(state.getLeft(), out, over_depth_, fs.frames[i].getLeftCamera(), pose, cv::Scalar(0,0,255,255), 0.2,value("show_frustrum", false)); + if (name) ftl::overlay::drawText(state.getLeft(), out, over_depth_, *name, pos, 0.5, cv::Scalar(0,0,255,255)); + } + } + + if (value("show_shapes", false)) { + if (fs.hasChannel(Channel::Shapes3D)) { + std::vector<ftl::codecs::Shape3D> shapes; + fs.get(Channel::Shapes3D, shapes); + + for (auto &s : shapes) { + auto pose = s.pose.cast<double>().inverse() * state.getPose(); + Eigen::Vector4d pos = pose.inverse() * Eigen::Vector4d(0,0,0,1); + pos /= pos[3]; + + ftl::overlay::drawBox(state.getLeft(), out, over_depth_, pose, cv::Scalar(0,0,255,255), s.size.cast<double>()); + ftl::overlay::drawText(state.getLeft(), out, over_depth_, s.label, pos, 0.5, cv::Scalar(0,0,255,255)); + } + } + + for (size_t i=0; i<fs.frames.size(); ++i) { + if (fs.frames[i].hasChannel(Channel::Shapes3D)) { + std::vector<ftl::codecs::Shape3D> shapes; + fs.frames[i].get(Channel::Shapes3D, shapes); + + for (auto &s : shapes) { + auto pose = s.pose.cast<double>().inverse() * state.getPose(); + Eigen::Vector4d pos = pose.inverse() * Eigen::Vector4d(0,0,0,1); + pos /= pos[3]; + + ftl::overlay::drawBox(state.getLeft(), out, over_depth_, pose, cv::Scalar(0,0,255,255), s.size.cast<double>()); + ftl::overlay::drawText(state.getLeft(), out, over_depth_, s.label, pos, 0.5, cv::Scalar(0,0,255,255)); + } + } + } + } + + cv::flip(out, out, 0); +} + void ftl::overlay::draw3DLine( const ftl::rgbd::Camera &cam, cv::Mat &colour, @@ -73,6 +139,56 @@ void ftl::overlay::drawPoseBox( draw3DLine(cam, colour, depth, p011, p111, linecolour); } +void ftl::overlay::drawBox( + const ftl::rgbd::Camera &cam, + cv::Mat &colour, + cv::Mat &depth, + const Eigen::Matrix4d &pose, + const cv::Scalar &linecolour, + const Eigen::Vector3d &size) { + + double size2x = size[0]/2.0; + double size2y = size[1]/2.0; + double size2z = size[2]/2.0; + + Eigen::Vector4d p001 = pose.inverse() * Eigen::Vector4d(size2x,size2y,-size2z,1); + Eigen::Vector4d p011 = pose.inverse() * Eigen::Vector4d(size2x,-size2y,-size2z,1); + Eigen::Vector4d p111 = pose.inverse() * Eigen::Vector4d(-size2x,-size2y,-size2z,1); + Eigen::Vector4d p101 = pose.inverse() * Eigen::Vector4d(-size2x,size2y,-size2z,1); + Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(-size2x,-size2y,size2z,1); + Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(-size2x,size2y,size2z,1); + Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(size2x,-size2y,size2z,1); + Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(size2x,size2y,size2z,1); + + p001 /= p001[3]; + p011 /= p011[3]; + p111 /= p111[3]; + p101 /= p101[3]; + p110 /= p110[3]; + p100 /= p100[3]; + p010 /= p010[3]; + p000 /= p000[3]; + + if (p001[2] < 0.1 || p011[2] < 0.1 || p111[2] < 0.1 || p101[2] < 0.1 || p110[2] < 0.1 || p100[2] < 0.1 || p010[2] < 0.1 || p000[2] < 0.1) return; + + draw3DLine(cam, colour, depth, p000, p001, linecolour); + draw3DLine(cam, colour, depth, p000, p010, linecolour); + draw3DLine(cam, colour, depth, p000, p100, linecolour); + + draw3DLine(cam, colour, depth, p001, p011, linecolour); + draw3DLine(cam, colour, depth, p001, p101, linecolour); + + draw3DLine(cam, colour, depth, p010, p011, linecolour); + draw3DLine(cam, colour, depth, p010, p110, linecolour); + + draw3DLine(cam, colour, depth, p100, p101, linecolour); + draw3DLine(cam, colour, depth, p100, p110, linecolour); + + draw3DLine(cam, colour, depth, p101, p111, linecolour); + draw3DLine(cam, colour, depth, p110, p111, linecolour); + draw3DLine(cam, colour, depth, p011, p111, linecolour); +} + void ftl::overlay::drawRectangle( const ftl::rgbd::Camera &cam, cv::Mat &colour, diff --git a/components/rgbd-sources/include/ftl/rgbd/frameset.hpp b/components/rgbd-sources/include/ftl/rgbd/frameset.hpp index 1fcbf4f3f9a4322c27bc065886e142fef678adfa..0276a2e8aa389009450702fba4bd8e12c860ccff 100644 --- a/components/rgbd-sources/include/ftl/rgbd/frameset.hpp +++ b/components/rgbd-sources/include/ftl/rgbd/frameset.hpp @@ -4,6 +4,7 @@ #include <ftl/threads.hpp> #include <ftl/timer.hpp> #include <ftl/rgbd/frame.hpp> +#include <ftl/data/frameset.hpp> //#include <opencv2/core.hpp> #include <vector> @@ -17,43 +18,45 @@ static const size_t kMaxFramesInSet = 32; class Source; +typedef ftl::data::FrameSet<ftl::rgbd::Frame> FrameSet; + /** * Represents a set of synchronised frames, each with two channels. This is * used to collect all frames from multiple computers that have the same * timestamp. */ -struct FrameSet { - int id=0; - int64_t timestamp; // Millisecond timestamp of all frames - std::vector<ftl::rgbd::Frame> frames; - std::atomic<int> count; // Number of valid frames - std::atomic<unsigned int> mask; // Mask of all sources that contributed - bool stale; // True if buffers have been invalidated - SHARED_MUTEX mtx; +//struct FrameSet { +// int id=0; +// int64_t timestamp; // Millisecond timestamp of all frames +// std::vector<ftl::rgbd::Frame> frames; +// std::atomic<int> count; // Number of valid frames +// std::atomic<unsigned int> mask; // Mask of all sources that contributed +// bool stale; // True if buffers have been invalidated +// SHARED_MUTEX mtx; /** * Upload all specified host memory channels to GPU memory. */ - void upload(ftl::codecs::Channels<0>, cudaStream_t stream=0); +// void upload(ftl::codecs::Channels<0>, cudaStream_t stream=0); /** * Download all specified GPU memory channels to host memory. */ - void download(ftl::codecs::Channels<0>, cudaStream_t stream=0); +// void download(ftl::codecs::Channels<0>, cudaStream_t stream=0); /** * Move the entire frameset to another frameset object. This will * invalidate the current frameset object as all memory buffers will be * moved. */ - void swapTo(ftl::rgbd::FrameSet &); +// void swapTo(ftl::rgbd::FrameSet &); /** * Clear all channels and all memory allocations within those channels. * This will perform a resetFull on all frames in the frameset. */ - void resetFull(); -}; +// void resetFull(); +//}; /** * Callback type for receiving video frames. diff --git a/components/rgbd-sources/src/frameset.cpp b/components/rgbd-sources/src/frameset.cpp index b14749fb60db736861c2d5ec2a46d466bfc3f508..4217cd3071abce6397bcf9630378e4e5aaa30281 100644 --- a/components/rgbd-sources/src/frameset.cpp +++ b/components/rgbd-sources/src/frameset.cpp @@ -16,7 +16,7 @@ using ftl::codecs::Channel; using ftl::rgbd::FrameSet; using ftl::codecs::Channels; -void FrameSet::upload(ftl::codecs::Channels<0> c, cudaStream_t stream) { +/*void FrameSet::upload(ftl::codecs::Channels<0> c, cudaStream_t stream) { for (auto &f : frames) { f.upload(c, stream); } @@ -55,7 +55,7 @@ void FrameSet::resetFull() { //for (auto &f : frames) { //f.resetFull(); //} -} +}*/ // ============================================================================= @@ -380,6 +380,8 @@ ftl::rgbd::FrameSet *Builder::_addFrameset(int64_t timestamp) { newf->mask = 0; newf->stale = false; newf->frames.resize(size_); + newf->pose.setIdentity(); + newf->clearData(); for (auto &f : newf->frames) f.reset(); diff --git a/components/structures/include/ftl/data/frameset.hpp b/components/structures/include/ftl/data/frameset.hpp index a233b8f9bd89b250ff0c0db1c95090450793e7e1..a0839983777d71f13cfaf98786606be2ee4328aa 100644 --- a/components/structures/include/ftl/data/frameset.hpp +++ b/components/structures/include/ftl/data/frameset.hpp @@ -22,7 +22,9 @@ static const size_t kMaxFramesInSet = 32; * timestamp. */ template <typename FRAME> -struct FrameSet { +class FrameSet { + public: + int id=0; int64_t timestamp; // Millisecond timestamp of all frames std::vector<FRAME> frames; @@ -31,31 +33,59 @@ struct FrameSet { bool stale; // True if buffers have been invalidated SHARED_MUTEX mtx; + Eigen::Matrix4d pose; // Set to identity by default. + /** - * Upload all specified host memory channels to GPU memory. + * Move the entire frameset to another frameset object. This will + * invalidate the current frameset object as all memory buffers will be + * moved. */ - //void upload(ftl::codecs::Channels<0>, cudaStream_t stream=0); + void swapTo(ftl::data::FrameSet<FRAME> &); + + typedef FRAME Frame; + typedef std::function<bool(ftl::data::FrameSet<FRAME> &)> Callback; /** - * Download all specified GPU memory channels to host memory. + * Get the data from a data channel. This only works for the data channels + * and will throw an exception with any others. */ - //void download(ftl::codecs::Channels<0>, cudaStream_t stream=0); + template <typename T> void get(ftl::codecs::Channel channel, T ¶ms) const; /** - * Move the entire frameset to another frameset object. This will - * invalidate the current frameset object as all memory buffers will be - * moved. + * Set the value of a channel. Some channels should not be modified via the + * non-const get method, for example the data channels. */ - void swapTo(ftl::data::FrameSet<FRAME> &); + template <typename T> void create(ftl::codecs::Channel channel, const T &value); /** - * Clear all channels and all memory allocations within those channels. - * This will perform a resetFull on all frames in the frameset. + * Access the raw data channel vector object. */ - //void resetFull(); + const std::vector<unsigned char> &getRawData(ftl::codecs::Channel c) const; - typedef FRAME Frame; - typedef std::function<bool(ftl::data::FrameSet<FRAME> &)> Callback; + /** + * Provide raw data for a data channel. + */ + void createRawData(ftl::codecs::Channel c, const std::vector<unsigned char> &v); + + /** + * Is there valid data in channel (either host or gpu). This does not + * verify that any memory or data exists for the channel. + */ + inline bool hasChannel(ftl::codecs::Channel channel) const { + int c = static_cast<int>(channel); + if (c == 66) return true; + else if (c >= 2048) return data_channels_.has(channel); + return false; + } + + void clearData() { + data_.clear(); + data_channels_.clear(); + } + + private: + std::unordered_map<int, std::vector<unsigned char>> data_; + ftl::codecs::Channels<2048> data_channels_; }; /** @@ -93,4 +123,77 @@ class Generator { } } +// === Implementations ========================================================= + +template <typename FRAME> +void ftl::data::FrameSet<FRAME>::swapTo(ftl::data::FrameSet<FRAME> &fs) { + //UNIQUE_LOCK(fs.mtx, lk); + std::unique_lock<std::shared_mutex> lk(fs.mtx); + + //if (fs.frames.size() != frames.size()) { + // Assume "this" is correct and "fs" is not. + fs.frames.resize(frames.size()); + //} + + fs.timestamp = timestamp; + fs.count = static_cast<int>(count); + fs.stale = stale; + fs.mask = static_cast<unsigned int>(mask); + fs.id = id; + fs.pose = pose; + + for (size_t i=0; i<frames.size(); ++i) { + frames[i].swapTo(ftl::codecs::Channels<0>::All(), fs.frames[i]); + } + + std::swap(fs.data_, data_); + fs.data_channels_ = data_channels_; + data_channels_.clear(); + + stale = true; +} + +// Default data channel implementation +template <typename FRAME> +// cppcheck-suppress * +template <typename T> +void ftl::data::FrameSet<FRAME>::get(ftl::codecs::Channel channel, T ¶ms) const { + if (static_cast<int>(channel) < static_cast<int>(ftl::codecs::Channel::Data)) throw FTL_Error("Cannot use generic type with non data channel"); + if (!hasChannel(channel)) throw FTL_Error("Data channel does not exist"); + + const auto &i = data_.find(static_cast<int>(channel)); + if (i == data_.end()) throw FTL_Error("Data channel does not exist"); + + auto unpacked = msgpack::unpack((const char*)(*i).second.data(), (*i).second.size()); + unpacked.get().convert(params); +} + +template <typename FRAME> +// cppcheck-suppress * +template <typename T> +void ftl::data::FrameSet<FRAME>::create(ftl::codecs::Channel channel, const T &value) { + if (static_cast<int>(channel) < static_cast<int>(ftl::codecs::Channel::Data)) throw FTL_Error("Cannot use generic type with non data channel"); + + data_channels_ += channel; + + auto &v = *std::get<0>(data_.insert({static_cast<int>(channel),{}})); + v.second.resize(0); + ftl::util::FTLVectorBuffer buf(v.second); + msgpack::pack(buf, value); +} + +template <typename FRAME> +const std::vector<unsigned char> &ftl::data::FrameSet<FRAME>::getRawData(ftl::codecs::Channel channel) const { + if (static_cast<int>(channel) < static_cast<int>(ftl::codecs::Channel::Data)) throw FTL_Error("Non data channel"); + if (!hasChannel(channel)) throw FTL_Error("Data channel does not exist"); + + return data_.at(static_cast<int>(channel)); +} + +template <typename FRAME> +void ftl::data::FrameSet<FRAME>::createRawData(ftl::codecs::Channel c, const std::vector<unsigned char> &v) { + data_.insert({static_cast<int>(c), v}); + data_channels_ += c; +} + #endif // _FTL_DATA_FRAMESET_HPP_