diff --git a/applications/gui/CMakeLists.txt b/applications/gui/CMakeLists.txt index cbb25e837f36e0b8e84175da47094fd6ddf40b28..60865a91c88e0f88c26f2695bfbbfe32b40683ff 100644 --- a/applications/gui/CMakeLists.txt +++ b/applications/gui/CMakeLists.txt @@ -14,6 +14,7 @@ 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 25dab31d1f4face1e7927a34fcb523be269e6791..7fb1276df7ad1762a7d720aeaa150a5219fb6ac6 100644 --- a/applications/gui/src/camera.cpp +++ b/applications/gui/src/camera.cpp @@ -8,6 +8,8 @@ #include <ftl/operators/antialiasing.hpp> +#include "overlay.hpp" + #define LOGURU_REPLACE_GLOG 1 #include <loguru.hpp> @@ -251,6 +253,22 @@ void ftl::gui::Camera::_draw(ftl::rgbd::FrameSet &fs) { _downloadFrames(&frame_); + 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 (int 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::drawPoseCone(state_.getLeft(), im1_, over_depth, pose, cv::Scalar(0,0,255,255), 0.2); + 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; diff --git a/applications/gui/src/overlay.cpp b/applications/gui/src/overlay.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f38d1056f44f23d7bb0a3adc4755b11e08f4476f --- /dev/null +++ b/applications/gui/src/overlay.cpp @@ -0,0 +1,125 @@ +#include "overlay.hpp" + +#include <opencv2/imgproc.hpp> + +void ftl::overlay::draw3DLine( + const ftl::rgbd::Camera &cam, + cv::Mat &colour, + cv::Mat &depth, + const Eigen::Vector4d &begin, + const Eigen::Vector4d &end, + const cv::Scalar &linecolour) { + + + auto begin_pos = cam.camToScreen<int2>(make_float3(begin[0], begin[1], begin[2])); + auto end_pos = cam.camToScreen<int2>(make_float3(end[0], end[1], end[2])); + + cv::LineIterator lineit(colour, cv::Point(begin_pos.x, colour.rows-begin_pos.y), cv::Point(end_pos.x, colour.rows-end_pos.y)); + double z_grad = (end[2] - begin[2]) / lineit.count; + double current_z = begin[2]; + + for(int i = 0; i < lineit.count; i++, ++lineit) { + colour.at<cv::Vec4b>(lineit.pos()) = linecolour; + depth.at<float>(lineit.pos()) = current_z; + current_z += z_grad; + } +} + +void ftl::overlay::drawPoseBox( + const ftl::rgbd::Camera &cam, + cv::Mat &colour, + cv::Mat &depth, + const Eigen::Matrix4d &pose, + const cv::Scalar &linecolour, + double size) { + + double size2 = size/2.0; + + Eigen::Vector4d p001 = pose.inverse() * Eigen::Vector4d(size2,size2,-size2,1); + Eigen::Vector4d p011 = pose.inverse() * Eigen::Vector4d(size2,-size2,-size2,1); + Eigen::Vector4d p111 = pose.inverse() * Eigen::Vector4d(-size2,-size2,-size2,1); + Eigen::Vector4d p101 = pose.inverse() * Eigen::Vector4d(-size2,size2,-size2,1); + Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(-size2,-size2,size2,1); + Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(-size2,size2,size2,1); + Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(size2,-size2,size2,1); + Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(size2,size2,size2,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::drawPoseCone( + const ftl::rgbd::Camera &cam, + cv::Mat &colour, + cv::Mat &depth, + const Eigen::Matrix4d &pose, + const cv::Scalar &linecolour, + double size) { + + double size2 = size; + + Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(-size2,-size2,size2,1); + Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(-size2,size2,size2,1); + Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(size2,-size2,size2,1); + Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(size2,size2,size2,1); + Eigen::Vector4d origin = pose.inverse() * Eigen::Vector4d(0,0,0,1); + + p110 /= p110[3]; + p100 /= p100[3]; + p010 /= p010[3]; + p000 /= p000[3]; + origin /= origin[3]; + + if (origin[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, origin, linecolour); + draw3DLine(cam, colour, depth, p000, p010, linecolour); + draw3DLine(cam, colour, depth, p000, p100, linecolour); + + draw3DLine(cam, colour, depth, p010, origin, linecolour); + draw3DLine(cam, colour, depth, p010, p110, linecolour); + + draw3DLine(cam, colour, depth, p100, origin, linecolour); + draw3DLine(cam, colour, depth, p100, p110, linecolour); + + draw3DLine(cam, colour, depth, p110, origin, linecolour); +} + +void ftl::overlay::drawText( + const ftl::rgbd::Camera &cam, + cv::Mat &colour, + cv::Mat &depth, + const std::string &text, + const Eigen::Vector4d &pos, + double size, + const cv::Scalar &textcolour) { + + auto pt = cam.camToScreen<int2>(make_float3(pos[0], pos[1], pos[2])); + if (pos[2] < 0.1) return; + cv::putText(colour, text, cv::Point(pt.x, colour.rows-pt.y), 0, size, textcolour, 1, cv::LINE_8, true); +} diff --git a/applications/gui/src/overlay.hpp b/applications/gui/src/overlay.hpp new file mode 100644 index 0000000000000000000000000000000000000000..336dee0be055d6ca092a2377c1a0a269554568b3 --- /dev/null +++ b/applications/gui/src/overlay.hpp @@ -0,0 +1,50 @@ +#ifndef _FTL_GUI_OVERLAY_HPP_ +#define _FTL_GUI_OVERLAY_HPP_ + +#include <opencv2/core/mat.hpp> +#include <Eigen/Eigen> +#include <ftl/rgbd/camera.hpp> + +namespace ftl { +namespace overlay { + +void draw3DLine( + const ftl::rgbd::Camera &cam, + cv::Mat &colour, + cv::Mat &depth, + const Eigen::Vector4d &begin, + const Eigen::Vector4d &end, + const cv::Scalar &linecolour); + +void drawText( + const ftl::rgbd::Camera &cam, + cv::Mat &colour, + cv::Mat &depth, + const std::string &text, + const Eigen::Vector4d &pos, + double size, + const cv::Scalar &textcolour); + +/** + * Draw a box at a given pose location and rotation. + */ +void drawPoseBox( + const ftl::rgbd::Camera &cam, + cv::Mat &colour, + cv::Mat &depth, + const Eigen::Matrix4d &pose, + const cv::Scalar &linecolour, + double size); + +void drawPoseCone( + const ftl::rgbd::Camera &cam, + cv::Mat &colour, + cv::Mat &depth, + const Eigen::Matrix4d &pose, + const cv::Scalar &linecolour, + double size); + +} +} + +#endif // _FTL_GUI_OVERLAY_HPP_