Accelerated jpeg deocde

.gitmodules

@@ -7,3 +7,6 @@
 [submodule "bos-logs"]
 	path = bos-logs
 	url = https://github.com/frc971/bos-logs
+[submodule "third_party/cpp-mjpeg-streamer"]
+	path = third_party/cpp-mjpeg-streamer
+	url = https://github.com/nadjieb/cpp-mjpeg-streamer.git

CMakeLists.txt

@@ -49,6 +49,8 @@ find_package(libuvc REQUIRED)
 add_subdirectory(third_party/json)
 add_subdirectory(third_party/971apriltag)
 add_subdirectory(third_party/abseil-cpp)
+add_subdirectory(third_party/nvjpeg)
+add_subdirectory(third_party/cpp-mjpeg-streamer)
 
 include(FetchContent)
 FetchContent_Declare(

decode_cat_with_nvjpeg.cpp

@@ -0,0 +1,126 @@
+#include <cstring>
+#include <fstream>
+#include <iostream>
+#include <iterator>
+#include <string>
+#include <vector>
+
+#include <linux/videodev2.h>
+
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "NvJpegDecoder.h"
+
+namespace {
+
+bool copy_plane(const NvBuffer::NvBufferPlane& plane, int rows, int cols, unsigned char* dst)
+{
+    if (!plane.data)
+    {
+        return false;
+    }
+
+    for (int y = 0; y < rows; ++y)
+    {
+        std::memcpy(dst + (y * cols), plane.data + (y * plane.fmt.stride), cols);
+    }
+
+    return true;
+}
+
+} // namespace
+
+int main(int argc, char* argv[])
+{
+    const std::string input_path = (argc > 1) ? argv[1] : "cat.jpg";
+    const std::string output_path = (argc > 2) ? argv[2] : "cat_nvjpeg.png";
+
+    std::ifstream input_stream(input_path, std::ios::binary);
+    if (!input_stream)
+    {
+        std::cerr << "Failed to open input JPEG: " << input_path << '\n';
+        return 1;
+    }
+
+    std::vector<unsigned char> jpeg_bytes((std::istreambuf_iterator<char>(input_stream)),
+        std::istreambuf_iterator<char>());
+
+    if (jpeg_bytes.empty())
+    {
+        std::cerr << "Input file is empty: " << input_path << '\n';
+        return 1;
+    }
+
+    NvJPEGDecoder* decoder = NvJPEGDecoder::createJPEGDecoder("nvjpeg_decoder");
+    if (!decoder)
+    {
+        std::cerr << "Failed to create NvJPEGDecoder" << '\n';
+        return 1;
+    }
+
+    NvBuffer* decoded_buffer = nullptr;
+    uint32_t pixfmt = 0;
+    uint32_t width = 0;
+    uint32_t height = 0;
+
+    const int decode_status = decoder->decodeToBuffer(
+        &decoded_buffer,
+        jpeg_bytes.data(),
+        jpeg_bytes.size(),
+        &pixfmt,
+        &width,
+        &height);
+
+    if (decode_status < 0 || !decoded_buffer)
+    {
+        std::cerr << "NvJPEGDecoder failed to decode: " << input_path << '\n';
+        delete decoder;
+        return 1;
+    }
+
+    if (pixfmt != V4L2_PIX_FMT_YUV420M || decoded_buffer->n_planes < 3)
+    {
+        std::cerr << "Unsupported decoded format. Expected YUV420M, got pixfmt=" << pixfmt
+                  << " with planes=" << decoded_buffer->n_planes << '\n';
+        delete decoded_buffer;
+        delete decoder;
+        return 1;
+    }
+
+    cv::Mat i420(height + (height / 2), width, CV_8UC1);
+
+    unsigned char* y_dst = i420.ptr<unsigned char>(0);
+    unsigned char* u_dst = i420.ptr<unsigned char>(height);
+    unsigned char* v_dst = i420.ptr<unsigned char>(height + (height / 4));
+
+    const bool y_ok = copy_plane(decoded_buffer->planes[0], static_cast<int>(height), static_cast<int>(width), y_dst);
+    const bool u_ok = copy_plane(decoded_buffer->planes[1], static_cast<int>(height / 2), static_cast<int>(width / 2), u_dst);
+    const bool v_ok = copy_plane(decoded_buffer->planes[2], static_cast<int>(height / 2), static_cast<int>(width / 2), v_dst);
+
+    if (!y_ok || !u_ok || !v_ok)
+    {
+        std::cerr << "Failed to copy decoded YUV planes" << '\n';
+        delete decoded_buffer;
+        delete decoder;
+        return 1;
+    }
+
+    cv::Mat bgr;
+    cv::cvtColor(i420, bgr, cv::COLOR_YUV2BGR_I420);
+
+    if (!cv::imwrite(output_path, bgr))
+    {
+        std::cerr << "Failed to write output image: " << output_path << '\n';
+        delete decoded_buffer;
+        delete decoder;
+        return 1;
+    }
+
+    std::cout << "Decoded " << input_path << " with NvJPEGDecoder (" << width << "x" << height
+              << ") and wrote " << output_path << '\n';
+
+    delete decoded_buffer;
+    delete decoder;
+    return 0;
+}

src/calibration/frame_shower.cc

@@ -1,4 +1,5 @@
 #include <memory>
+#include "NvJpegDecoder.h"
 #include "absl/flags/flag.h"
 #include "absl/flags/parse.h"
 #include "src/camera/camera.h"
@@ -7,13 +8,15 @@
 #include "src/camera/cv_camera.h"
 #include "src/camera/select_camera.h"
 #include "src/utils/log.h"
+#include "src/utils/stop.h"
 
 ABSL_FLAG(std::optional<std::string>, camera_name, std::nullopt, "");  //NOLINT
 ABSL_FLAG(std::optional<int>, port, std::nullopt, "");                 //NOLINT
 ABSL_FLAG(std::optional<std::string>, log_path, std::nullopt, "");     //NOLINT
 
 auto main(int argc, char* argv[]) -> int {
   absl::ParseCommandLine(argc, argv);
+  stop::RegisterHandler();
 
   std::unique_ptr<camera::ICamera> camera = camera::SelectCameraConfig(
       absl::GetFlag(FLAGS_camera_name), camera::GetCameraConstants());
@@ -26,7 +29,8 @@ auto main(int argc, char* argv[]) -> int {
 
   cv::Mat frame = camera->GetFrame().frame;
   LOG(INFO) << "Size of frame: " << frame.size;
-  while (true) {
+  while (!stop::stop) {
+    LOG(INFO) << stop::stop;
     frame = camera->GetFrame().frame;
     streamer.WriteFrame(frame);
   }

src/camera/CMakeLists.txt

@@ -7,6 +7,17 @@ target_link_libraries(CscoreStreamer PRIVATE
   utils
 )
 
+add_library(NvidiaMjpegStreamer)
+target_sources(NvidiaMjpegStreamer PRIVATE
+  PRIVATE
+    nvidia_mjpeg_streamer.cc
+)
+target_link_libraries(NvidiaMjpegStreamer PRIVATE
+  utils
+  nadjieb_mjpeg_streamer::nadjieb_mjpeg_streamer
+  nvjpeg
+)
+
 add_library(DiskCamera)
 target_sources(DiskCamera
   PRIVATE
@@ -35,6 +46,7 @@ target_sources(SelectCamera
 target_link_libraries(SelectCamera PRIVATE
   DiskCamera
   CvCamera
+  UVCCamera
   utils
 )
 
@@ -70,10 +82,11 @@ target_sources(UVCCamera
   PRIVATE
     uvc_camera.cc
 )
-target_link_libraries(UVCCamera PRIVATE
+target_link_libraries(UVCCamera PUBLIC 
   uvc
   absl::status
   utils
+  nvjpeg
 )
 
 add_library(camera INTERFACE)
@@ -86,4 +99,5 @@ target_link_libraries(camera INTERFACE
   CameraConstants
   MultiCameraSource
   UVCCamera
+  NvidiaMjpegStreamer
 )

src/camera/nvidia_mjpeg_streamer.cc

@@ -0,0 +1,35 @@
+#include "src/camera/nvidia_mjpeg_streamer.h"
+#include <opencv2/imgproc.hpp>
+#include "nvbufsurface.h"
+
+namespace camera {
+NvidiaMjpegStreamer::NvidiaMjpegStreamer(uint port)
+    : streamer_(),
+      encoder_(NvJPEGEncoder::createJPEGEncoder("streamer_encoder")) {
+  streamer_.start(port);
+}
+
+void NvidiaMjpegStreamer::WriteFrame(const cv::Mat& mat) {
+  size_t jpeg_size = mat.total() * mat.elemSize();
+  // encoder_->setCropRect(0, 0, 0, 0);
+  cv::Mat bgraMat;
+  cv::cvtColor(mat, bgraMat, cv::COLOR_BGR2BGRA);
+
+  NvBufSurfaceParamsEx a;
+  NvBufSurfaceChromaSubsamplingParams b;
+  NvBufSurfaceCreateParams c;
+  int dmaFd;
+  // NvBufferCreateParams params = {.width = bgraMat.cols,
+  //                                .height = bgraMat.rows,
+  //                                .payloadType = NvBufferPayload_SurfArray,
+  //                                .memsize = (4 * bgraMat.cols * bgraMat.rows),
+  //                                .colorFormat = NvBufferColorFormat_ARGB32,
+  //                                .nvbuf_tag = NvBufferTag_NONE};
+
+  Raw2NvBufSurface();
+
+  // encoder_->encodeFromBuffer(&buffer, J_COLOR_SPACE color_space,
+  //                            unsigned char** out_buf,
+  //                            unsigned long& out_buf_size)
+}
+}  // namespace camera

src/camera/nvidia_mjpeg_streamer.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include <nadjieb/mjpeg_streamer.hpp>
+#include <opencv2/core/mat.hpp>
+#include "NvJpegEncoder.h"
+
+namespace camera {
+
+class NvidiaMjpegStreamer {
+ public:
+  NvidiaMjpegStreamer(uint port);
+
+  void WriteFrame(const cv::Mat& mat);
+
+ private:
+  nadjieb::MJPEGStreamer streamer_;
+  NvJPEGEncoder* encoder_ = nullptr;
+};
+
+}  // namespace camera

src/camera/uvc_camera.cc

@@ -1,42 +1,56 @@
 #include "src/camera/uvc_camera.h"
-#include <opencv2/highgui/highgui_c.h>
-#include <opencv2/opencv.hpp>
 #include "absl/status/status.h"
-#include "src/utils/pch.h"
 
 namespace camera {
 
-const cv::Mat UVCCamera::backup_image_ =
-    cv::imread("/bos/constants/dont_worry_about_it.jpg");
+auto CopyPlane(const NvBuffer::NvBufferPlane& plane, int rows, int cols,
+               unsigned char* dst) -> void {
+
+  for (int y = 0; y < rows; ++y) {
+    std::memcpy(dst + (y * cols), plane.data + (y * plane.fmt.stride), cols);
+  }
+}
 
 void callback(uvc_frame_t* frame, void* ptr) {
+  // return;
   auto ptr_ = static_cast<UVCCamera*>(ptr);
   if (ptr_->mutex_.try_lock()) {
     switch (frame->frame_format) {
       case UVC_COLOR_FORMAT_MJPEG: {
-        char* data = static_cast<char*>(frame->data);
-        std::vector<uchar> buffer(data, data + frame->data_bytes);
-        ptr_->frame_buffer.frame = cv::imdecode(buffer, UVCCamera::read_type);
+        auto* data = static_cast<unsigned char*>(frame->data);
+        NvBuffer* decoded_buffer = nullptr;
+        uint32_t pixfmt = 0;
+        uint32_t width = 0;
+        uint32_t height = 0;
+
+        const int decode_status = ptr_->decoder_->decodeToBuffer(
+            &decoded_buffer, data, frame->data_bytes, &pixfmt, &width, &height);
+
+        if (decode_status < 0 || !decoded_buffer) {
+          LOG(WARNING) << "Failed to decode buffer";
+          break;
+        }
+
+        cv::Mat i420(height + (height / 2), width, CV_8UC1);
+
+        auto* y_dst = i420.ptr<unsigned char>(0);
+        auto* u_dst = i420.ptr<unsigned char>(height);
+        auto* v_dst = i420.ptr<unsigned char>(height + (height / 4));
+
+        CopyPlane(decoded_buffer->planes[0], static_cast<int>(height),
+                  static_cast<int>(width), y_dst);
+        CopyPlane(decoded_buffer->planes[1], static_cast<int>(height / 2),
+                  static_cast<int>(width / 2), u_dst);
+        CopyPlane(decoded_buffer->planes[2], static_cast<int>(height / 2),
+                  static_cast<int>(width / 2), v_dst);
+
+        cv::cvtColor(i420, ptr_->frame_buffer.frame, cv::COLOR_YUV2BGR_I420);
+        cv::imwrite("bgr.png", ptr_->frame_buffer.frame);
+        // std::abort();
         break;
       }
       case UVC_COLOR_FORMAT_YUYV: {
-        uvc_frame_t* bgr = uvc_allocate_frame(frame->width * frame->height * 3);
-        if (!bgr) {
-          LOG(WARNING) << "Camera " << ptr_->camera_constant_.name
-                       << " failed to allocate ";
-          ptr_->mutex_.unlock();
-          return;
-        }
-        uvc_error_t ret = uvc_yuyv2bgr(frame, bgr);
-        if (ret != 0) {
-          LOG(WARNING) << "YUYV failed to convert to BGR";
-        }
-        IplImage* ipl_image;
-        ipl_image = cvCreateImageHeader(cvSize(bgr->width, bgr->height),
-                                        IPL_DEPTH_8U, 3);
-        cvSetData(ipl_image, bgr->data, bgr->width * 3);
-        ptr_->frame_buffer.frame = cv::cvarrToMat(ipl_image, true);
-        uvc_free_frame(bgr);
+        LOG(FATAL) << "Unimplemented";
         break;
       }
       default:
@@ -60,7 +74,11 @@ void callback(uvc_frame_t* frame, void* ptr) {
 
 UVCCamera::UVCCamera(const CameraConstant& camera_constant,
                      absl::Status& status, std::optional<std::string> log_path)
-    : camera_constant_(camera_constant), log_path_(std::move(log_path)) {
+    : camera_constant_(camera_constant),
+      log_path_(std::move(log_path)),
+      backup_image_(cv::imread("/bos/constants/dont_worry_about_it.jpg")) {
+
+  decoder_ = NvJPEGDecoder::createJPEGDecoder("jpegdec");
 
   int res = uvc_init(&context_, nullptr);
   if (res != 0) {