Draw camera with focal and principal point

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::drawPoseCone(state_.getLeft(), im1_, over_depth, 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);
 			if (name) ftl::overlay::drawText(state_.getLeft(), im1_, over_depth, *name, pos, 0.5, cv::Scalar(0,0,255,255));
 		}
 	}

applications/gui/src/overlay.cpp

@@ -110,6 +110,53 @@ void ftl::overlay::drawPoseCone(
     draw3DLine(cam, colour, depth, p110, origin, linecolour);
 }
 
+void ftl::overlay::drawCamera(
+        const ftl::rgbd::Camera &vcam,
+        cv::Mat &colour,
+        cv::Mat &depth,
+        const ftl::rgbd::Camera &camera,
+        const Eigen::Matrix4d &pose,
+        const cv::Scalar &linecolour,
+        double scale) {
+
+    //double size2 = size;
+
+    const auto &params = camera;
+    double width = (static_cast<double>(params.width) / static_cast<double>(params.fx)) * scale;
+    double height = (static_cast<double>(params.height) / static_cast<double>(params.fx)) * scale;
+    double width2 = width / 2.0;
+    double height2 = height / 2.0;
+
+    double principx = (((static_cast<double>(params.width) / 2.0) + params.cx) / static_cast<double>(params.fx)) * scale;
+    double principy = (((static_cast<double>(params.height) / 2.0) + params.cy) / static_cast<double>(params.fx)) * scale;
+
+    Eigen::Vector4d p110 = pose.inverse() * Eigen::Vector4d(-width2,-height2,scale,1);
+    Eigen::Vector4d p100 = pose.inverse() * Eigen::Vector4d(-width2,height2,scale,1);
+    Eigen::Vector4d p010 = pose.inverse() * Eigen::Vector4d(width2,-height2,scale,1);
+    Eigen::Vector4d p000 = pose.inverse() * Eigen::Vector4d(width2,height2,scale,1);
+    Eigen::Vector4d origin = pose.inverse() * Eigen::Vector4d(principx,principy,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(vcam, colour, depth, p000, origin, linecolour);
+    draw3DLine(vcam, colour, depth, p000, p010, linecolour);
+    draw3DLine(vcam, colour, depth, p000, p100, linecolour);
+
+    draw3DLine(vcam, colour, depth, p010, origin, linecolour);
+    draw3DLine(vcam, colour, depth, p010, p110, linecolour);
+
+    draw3DLine(vcam, colour, depth, p100, origin, linecolour);
+    draw3DLine(vcam, colour, depth, p100, p110, linecolour);
+
+    draw3DLine(vcam, colour, depth, p110, origin, linecolour);
+}
+
 void ftl::overlay::drawText(
         const ftl::rgbd::Camera &cam,
         cv::Mat &colour,

applications/gui/src/overlay.hpp

@@ -3,7 +3,7 @@
 
 #include <opencv2/core/mat.hpp>
 #include <Eigen/Eigen>
-#include <ftl/rgbd/camera.hpp>
+#include <ftl/rgbd/frame.hpp>
 
 namespace ftl {
 namespace overlay {
@@ -44,6 +44,15 @@ void drawPoseCone(
     const cv::Scalar &linecolour,
     double size);
 
+void drawCamera(
+    const ftl::rgbd::Camera &cam,
+    cv::Mat &colour,
+    cv::Mat &depth,
+    const ftl::rgbd::Camera &camera,
+    const Eigen::Matrix4d &pose,
+    const cv::Scalar &linecolour,
+    double scale);
+
 }
 }