diff --git a/.gitmodules b/.gitmodules index 7c7092ff..a785a56b 100644 --- a/.gitmodules +++ b/.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 diff --git a/CMakeLists.txt b/CMakeLists.txt index c022cfd3..a56aad78 100644 --- a/CMakeLists.txt +++ b/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( diff --git a/bgr.png b/bgr.png new file mode 100644 index 00000000..fc07fc1d Binary files /dev/null and b/bgr.png differ diff --git a/decode_cat_with_nvjpeg.cpp b/decode_cat_with_nvjpeg.cpp new file mode 100644 index 00000000..b1ddd3b5 --- /dev/null +++ b/decode_cat_with_nvjpeg.cpp @@ -0,0 +1,126 @@ +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#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 jpeg_bytes((std::istreambuf_iterator(input_stream)), + std::istreambuf_iterator()); + + 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(0); + unsigned char* u_dst = i420.ptr(height); + unsigned char* v_dst = i420.ptr(height + (height / 4)); + + const bool y_ok = copy_plane(decoded_buffer->planes[0], static_cast(height), static_cast(width), y_dst); + const bool u_ok = copy_plane(decoded_buffer->planes[1], static_cast(height / 2), static_cast(width / 2), u_dst); + const bool v_ok = copy_plane(decoded_buffer->planes[2], static_cast(height / 2), static_cast(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; +} diff --git a/src/calibration/frame_shower.cc b/src/calibration/frame_shower.cc index d018bbd8..0b70ca61 100644 --- a/src/calibration/frame_shower.cc +++ b/src/calibration/frame_shower.cc @@ -1,4 +1,5 @@ #include +#include "NvJpegDecoder.h" #include "absl/flags/flag.h" #include "absl/flags/parse.h" #include "src/camera/camera.h" @@ -7,6 +8,7 @@ #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, camera_name, std::nullopt, ""); //NOLINT ABSL_FLAG(std::optional, port, std::nullopt, ""); //NOLINT @@ -14,6 +16,7 @@ ABSL_FLAG(std::optional, log_path, std::nullopt, ""); //NOLINT auto main(int argc, char* argv[]) -> int { absl::ParseCommandLine(argc, argv); + stop::RegisterHandler(); std::unique_ptr 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); } diff --git a/src/camera/CMakeLists.txt b/src/camera/CMakeLists.txt index ef546bdc..0ef8ed47 100644 --- a/src/camera/CMakeLists.txt +++ b/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 ) diff --git a/src/camera/nvidia_mjpeg_streamer.cc b/src/camera/nvidia_mjpeg_streamer.cc new file mode 100644 index 00000000..190fef12 --- /dev/null +++ b/src/camera/nvidia_mjpeg_streamer.cc @@ -0,0 +1,35 @@ +#include "src/camera/nvidia_mjpeg_streamer.h" +#include +#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 diff --git a/src/camera/nvidia_mjpeg_streamer.h b/src/camera/nvidia_mjpeg_streamer.h new file mode 100644 index 00000000..1af6be45 --- /dev/null +++ b/src/camera/nvidia_mjpeg_streamer.h @@ -0,0 +1,20 @@ +#pragma once + +#include +#include +#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 diff --git a/src/camera/uvc_camera.cc b/src/camera/uvc_camera.cc index 91ecc930..a44fe9b2 100644 --- a/src/camera/uvc_camera.cc +++ b/src/camera/uvc_camera.cc @@ -1,42 +1,56 @@ #include "src/camera/uvc_camera.h" -#include -#include #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(ptr); if (ptr_->mutex_.try_lock()) { switch (frame->frame_format) { case UVC_COLOR_FORMAT_MJPEG: { - char* data = static_cast(frame->data); - std::vector buffer(data, data + frame->data_bytes); - ptr_->frame_buffer.frame = cv::imdecode(buffer, UVCCamera::read_type); + auto* data = static_cast(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(0); + auto* u_dst = i420.ptr(height); + auto* v_dst = i420.ptr(height + (height / 4)); + + CopyPlane(decoded_buffer->planes[0], static_cast(height), + static_cast(width), y_dst); + CopyPlane(decoded_buffer->planes[1], static_cast(height / 2), + static_cast(width / 2), u_dst); + CopyPlane(decoded_buffer->planes[2], static_cast(height / 2), + static_cast(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 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) { diff --git a/src/camera/uvc_camera.h b/src/camera/uvc_camera.h index 34dafe2a..d99114ef 100644 --- a/src/camera/uvc_camera.h +++ b/src/camera/uvc_camera.h @@ -1,9 +1,10 @@ #pragma once #include +#include "NvJpegDecoder.h" #include "camera_constants.h" #include "libuvc/libuvc.h" #include "src/camera/camera.h" -#include "src/utils/pch.h" +// #include "src/utils/pch.h" namespace camera { @@ -18,18 +19,19 @@ class UVCCamera : public ICamera { [[nodiscard]] auto GetCameraConstant() const -> camera_constant_t override; public: - const camera_constant_t camera_constant_; + camera_constant_t camera_constant_; std::optional log_path_; - static const cv::Mat backup_image_; + cv::Mat backup_image_; uvc_context_t* context_; uvc_device_t* device_; uvc_device_handle_t* device_handle_; timestamped_frame_t frame_buffer; uvc_stream_ctrl_t ctrl_; std::mutex mutex_; - int frame_index_; - int previous_frame_index_; - static constexpr cv::ImreadModes read_type = cv::IMREAD_GRAYSCALE; + int frame_index_ = 0; + int previous_frame_index_ = 0; + NvJPEGDecoder* decoder_ = nullptr; + // static constexpr cv::ImreadModes read_type = cv::IMREAD_GRAYSCALE; private: auto StartCamera(uvc_stream_ctrl_t ctrl) -> void; diff --git a/src/localization/CMakeLists.txt b/src/localization/CMakeLists.txt index 64652f5c..5dfacabc 100644 --- a/src/localization/CMakeLists.txt +++ b/src/localization/CMakeLists.txt @@ -71,6 +71,7 @@ target_sources(UnambiguousEstimator target_link_libraries(UnambiguousEstimator PRIVATE SquareSolver MultitagSolver + UVCCamera utils ) diff --git a/src/test/integration_test/CMakeLists.txt b/src/test/integration_test/CMakeLists.txt index 951f9606..af7a5435 100644 --- a/src/test/integration_test/CMakeLists.txt +++ b/src/test/integration_test/CMakeLists.txt @@ -30,3 +30,7 @@ target_link_libraries(pva_test PRIVATE utils localization vpi) add_executable(bfs-test bfs_tester.cc) target_link_libraries(bfs-test PRIVATE pathing utils nlohmann_json::nlohmann_json wpiutil) + + +add_executable(nvjpeg_test nvjpeg_test.cc) +target_link_libraries(nvjpeg_test PRIVATE nvjpeg utils) diff --git a/src/test/integration_test/nvjpeg_test.cc b/src/test/integration_test/nvjpeg_test.cc new file mode 100644 index 00000000..07e81626 --- /dev/null +++ b/src/test/integration_test/nvjpeg_test.cc @@ -0,0 +1,112 @@ +#include +#include +#include +#include +#include +#include +#include +#include "NvJpegDecoder.h" + +#include +#include + +auto copy_plane(const NvBuffer::NvBufferPlane& plane, int rows, int cols, + unsigned char* dst) -> bool { + if (!plane.data) { + return false; + } + + for (int y = 0; y < rows; ++y) { + memcpy(dst + (y * cols), plane.data + (y * plane.fmt.stride), cols); + } + + return true; +} +auto main() -> int { + const std::string input_path = "cat.jpg"; + const std::string output_path = "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 jpeg_bytes( + (std::istreambuf_iterator(input_stream)), + std::istreambuf_iterator()); + + 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(0); + unsigned char* u_dst = i420.ptr(height); + unsigned char* v_dst = i420.ptr(height + (height / 4)); + + const bool y_ok = + copy_plane(decoded_buffer->planes[0], static_cast(height), + static_cast(width), y_dst); + const bool u_ok = + copy_plane(decoded_buffer->planes[1], static_cast(height / 2), + static_cast(width / 2), u_dst); + const bool v_ok = + copy_plane(decoded_buffer->planes[2], static_cast(height / 2), + static_cast(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; +} diff --git a/third_party/cpp-mjpeg-streamer b/third_party/cpp-mjpeg-streamer new file mode 160000 index 00000000..43692a0f --- /dev/null +++ b/third_party/cpp-mjpeg-streamer @@ -0,0 +1 @@ +Subproject commit 43692a0f917bea8abe1fb2b3104137ab7a45f895 diff --git a/third_party/nvjpeg/CMakeLists.txt b/third_party/nvjpeg/CMakeLists.txt new file mode 100644 index 00000000..03b619b0 --- /dev/null +++ b/third_party/nvjpeg/CMakeLists.txt @@ -0,0 +1,22 @@ +find_library(NVJPEG_LIB + NAMES nvjpeg + PATHS + /usr/lib/aarch64-linux-gnu/nvidia +) +add_library(nvjpeg + NvJpegDecoder.cpp + NvBuffer.cpp + NvElement.cpp + NvElementProfiler.cpp + NvLogging.cpp +) + +target_include_directories(nvjpeg PUBLIC + /usr/src/jetson_multimedia_api/include + /usr/src/jetson_multimedia_api/include/libjpeg-8b +) + +target_link_libraries(nvjpeg PUBLIC + ${NVJPEG_LIB} + ${NVBUFSURFACE_LIB} +) diff --git a/third_party/nvjpeg/NvBuffer.cpp b/third_party/nvjpeg/NvBuffer.cpp new file mode 100644 index 00000000..0b76e493 --- /dev/null +++ b/third_party/nvjpeg/NvBuffer.cpp @@ -0,0 +1,456 @@ +/* + * Copyright (c) 2016-2021, NVIDIA CORPORATION. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of NVIDIA CORPORATION nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY + * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include "NvBuffer.h" +#include "NvLogging.h" + +#include +#include +#include +#include + +#define CAT_NAME "Buffer" + +#define MAX(a,b) (a > b ? a : b) + +NvBuffer::NvBuffer(enum v4l2_buf_type buf_type, enum v4l2_memory memory_type, + uint32_t n_planes, NvBufferPlaneFormat * fmt, uint32_t index) + :buf_type(buf_type), + memory_type(memory_type), + index(index), + n_planes(n_planes) +{ + uint32_t i; + + mapped = false; + allocated = false; + + memset(planes, 0, sizeof(planes)); + for (i = 0; i < n_planes; i++) + { + this->planes[i].fd = -1; + this->planes[i].fmt = fmt[i]; + } + + ref_count = 0; + pthread_mutex_init(&ref_lock, NULL); + shared_buffer = NULL; +} + +NvBuffer::NvBuffer(uint32_t pixfmt, uint32_t width, uint32_t height, + uint32_t index) + :buf_type(V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE), + memory_type(V4L2_MEMORY_USERPTR), + index(index) +{ + uint32_t i; + NvBuffer::NvBufferPlaneFormat fmt[MAX_PLANES]; + + mapped = false; + allocated = false; + + fill_buffer_plane_format(&n_planes, fmt, width, height, pixfmt); + + for (i = 0; i < MAX_PLANES; i++) + { + this->planes[i].fd = -1; + this->planes[i].data = NULL; + this->planes[i].bytesused = 0; + this->planes[i].mem_offset = 0; + this->planes[i].length = 0; + this->planes[i].fmt = fmt[i]; + this->planes[i].fmt.sizeimage = fmt[i].width * fmt[i].height * + fmt[i].bytesperpixel; + this->planes[i].fmt.stride = fmt[i].width * fmt[i].bytesperpixel; + } + + ref_count = 0; + pthread_mutex_init(&ref_lock, NULL); + shared_buffer = NULL; +} + +NvBuffer::NvBuffer(uint32_t size, uint32_t index) + :buf_type(V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE), + memory_type(V4L2_MEMORY_USERPTR), + index(index) +{ + uint32_t i; + + mapped = false; + allocated = false; + + n_planes = 1; + for (i = 0; i < n_planes; i++) + { + this->planes[i].fd = -1; + this->planes[i].data = NULL; + this->planes[i].bytesused = 0; + this->planes[i].mem_offset = 0; + this->planes[i].length = 0; + this->planes[i].fmt.sizeimage = size; + } + + ref_count = 0; + pthread_mutex_init(&ref_lock, NULL); + shared_buffer = NULL; +} + +NvBuffer::~NvBuffer() +{ + if (mapped) + { + unmap(); + } + if (allocated) + { + deallocateMemory(); + } + + pthread_mutex_destroy(&ref_lock); +} + +int +NvBuffer::map() +{ + uint32_t j; + + if (memory_type != V4L2_MEMORY_MMAP) + { + CAT_WARN_MSG("Buffer " << index << "already mapped"); + return -1; + } + + if (mapped) + { + CAT_WARN_MSG("Buffer " << index << "already mapped"); + return 0; + } + + for (j = 0; j < n_planes; j++) + { + if (planes[j].fd == -1) + { + return -1; + } + + planes[j].data = (unsigned char *) mmap(NULL, + planes[j].length, + PROT_READ | PROT_WRITE, + MAP_SHARED, + planes[j].fd, + planes[j].mem_offset); + if (planes[j].data == MAP_FAILED) + { + CAT_ERROR_MSG("Could not map buffer " << index << ", plane " << j); + return -1; + } + else + { + CAT_DEBUG_MSG("Mapped buffer " << index << ", plane " << j << " to " + << planes[j].data); + } + } + mapped = true; + return 0; +} + +void +NvBuffer::unmap() +{ + if (memory_type != V4L2_MEMORY_MMAP || !mapped) + { + CAT_WARN_MSG("Cannot Unmap Buffer " << index << + ". Only mapped MMAP buffer can be unmapped"); + return; + } + + for (uint32_t j = 0; j < n_planes; j++) + { + if (planes[j].data) + { + munmap(planes[j].data, planes[j].length); + } + planes[j].data = NULL; + } + mapped = false; + CAT_DEBUG_MSG("Buffer " << index << " unmapped "); +} + +int +NvBuffer::allocateMemory() +{ + uint32_t j; + + if (memory_type != V4L2_MEMORY_USERPTR) + { + CAT_ERROR_MSG("Only USERPTR buffers can be allocated"); + return -1; + } + + if (allocated) + { + CAT_WARN_MSG("Buffer " << index << "already allocated memory"); + return 0; + } + + for (j = 0; j < n_planes; j++) + { + if (planes[j].data) + { + ERROR_MSG("Buffer " << index << ", Plane " << j << + " already allocated"); + return -1; + } + + planes[j].length = MAX(planes[j].fmt.sizeimage, + planes[j].fmt.width * + planes[j].fmt.bytesperpixel * + planes[j].fmt.height); + planes[j].data = new unsigned char [planes[j].length]; + + if (planes[j].data == MAP_FAILED) + { + SYS_ERROR_MSG("Error while allocating buffer " << index << + " plane " << j); + return -1; + } + else + { + DEBUG_MSG("Buffer " << index << ", Plane " << j << + " allocated to " << (void *) planes[j].data); + } + } + allocated = true; + return 0; +} + +void +NvBuffer::deallocateMemory() +{ + uint32_t j; + + if (memory_type != V4L2_MEMORY_USERPTR || !allocated) + { + ERROR_MSG("Only allocated USERPTR buffers can be deallocated"); + return; + } + + for (j = 0; j < n_planes; j++) + { + if (!planes[j].data) + { + DEBUG_MSG("Buffer " << index << ", Plane " << j << + " not allocated"); + continue; + } + delete[] planes[j].data; + planes[j].data = NULL; + } + allocated = false; + DEBUG_MSG("Buffer " << index << " deallocated"); +} + +int +NvBuffer::ref() +{ + int ref_count; + + pthread_mutex_lock(&ref_lock); + ref_count = ++this->ref_count; + pthread_mutex_unlock(&ref_lock); + return ref_count; +} + +int +NvBuffer::unref() +{ + int ref_count; + + pthread_mutex_lock(&ref_lock); + if (this->ref_count > 0) + { + --this->ref_count; + } + ref_count = this->ref_count; + pthread_mutex_unlock(&ref_lock); + return ref_count; +} + +int +NvBuffer::fill_buffer_plane_format(uint32_t *num_planes, + NvBuffer::NvBufferPlaneFormat *planefmts, + uint32_t width, uint32_t height, uint32_t raw_pixfmt) +{ + switch (raw_pixfmt) + { + case V4L2_PIX_FMT_YUV444M: + *num_planes = 3; + + planefmts[0].width = width; + planefmts[1].width = width; + planefmts[2].width = width; + + planefmts[0].height = height; + planefmts[1].height = height; + planefmts[2].height = height; + + planefmts[0].bytesperpixel = 1; + planefmts[1].bytesperpixel = 1; + planefmts[2].bytesperpixel = 1; + break; + case V4L2_PIX_FMT_YUV422M: + *num_planes = 3; + + planefmts[0].width = width; + planefmts[1].width = width / 2; + planefmts[2].width = width / 2; + + planefmts[0].height = height; + planefmts[1].height = height; + planefmts[2].height = height; + + planefmts[0].bytesperpixel = 1; + planefmts[1].bytesperpixel = 1; + planefmts[2].bytesperpixel = 1; + break; + case V4L2_PIX_FMT_YUV422RM: + *num_planes = 3; + + planefmts[0].width = width; + planefmts[1].width = width; + planefmts[2].width = width; + + planefmts[0].height = height; + planefmts[1].height = height / 2; + planefmts[2].height = height / 2; + + planefmts[0].bytesperpixel = 1; + planefmts[1].bytesperpixel = 1; + planefmts[2].bytesperpixel = 1; + break; + case V4L2_PIX_FMT_YUV420M: + case V4L2_PIX_FMT_YVU420M: + *num_planes = 3; + + planefmts[0].width = width; + planefmts[1].width = width / 2; + planefmts[2].width = width / 2; + + planefmts[0].height = height; + planefmts[1].height = height / 2; + planefmts[2].height = height / 2; + + planefmts[0].bytesperpixel = 1; + planefmts[1].bytesperpixel = 1; + planefmts[2].bytesperpixel = 1; + break; + case V4L2_PIX_FMT_NV12M: + *num_planes = 2; + + planefmts[0].width = width; + planefmts[1].width = width / 2; + + planefmts[0].height = height; + planefmts[1].height = height / 2; + + planefmts[0].bytesperpixel = 1; + planefmts[1].bytesperpixel = 2; + break; + case V4L2_PIX_FMT_GREY: + *num_planes = 1; + + planefmts[0].width = width; + + planefmts[0].height = height; + + planefmts[0].bytesperpixel = 1; + break; + case V4L2_PIX_FMT_YUYV: + case V4L2_PIX_FMT_YVYU: + case V4L2_PIX_FMT_UYVY: + case V4L2_PIX_FMT_VYUY: + *num_planes = 1; + + planefmts[0].width = width; + + planefmts[0].height = height; + + planefmts[0].bytesperpixel = 2; + break; + case V4L2_PIX_FMT_ABGR32: + case V4L2_PIX_FMT_XRGB32: + *num_planes = 1; + + planefmts[0].width = width; + + planefmts[0].height = height; + + planefmts[0].bytesperpixel = 4; + break; + case V4L2_PIX_FMT_P010M: + *num_planes = 2; + + planefmts[0].width = width; + planefmts[1].width = width / 2; + + planefmts[0].height = height; + planefmts[1].height = height / 2; + + planefmts[0].bytesperpixel = 2; + planefmts[1].bytesperpixel = 4; + break; + case V4L2_PIX_FMT_NV24M: + *num_planes = 2; + + planefmts[0].width = width; + planefmts[1].width = width; + + planefmts[0].height = height; + planefmts[1].height = height; + + planefmts[0].bytesperpixel = 1; + planefmts[1].bytesperpixel = 2; + break; + case V4L2_PIX_FMT_NV24_10LE: + *num_planes = 2; + + planefmts[0].width = width; + planefmts[1].width = width; + + planefmts[0].height = height; + planefmts[1].height = height; + + planefmts[0].bytesperpixel = 2; + planefmts[1].bytesperpixel = 4; + break; + default: + ERROR_MSG("Unsupported pixel format " << raw_pixfmt); + return -1; + } + return 0; +} diff --git a/third_party/nvjpeg/NvElement.cpp b/third_party/nvjpeg/NvElement.cpp new file mode 100644 index 00000000..7d32974d --- /dev/null +++ b/third_party/nvjpeg/NvElement.cpp @@ -0,0 +1,60 @@ +/* + * Copyright (c) 2016, NVIDIA CORPORATION. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of NVIDIA CORPORATION nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY + * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include "NvElement.h" + +void NvElement::getProfilingData(NvElementProfiler::NvElementProfilerData &data) +{ + profiler.getProfilerData(data); +} + +void NvElement::printProfilingStats(std::ostream &out_stream) +{ + out_stream << "----------- Element = " << comp_name << " -----------" << std::endl; + profiler.printProfilerData(out_stream); + out_stream << "-------------------------------------" << std::endl; +} + +void NvElement::enableProfiling() +{ + profiler.enableProfiling(true); +} + +bool NvElement::isProfilingEnabled() +{ + return profiler.enabled; +} + +NvElement::NvElement(const char *name, NvElementProfiler::ProfilerField fields) + :profiler(fields) +{ + is_in_error = 0; + if (!name) + is_in_error = 1; + this->comp_name = name; +} diff --git a/third_party/nvjpeg/NvElementProfiler.cpp b/third_party/nvjpeg/NvElementProfiler.cpp new file mode 100644 index 00000000..e5855f57 --- /dev/null +++ b/third_party/nvjpeg/NvElementProfiler.cpp @@ -0,0 +1,298 @@ +/* + * Copyright (c) 2016, NVIDIA CORPORATION. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of NVIDIA CORPORATION nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY + * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include "NvElementProfiler.h" + +#define LOCK() pthread_mutex_lock(&profiler_lock) +#define UNLOCK() pthread_mutex_unlock(&profiler_lock) + +#define RETURN_IF_DISABLED() \ + if (!enabled) { \ + UNLOCK(); \ + return; \ + } + +#define GET_TIME(timeval) gettimeofday(timeval, NULL); + +#define TIMESPEC_DIFF_USEC(timespec1, timespec2) \ + (((timespec1)->tv_sec - (timespec2)->tv_sec) * 1000000L + \ + (timespec1)->tv_usec - (timespec2)->tv_usec) + +using namespace std; + +NvElementProfiler::NvElementProfiler(ProfilerField fields) + :valid_fields(fields) +{ + enabled = false; + unit_id_counter = 0; + + reset(); + + pthread_mutex_init(&profiler_lock, NULL); +} + +NvElementProfiler::~NvElementProfiler() +{ + LOCK(); + reset(); + UNLOCK(); + pthread_mutex_destroy(&profiler_lock); +} +void +NvElementProfiler::enableProfiling(bool reset_data) +{ + LOCK(); + if (enabled) + { + UNLOCK(); + return; + } + + if(reset_data) + { + reset(); + } + + enabled = true; + UNLOCK(); +} + +void +NvElementProfiler::disableProfiling() +{ + LOCK(); + RETURN_IF_DISABLED(); + + data_int.accumulated_time.tv_sec += + (data_int.stop_time.tv_sec - data_int.start_time.tv_sec); + data_int.accumulated_time.tv_usec += + (data_int.stop_time.tv_usec - data_int.start_time.tv_usec); + data_int.start_time.tv_sec = 0; + data_int.start_time.tv_usec = 0; + data_int.stop_time.tv_sec = 0; + data_int.stop_time.tv_usec = 0; + enabled = false; + UNLOCK(); +} + +void NvElementProfiler::getProfilerData(NvElementProfiler::NvElementProfilerData &data) +{ + uint64_t total_time; + + LOCK(); + + total_time = data_int.accumulated_time.tv_sec * 1000000L + + data_int.accumulated_time.tv_usec + + TIMESPEC_DIFF_USEC(&data_int.stop_time, &data_int.start_time); + + if (data_int.total_processed_units == 0 || total_time == 0) + { + data.average_fps = 0; + } + else + { + data.average_fps = ((float) (data_int.total_processed_units - 1)) * + 1000000 / total_time; + } + + if (data_int.total_processed_units == 0) + { + data.max_latency_usec = 0; + data.min_latency_usec = 0; + data.average_latency_usec = 0; + } + else + { + data.max_latency_usec = data_int.max_latency_usec; + data.min_latency_usec = data_int.min_latency_usec; + data.average_latency_usec = + data_int.total_latency / data_int.total_processed_units; + } + + data.profiling_time.tv_sec = + data_int.accumulated_time.tv_sec + data_int.stop_time.tv_sec - + data_int.start_time.tv_sec; + + data.profiling_time.tv_usec = + data_int.accumulated_time.tv_usec + data_int.stop_time.tv_usec - + data_int.start_time.tv_usec; + + if (data.profiling_time.tv_usec < 0) + { + data.profiling_time.tv_usec += 1000000; + data.profiling_time.tv_sec--; + } + + if (data.profiling_time.tv_usec > 1000000) + { + data.profiling_time.tv_usec -= 1000000; + data.profiling_time.tv_sec++; + } + + data.total_processed_units = data_int.total_processed_units; + data.num_late_units = data_int.num_late_units; + data.valid_fields = valid_fields; + UNLOCK(); +} + +void NvElementProfiler::printProfilerData(ostream &out_stream) +{ + NvElementProfilerData data; + getProfilerData(data); + + if (data.valid_fields & PROFILER_FIELD_FPS) + { + out_stream << "Total Profiling time = " << + (data.profiling_time.tv_sec + + (data.profiling_time.tv_usec / 1000000.0)) << endl; + out_stream << "Average FPS = " << data.average_fps << endl; + } + if (data.valid_fields & PROFILER_FIELD_TOTAL_UNITS) + { + out_stream << "Total units processed = " << + data.total_processed_units << endl; + } + if (data.valid_fields & PROFILER_FIELD_LATE_UNITS) + { + out_stream << "Num. of late units = " << + data.num_late_units << endl; + } + if (data.valid_fields & PROFILER_FIELD_LATENCIES) + { + out_stream << "Average latency(usec) = " << + data.average_latency_usec << endl; + out_stream << "Minimum latency(usec) = " << + data.min_latency_usec << endl; + out_stream << "Maximum latency(usec) = " << + data.max_latency_usec << endl; + } +} + +void +NvElementProfiler::reset() +{ + memset(&data_int, 0, sizeof(data_int)); + data_int.min_latency_usec = (uint64_t) -1; + + unit_start_time_queue.clear(); +} + +uint64_t +NvElementProfiler::startProcessing() +{ + struct timeval time; + uint64_t ret = 0; + LOCK(); + if (enabled) + { + std::map::iterator it = + unit_start_time_queue.end(); + + unit_id_counter++; + GET_TIME(&time); + + unit_start_time_queue.insert(it, + std::pair(unit_id_counter, time)); + + ret = unit_id_counter; + } + UNLOCK(); + return ret; +} + +void +NvElementProfiler::finishProcessing(uint64_t id, bool is_late) +{ + struct timeval unit_start_time; + struct timeval stop_time; + uint64_t latency; + + LOCK(); + RETURN_IF_DISABLED(); + + if ((valid_fields & PROFILER_FIELD_LATENCIES) && + unit_start_time_queue.empty()) + { + UNLOCK(); + return; + } + + GET_TIME(&stop_time); + + if (valid_fields & PROFILER_FIELD_LATENCIES) + { + std::map::iterator it; + if (id) + { + it = unit_start_time_queue.find(id); + } + else + { + it = unit_start_time_queue.begin(); + } + + if (it == unit_start_time_queue.end()) + { + UNLOCK(); + return; + } + unit_start_time = it->second; + unit_start_time_queue.erase(it); + + latency = TIMESPEC_DIFF_USEC(&stop_time, &unit_start_time); + data_int.total_latency += latency; + + if (latency < data_int.min_latency_usec) + { + data_int.min_latency_usec = latency; + } + if(latency > data_int.max_latency_usec) + { + data_int.max_latency_usec = latency; + } + } + + data_int.stop_time = stop_time; + + if (!data_int.start_time.tv_sec && !data_int.start_time.tv_usec) + { + data_int.start_time = data_int.stop_time; + } + + if (is_late) + { + data_int.num_late_units++; + } + data_int.total_processed_units++; + + UNLOCK(); +} diff --git a/third_party/nvjpeg/NvJpegDecoder.cpp b/third_party/nvjpeg/NvJpegDecoder.cpp new file mode 100644 index 00000000..971bba81 --- /dev/null +++ b/third_party/nvjpeg/NvJpegDecoder.cpp @@ -0,0 +1,420 @@ +/* + * Copyright (c) 2016-2024, NVIDIA CORPORATION. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of NVIDIA CORPORATION nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY + * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include "NvJpegDecoder.h" +#include "NvLogging.h" +#include +#include +#include "unistd.h" +#include "stdlib.h" +#include "nvbufsurface.h" +#include "jpegint.h" + +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#define ROUND_UP_4(num) (((num) + 3) & ~3) + +#define CAT_NAME "JpegDecoder" + +NvJPEGDecoder::NvJPEGDecoder(const char *comp_name) + :NvElement(comp_name, valid_fields) +{ + memset(&cinfo, 0, sizeof(cinfo)); + memset(&jerr, 0, sizeof(jerr)); + cinfo.err = jpeg_std_error(&jerr); + + jpeg_create_decompress(&cinfo); + + // Enable MJPEG decode + cinfo.mjpeg_decode = TRUE; +} + +NvJPEGDecoder * +NvJPEGDecoder::createJPEGDecoder(const char *comp_name) +{ + NvJPEGDecoder *jpegdec = new NvJPEGDecoder(comp_name); + if (jpegdec->isInError()) + { + delete jpegdec; + return NULL; + } + return jpegdec; +} + +NvJPEGDecoder::~NvJPEGDecoder() +{ + jpeg_destroy_decompress(&cinfo); + CAT_DEBUG_MSG(comp_name << " (" << this << ") destroyed"); +} + +void +NvJPEGDecoder::disableMjpegDecode() +{ + cinfo.mjpeg_decode = FALSE; +} + +int +NvJPEGDecoder::decodeToFd(int &fd, unsigned char * in_buf, + unsigned long in_buf_size, uint32_t &pixfmt, uint32_t &width, + uint32_t &height) +{ + uint32_t pixel_format = 0; + uint32_t buffer_id; + NvBufSurface surface; + + if (in_buf == NULL || in_buf_size == 0) + { + COMP_ERROR_MSG("Not decoding because input buffer = NULL or size = 0"); + return -1; + } + + buffer_id = profiler.startProcessing(); + + cinfo.out_color_space = JCS_YCbCr; + + jpeg_mem_src(&cinfo, in_buf, in_buf_size); + + cinfo.out_color_space = JCS_YCbCr; + + /* Read file header, set default decompression parameters */ + (void) jpeg_read_header(&cinfo, TRUE); + + cinfo.out_color_space = JCS_YCbCr; + cinfo.IsVendorbuf = TRUE; + cinfo.pVendor_buf = (unsigned char*)&surface; + + if (cinfo.comp_info[0].h_samp_factor == 2) + { + if (cinfo.comp_info[0].v_samp_factor == 2) + { + pixel_format = V4L2_PIX_FMT_YUV420M; + } + else + { + pixel_format = V4L2_PIX_FMT_YUV422M; + } + } + else + { + if (cinfo.comp_info[0].v_samp_factor == 1) + { + pixel_format = V4L2_PIX_FMT_YUV444M; + } + else + { + pixel_format = V4L2_PIX_FMT_YUV422RM; + } + } + + jpeg_start_decompress (&cinfo); + + if (cinfo.global_state != DSTATE_READY) { + COMP_ERROR_MSG("JPEG format is not supported by libnvjpeg"); + return -1; + } + + jpeg_read_raw_data (&cinfo, NULL, cinfo.comp_info[0].v_samp_factor * DCTSIZE); + jpeg_finish_decompress(&cinfo); + + // Image width and height should be even + width = (cinfo.image_width % 2 == 1) ? cinfo.image_width + 1 : cinfo.image_width; + height = (cinfo.image_height % 2 == 1) ? cinfo.image_height + 1 : cinfo.image_height; + pixfmt = pixel_format; + fd = cinfo.fd; + + COMP_DEBUG_MSG("Succesfully decoded Buffer fd=" << fd); + + profiler.finishProcessing(buffer_id, false); + return 0; +} + +int +NvJPEGDecoder::decodeToBuffer(NvBuffer ** buffer, unsigned char * in_buf, + unsigned long in_buf_size, uint32_t *pixfmt, uint32_t * width, + uint32_t * height) +{ + NvBuffer *out_buf = NULL; + uint32_t pixel_format = 0; + uint32_t buffer_id; + + if (buffer == NULL) + { + COMP_ERROR_MSG("Not decoding because buffer = NULL"); + return -1; + } + + if (in_buf == NULL || in_buf_size == 0) + { + COMP_ERROR_MSG("Not decoding because input buffer = NULL or size = 0"); + return -1; + } + + buffer_id = profiler.startProcessing(); + + cinfo.out_color_space = JCS_YCbCr; + jpeg_mem_src(&cinfo, in_buf, in_buf_size); + cinfo.out_color_space = JCS_YCbCr; + + (void) jpeg_read_header(&cinfo, TRUE); + + cinfo.out_color_space = JCS_YCbCr; + + if (cinfo.comp_info[0].h_samp_factor == 2) + { + if (cinfo.comp_info[0].v_samp_factor == 2) + { + pixel_format = V4L2_PIX_FMT_YUV420M; + } + else + { + pixel_format = V4L2_PIX_FMT_YUV422M; + } + } + else + { + if (cinfo.comp_info[0].v_samp_factor == 1) + { + pixel_format = V4L2_PIX_FMT_YUV444M; + } + else + { + pixel_format = V4L2_PIX_FMT_YUV422RM; + } + } + + out_buf = new NvBuffer(pixel_format, cinfo.image_width, + cinfo.image_height, 0); + out_buf->allocateMemory(); + + cinfo.do_fancy_upsampling = FALSE; + cinfo.do_block_smoothing = FALSE; + cinfo.out_color_space = cinfo.jpeg_color_space; + cinfo.dct_method = JDCT_FASTEST; + cinfo.bMeasure_ImageProcessTime = FALSE; + cinfo.raw_data_out = TRUE; + jpeg_start_decompress (&cinfo); + + /* For some widths jpeglib requires more horizontal padding than I420 + * provides. In those cases we need to decode into separate buffers and then + * copy over the data into our final picture buffer, otherwise jpeglib might + * write over the end of a line into the beginning of the next line, + * resulting in blocky artifacts on the left side of the picture. */ + if ((cinfo.output_width % (cinfo.max_h_samp_factor * DCTSIZE)) != 0 + || cinfo.comp_info[0].h_samp_factor != 2 + || cinfo.comp_info[1].h_samp_factor != 1 + || cinfo.comp_info[2].h_samp_factor != 1 + || cinfo.comp_info[0].v_samp_factor != 2 + || cinfo.comp_info[1].v_samp_factor != 1 + || cinfo.comp_info[2].v_samp_factor != 1) + { + COMP_DEBUG_MSG("indirect decoding using extra buffer copy"); + decodeIndirect(out_buf, pixel_format); + } + else + { + decodeDirect(out_buf, pixel_format); + } + + jpeg_finish_decompress(&cinfo); + if (width) + { + *width= cinfo.image_width; + } + if (height) + { + *height= cinfo.image_height; + } + if (pixfmt) + { + *pixfmt = pixel_format; + } + *buffer = out_buf; + + COMP_DEBUG_MSG("Succesfully decoded Buffer " << buffer); + + profiler.finishProcessing(buffer_id, false); + + return 0; +} + +void +NvJPEGDecoder::decodeIndirect(NvBuffer *out_buf, uint32_t pixel_format) +{ + unsigned char *y_rows[16] = { NULL, }; + unsigned char *u_rows[16] = { NULL, }; + unsigned char *v_rows[16] = { NULL, }; + unsigned char **scanarray[3] = { y_rows, u_rows, v_rows }; + int i, j, k; + int lines; + unsigned char *base[3] = { NULL, }; + unsigned char *last[3] = { NULL, }; + int stride[3]; + int width, height; + int r_v, r_h, width_32, read_rows; + + r_v = cinfo.comp_info[0].v_samp_factor; + r_h = cinfo.comp_info[0].h_samp_factor; + width = cinfo.image_width; + height = cinfo.image_height; + read_rows = r_v * DCTSIZE; + + for (i = 0; i < 3; i++) + { + stride[i] = out_buf->planes[i].fmt.stride; + base[i] = out_buf->planes[i].data; + last[i] = base[i] + (stride[i] * (out_buf->planes[i].fmt.height - 1)); + } + width_32 = (width + 31) & 0xFFFFFFE0; + for (i = 0; i < read_rows; i++) { + y_rows[i] = new unsigned char [width_32]; + u_rows[i] = new unsigned char [width_32]; + v_rows[i] = new unsigned char [width_32]; + } + for (i = 0; i < height; i += read_rows) + { + lines = jpeg_read_raw_data (&cinfo, scanarray, read_rows); + if (lines > 0) + { + for (j = 0, k = 0; j < read_rows; j += r_v, k++) + { + if (base[0] <= last[0]) + { + memcpy ((void*)base[0], (void*)y_rows[j], + stride[0]*sizeof(unsigned char)); + base[0] += stride[0]; + } + if (r_v == 2) + { + if (base[0] <= last[0]) + { + memcpy ((void*)base[0], (void*)y_rows[j + 1], + stride[0]*sizeof(unsigned char)); + base[0] += stride[0]; + } + } + if (base[1] <= last[1] && base[2] <= last[2]) + { + if (r_h == 2 + || pixel_format == V4L2_PIX_FMT_YUV444M + || pixel_format == V4L2_PIX_FMT_YUV422RM) + { + memcpy ((void*)base[1], (void*)u_rows[k], + stride[1]*sizeof(unsigned char)); + memcpy ((void*)base[2], (void*)v_rows[k], + stride[2]*sizeof(unsigned char)); + } + } + if (r_v == 2 || (k & 1) != 0 || + pixel_format == V4L2_PIX_FMT_YUV444M) + { + base[1] += stride[1]; + base[2] += stride[2]; + } + } + } + else + { + COMP_ERROR_MSG("jpeg_read_raw_data() returned 0"); + } + } + for (i = 0; i < read_rows; i++) + { + delete[] y_rows[i]; + delete[] u_rows[i]; + delete[] v_rows[i]; + } +} + +void +NvJPEGDecoder::decodeDirect(NvBuffer *out_buf, uint32_t pixel_format) +{ + unsigned char **line[3]; + unsigned char *y[4 * DCTSIZE] = { NULL, }; + unsigned char *u[4 * DCTSIZE] = { NULL, }; + unsigned char *v[4 * DCTSIZE] = { NULL, }; + int i, j; + int lines, v_samp[3]; + unsigned char *base[3], *last[3]; + int stride[3]; + + line[0] = y; + line[1] = u; + line[2] = v; + + for (i = 0; i < 3; i++) + { + v_samp[i] = cinfo.comp_info[i].v_samp_factor; + stride[i] = out_buf->planes[i].fmt.width; + base[i] = out_buf->planes[i].data; + last[i] = base[i] + (stride[i] * (out_buf->planes[i].fmt.height - 1)); + } + + for (i = 0; i < (int) cinfo.image_height; i += v_samp[0] * DCTSIZE) + { + for (j = 0; j < (v_samp[0] * DCTSIZE); ++j) + { + /* Y */ + line[0][j] = base[0] + (i + j) * stride[0]; + + /* U,V */ + if (pixel_format == V4L2_PIX_FMT_YUV420M) + { + /* Y */ + line[0][j] = base[0] + (i + j) * stride[0]; + if ((line[0][j] > last[0])) + line[0][j] = last[0]; + /* U */ + if (v_samp[1] == v_samp[0]) { + line[1][j] = base[1] + ((i + j) / 2) * stride[1]; + } else if (j < (v_samp[1] * DCTSIZE)) { + line[1][j] = base[1] + ((i / 2) + j) * stride[1]; + } + if ((line[1][j] > last[1])) + line[1][j] = last[1]; + /* V */ + if (v_samp[2] == v_samp[0]) { + line[2][j] = base[2] + ((i + j) / 2) * stride[2]; + } else if (j < (v_samp[2] * DCTSIZE)) { + line[2][j] = base[2] + ((i / 2) + j) * stride[2]; + } + if ((line[2][j] > last[2])) + line[2][j] = last[2]; + } + else + { + line[1][j] = base[1] + (i + j) * stride[1]; + line[2][j] = base[2] + (i + j) * stride[2]; + } + } + + lines = jpeg_read_raw_data (&cinfo, line, v_samp[0] * DCTSIZE); + if ((!lines)) + { + COMP_DEBUG_MSG( "jpeg_read_raw_data() returned 0\n"); + } + } +} diff --git a/third_party/nvjpeg/NvLogging.cpp b/third_party/nvjpeg/NvLogging.cpp new file mode 100644 index 00000000..a8069ac0 --- /dev/null +++ b/third_party/nvjpeg/NvLogging.cpp @@ -0,0 +1,33 @@ +/* + * Copyright (c) 2016, NVIDIA CORPORATION. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of NVIDIA CORPORATION nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ``AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR + * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY + * OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include "NvLogging.h" + +int log_level = DEFAULT_LOG_LEVEL; + +const char *log_level_name[] = {"INFO", "ERROR", "WARN", "DEBUG"}; \ No newline at end of file