From e99ddb339f953ac50fe934b19932a20fd07d10b8 Mon Sep 17 00:00:00 2001
From: Nicolas Pope <nwpope@utu.fi>
Date: Fri, 31 Jan 2020 12:35:35 +0200
Subject: [PATCH] Show camera frustrums
---
applications/gui/src/camera.cpp | 2 +-
applications/gui/src/overlay.cpp | 19 ++++++++++++++++++-
applications/gui/src/overlay.hpp | 3 ++-
3 files changed, 21 insertions(+), 3 deletions(-)
diff --git a/applications/gui/src/camera.cpp b/applications/gui/src/camera.cpp
index c0691c563..de1518695 100644
--- a/applications/gui/src/camera.cpp
+++ b/applications/gui/src/camera.cpp
@@ -268,7 +268,7 @@ void ftl::gui::Camera::_draw(ftl::rgbd::FrameSet &fs) {
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);
+ 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));
}
}
diff --git a/applications/gui/src/overlay.cpp b/applications/gui/src/overlay.cpp
index ce92c3579..f5460febc 100644
--- a/applications/gui/src/overlay.cpp
+++ b/applications/gui/src/overlay.cpp
@@ -117,7 +117,7 @@ void ftl::overlay::drawCamera(
const ftl::rgbd::Camera &camera,
const Eigen::Matrix4d &pose,
const cv::Scalar &linecolour,
- double scale) {
+ double scale, bool frustrum) {
//double size2 = size;
@@ -155,6 +155,23 @@ void ftl::overlay::drawCamera(
draw3DLine(vcam, colour, depth, p100, p110, linecolour);
draw3DLine(vcam, colour, depth, p110, origin, linecolour);
+
+ if (frustrum) {
+ const double fscale = 16.0;
+ Eigen::Vector4d f110 = pose.inverse() * Eigen::Vector4d(-width2*fscale,-height2*fscale,scale*fscale,1);
+ Eigen::Vector4d f100 = pose.inverse() * Eigen::Vector4d(-width2*fscale,height2*fscale,scale*fscale,1);
+ Eigen::Vector4d f010 = pose.inverse() * Eigen::Vector4d(width2*fscale,-height2*fscale,scale*fscale,1);
+ Eigen::Vector4d f000 = pose.inverse() * Eigen::Vector4d(width2*fscale,height2*fscale,scale*fscale,1);
+ draw3DLine(vcam, colour, depth, f000, p000, cv::Scalar(0,255,0,0));
+ draw3DLine(vcam, colour, depth, f010, p010, cv::Scalar(0,255,0,0));
+ draw3DLine(vcam, colour, depth, f100, p100, cv::Scalar(0,255,0,0));
+ draw3DLine(vcam, colour, depth, f110, p110, cv::Scalar(0,255,0,0));
+
+ draw3DLine(vcam, colour, depth, f000, f010, cv::Scalar(0,255,0,0));
+ draw3DLine(vcam, colour, depth, f000, f100, cv::Scalar(0,255,0,0));
+ draw3DLine(vcam, colour, depth, f010, f110, cv::Scalar(0,255,0,0));
+ draw3DLine(vcam, colour, depth, f100, f110, cv::Scalar(0,255,0,0));
+ }
}
void ftl::overlay::drawText(
diff --git a/applications/gui/src/overlay.hpp b/applications/gui/src/overlay.hpp
index 78a02bf51..d9ce66572 100644
--- a/applications/gui/src/overlay.hpp
+++ b/applications/gui/src/overlay.hpp
@@ -51,7 +51,8 @@ void drawCamera(
const ftl::rgbd::Camera &camera,
const Eigen::Matrix4d &pose,
const cv::Scalar &linecolour,
- double scale);
+ double scale=1.0,
+ bool frustrum=false);
}
}
--
GitLab