DepthGuide/src/device/impl/uvc_camera.cpp

#include "uvc_camera_impl.h"
#include "core/utility.hpp"
#include "third_party/scope_guard.hpp"
#include "image_process_v5/sp_image.h"
#include "image_process_v5/image_process.h"

#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>

#include <libusb.h>

#include <fmt/format.h>
#include <spdlog/spdlog.h>

#include <boost/asio/post.hpp>

namespace uvc_camera_impl {

    bool check_api_call(uvc_error_t api_ret, unsigned int line_number,
                        const char *file_name, const char *api_call_str) {
        if (api_ret == UVC_SUCCESS) [[likely]] return true;
        SPDLOG_ERROR("libuvc api call {} failed at {}:{} with error 0x{:x}",
                     api_call_str, file_name, line_number, (int) api_ret);
        return false;
    }

#define API_CHECK(api_call) \
    check_api_call( \
        api_call, __LINE__, __FILE__, #api_call)

#define API_CHECK_P(api_call) \
    if (!check_api_call( \
        api_call, __LINE__, __FILE__, #api_call)) [[unlikely]] \
        return nullptr

    uvc_context_t *uvc_ctx = nullptr;

    void free_device_list(dev_info_list_type::element_type *data) {
        for (auto &item: *data) {
            uvc_unref_device((uvc_device_t *) item.h_dev);
        }
        delete data;
    }

    dev_info_list_type get_device_list() {
        if (uvc_ctx == nullptr) [[unlikely]] {
            API_CHECK_P(uvc_init(&uvc_ctx, nullptr));
            std::atexit([] { uvc_exit(uvc_ctx); });
        }
        assert(uvc_ctx != nullptr);

        uvc_device_t **dev_list = nullptr;
        auto dev_list_closer = sg::make_scope_guard([&] {
            if (dev_list == nullptr) return;
            uvc_free_device_list(dev_list, 1);
        });
        API_CHECK_P(uvc_find_devices(uvc_ctx, &dev_list, 0, 0, nullptr));

        auto dev_info_list =
                dev_info_list_type(new dev_info_list_type::element_type(),
                                   [](auto ptr) { free_device_list(ptr); });
        for (auto p_dev = dev_list; *p_dev != nullptr; ++p_dev) {
            auto dev = *p_dev;

            uvc_device_descriptor_t *dev_desc = nullptr;
            auto dev_desc_closer = sg::make_scope_guard([&] {
                if (dev_desc == nullptr) return;
                uvc_free_device_descriptor(dev_desc);
            });

            auto safe_str = [](const char *str) -> auto {
                return (str == nullptr || *str == 0) ? "??" : str;
            };

            API_CHECK_P(uvc_get_device_descriptor(dev, &dev_desc));
            assert(dev_desc != nullptr);
            std::string dev_name;
            if (dev_desc->serialNumber != nullptr) {
                dev_name = fmt::format("{}: {}, {} ({})",
                    p_dev - dev_list,
                    safe_str(dev_desc->manufacturer),
                    safe_str(dev_desc->product),
                    safe_str(dev_desc->serialNumber));
            } else {
                dev_name = fmt::format("{:x}:{:x}",
                                       dev_desc->idVendor, dev_desc->idProduct);
            }

            uvc_ref_device(dev);
            dev_info_list->emplace_back(dev, dev_name);
        }
        return dev_info_list;
    }

    sp_image mjpeg_frame_to_image(uvc_frame_t *frame) { // TODO: accelerate with CUDA
        assert(frame->frame_format == UVC_FRAME_FORMAT_MJPEG);
        auto img_data = cv::_InputArray((uchar *) frame->data, frame->data_bytes);
        auto img_bgr = cv::imdecode(img_data, cv::IMREAD_UNCHANGED);
        if (img_bgr.empty()) return {};
        const auto img_rgb = sp_image::create(CV_8UC3, img_bgr.size());
        const auto helper = write_access_helper(img_rgb.host());
        auto img_rgb_mat = img_rgb.cv_mat(helper.ptr());
        cv::cvtColor(img_bgr, img_rgb_mat, cv::COLOR_BGR2RGB);
        return img_rgb;
    }

    sp_image yuyv_frame_to_image(uvc_frame_t *frame) {
        assert(frame->frame_format == UVC_FRAME_FORMAT_YUYV);
        auto img_size = cv::Size(frame->width, frame->height);
        if (img_size.area() * sizeof(uchar2) != frame->data_bytes) return {};
        return sp_image::create<uchar2>(img_size, frame->data);
    }

}

uvc_camera::impl::~impl() {
    if (is_capturing) {
        stop();
    }
    if (dev_h != nullptr) {
        uvc_close(dev_h);
    }
    if (dev != nullptr) {
        uvc_unref_device(dev);
    }
}

std::unique_ptr<uvc_camera::impl>
uvc_camera::impl::create(create_config conf) {
    assert(uvc_ctx != nullptr);

    auto ret = std::make_unique<impl>();
    ret->conf = conf;
    ret->dev = (uvc_device_t *) conf.h_dev;
    uvc_ref_device(ret->dev);
    API_CHECK_P(uvc_open(ret->dev, &ret->dev_h));

#ifndef NDEBUG
    uvc_print_diag(ret->dev_h, stdout);
#endif

    // create stream list
    ret->stream_list =
            std::make_shared<stream_info_list_type::element_type>();
    for (auto fmt_desc = uvc_get_format_descs(ret->dev_h);
         fmt_desc != nullptr; fmt_desc = fmt_desc->next) {

        auto fmt_name = std::string((char *) fmt_desc->fourccFormat, 4);

        for (auto f_desc = fmt_desc->frame_descs;
             f_desc != nullptr; f_desc = f_desc->next) {

            auto fps = 1e7 / f_desc->dwDefaultFrameInterval;
            ret->stream_list->emplace_back(
                    fmt_name, f_desc->wWidth, f_desc->wHeight, fps, f_desc);
        }
    }

    return ret;
}

void uvc_camera::impl::frame_callback(uvc_frame *frame) {
    switch (frame->frame_format) {
        case UVC_FRAME_FORMAT_MJPEG: {
            auto img = mjpeg_frame_to_image(frame);
            OBJ_SAVE(img_name, img);
            break;
        }
        case UVC_FRAME_FORMAT_YUYV: {
            auto img_yuyv = yuyv_frame_to_image(frame);
            if (img_yuyv.empty()) break;
            boost::asio::post(*conf.ctx, [=, this] { // handle CUDA context problem
                // TODO: check why CUDA commands cannot be used in this thread
                auto img_rgb = image_yuyv_to_rgb(img_yuyv);
                OBJ_SAVE(img_name, img_rgb);
            });
            break;
        }
        default: {
            RET_ERROR;
        }
    }
}

void uvc_camera::impl::start(start_config conf) {
    assert(!is_capturing);

    // reset usb device to make it work
    auto usb_dev_h = uvc_get_libusb_handle(dev_h);
    auto usb_ret = libusb_reset_device(usb_dev_h);
    assert(usb_ret == LIBUSB_SUCCESS);

    auto f_desc = (uvc_frame_desc_t *) conf.h_desc;
    auto fmt_desc = f_desc->parent;
    uvc_frame_format fmt = UVC_FRAME_FORMAT_ANY;
    switch (fmt_desc->bDescriptorSubtype) {
        // @formatter:off
        case UVC_VS_FORMAT_MJPEG: { fmt = UVC_FRAME_FORMAT_MJPEG; break; }
        case UVC_VS_FORMAT_UNCOMPRESSED: { fmt = UVC_FRAME_FORMAT_YUYV; break; }
        // @formatter:on
        default: {
            RET_ERROR;
        }
    }

    auto fps = 1e7 / f_desc->dwDefaultFrameInterval;
    API_CHECK(uvc_get_stream_ctrl_format_size(dev_h, &stream_ctrl, fmt,
                                              f_desc->wWidth, f_desc->wHeight, fps));

#ifndef NDEBUG
    uvc_print_stream_ctrl(&stream_ctrl, stdout);
#endif

    img_name = conf.img_name;
    API_CHECK(uvc_start_streaming(dev_h, &stream_ctrl, [](auto frame, void *user) {
        ((impl *) user)->frame_callback(frame);
    }, this, 0));

    is_capturing = true;
}

void uvc_camera::impl::stop() {
    assert(is_capturing);
    uvc_stop_streaming(dev_h);
    is_capturing = false;
}

dev_info_list_type uvc_camera::get_device_list() {
    return ::get_device_list();
}

uvc_camera::pointer uvc_camera::create(create_config conf) {
    auto pimpl = impl::create(conf);
    if (pimpl == nullptr) return nullptr;
    auto ret = std::make_unique<this_type>();
    ret->pimpl = std::move(pimpl);
    return ret;
}

uvc_camera::~uvc_camera() = default;

uvc_camera::stream_info_list_type
uvc_camera::get_stream_list() {
    return pimpl->stream_list;
}

void uvc_camera::start(start_config conf) {
    pimpl->start(conf);
}

bool uvc_camera::is_capturing() const {
    return pimpl->is_capturing;
}

void uvc_camera::stop() {
    pimpl->stop();
}

uvc_camera::stream_info_type::operator std::string() {
    return fmt::format("{}x{} @ {}fps ({})", width, height, fps, fmt_name);
}