diff --git a/applications/gui/src/camera.cpp b/applications/gui/src/camera.cpp index c0691c563943c50821ed95c5de3cc3f6e892a00f..de1518695bb7b96d8871f64c180c702e822062bb 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 ce92c3579bc5fa4af3cccff06fd7a37160d3294e..f5460febc689584dcdca6dc24c5c12a3f9383081 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 78a02bf514f1de0cf83f6fd2d70208047f6682ed..d9ce665724bbf94c9382bfe4b5b7e12c3307a093 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); } }