Merge branch 'feature/241/overlay' into 'master'

Implements #241 3d line and box drawing

Closes #241

See merge request nicolas.pope/ftl!234

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)

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;

applications/gui/src/overlay.cpp

+#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);
+}

applications/gui/src/overlay.hpp

+#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_