merge lgv coordinate overlay into linbo encoder

cmvr-es/devices/camera/common/include/camera_stream_encoder.h

@@ -5,11 +5,20 @@
 #include <vector>
 
 #include <opencv2/opencv.hpp>
+#include <Eigen/Core>
 
 #include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h"
 
 namespace cmvr::device {
 
+struct Rs2Intrinsics;
+
+void drawCoordinateFrame(cv::Mat& image,
+                         const Eigen::Matrix4d& T_C_Frame,
+                         const Rs2Intrinsics& intrinsics,
+                         double axis_length_m,
+                         const std::string& label);
+
 struct FfmpegEncoderInfo {
     std::string codec_name;
     int width = 0;

cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp

@@ -1,6 +1,7 @@
 #include "devices/camera/common/include/camera_stream_encoder.h"
 
 #include <chrono>
+#include <cmath>
 #include <ctime>
 #include <iomanip>
 #include <sstream>
@@ -11,6 +12,7 @@
 #include <opencv2/imgproc.hpp>
 
 #include "common/base/logging/logger.h"
+#include "devices/camera/abstract_camera.h"
 
 namespace cmvr::device {
 namespace {
@@ -48,6 +50,36 @@ void drawTimeStamp(cv::Mat& image)
     cv::putText(image, time_str, text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), thickness);
 }
 
+bool projectPoint(const Eigen::Vector3d& point,
+                  const Rs2Intrinsics& intrinsics,
+                  cv::Point& pixel)
+{
+    if (!point.allFinite() || point.z() <= 1e-9 ||
+        !std::isfinite(intrinsics.fx) || !std::isfinite(intrinsics.fy) ||
+        !std::isfinite(intrinsics.cx) || !std::isfinite(intrinsics.cy) ||
+        intrinsics.fx <= 0.0f || intrinsics.fy <= 0.0f) {
+        return false;
+    }
+    const double u = intrinsics.fx * point.x() / point.z() + intrinsics.cx;
+    const double v = intrinsics.fy * point.y() / point.z() + intrinsics.cy;
+    if (!std::isfinite(u) || !std::isfinite(v)) {
+        return false;
+    }
+    pixel = cv::Point(cvRound(u), cvRound(v));
+    return true;
+}
+
+void drawOutlinedText(cv::Mat& image,
+                      const std::string& text,
+                      const cv::Point& origin,
+                      const cv::Scalar& color)
+{
+    cv::putText(image, text, origin, cv::FONT_HERSHEY_SIMPLEX, 0.55,
+                cv::Scalar(0, 0, 0), 3, cv::LINE_AA);
+    cv::putText(image, text, origin, cv::FONT_HERSHEY_SIMPLEX, 0.55,
+                color, 1, cv::LINE_AA);
+}
+
 const AVCodec* findEncoder(const std::string& codec_name)
 {
     if (codec_name == "h264_qsv" || codec_name == "H264_QSV") {
@@ -133,6 +165,47 @@ bool encodePreparedFrame(FfmpegEncoderInfo& encoder,
 
 } // namespace
 
+void drawCoordinateFrame(cv::Mat& image,
+                         const Eigen::Matrix4d& T_C_Frame,
+                         const Rs2Intrinsics& intrinsics,
+                         const double axis_length_m,
+                         const std::string& label)
+{
+    if (image.empty() || image.channels() < 3 || !T_C_Frame.allFinite() ||
+        !std::isfinite(axis_length_m) || axis_length_m <= 0.0) {
+        return;
+    }
+
+    const Eigen::Vector3d origin = T_C_Frame.block<3, 1>(0, 3);
+    const Eigen::Matrix3d rotation = T_C_Frame.block<3, 3>(0, 0);
+    cv::Point origin_px;
+    if (!projectPoint(origin, intrinsics, origin_px)) {
+        return;
+    }
+
+    const Eigen::Vector3d endpoints[] = {
+        origin + rotation.col(0) * axis_length_m,
+        origin + rotation.col(1) * axis_length_m,
+        origin + rotation.col(2) * axis_length_m};
+    const cv::Scalar colors[] = {
+        cv::Scalar(0, 0, 255), cv::Scalar(0, 255, 0), cv::Scalar(255, 0, 0)};
+    const char* names[] = {"X", "Y", "Z"};
+    for (int i = 0; i < 3; ++i) {
+        cv::Point endpoint_px;
+        if (projectPoint(endpoints[i], intrinsics, endpoint_px)) {
+            cv::arrowedLine(image, origin_px, endpoint_px, colors[i], 2,
+                            cv::LINE_AA, 0, 0.15);
+            drawOutlinedText(image, names[i], endpoint_px + cv::Point(4, -4), colors[i]);
+        }
+    }
+    cv::drawMarker(image, origin_px, cv::Scalar(255, 255, 255),
+                   cv::MARKER_CROSS, 9, 1, cv::LINE_AA);
+    if (!label.empty()) {
+        drawOutlinedText(image, label, origin_px + cv::Point(7, -7),
+                         cv::Scalar(255, 255, 255));
+    }
+}
+
 FfmpegEncoderInfo::~FfmpegEncoderInfo()
 {
     if (frame) {