diff options
Diffstat (limited to 'tracker-kinect-face/camera_kinect_ir.cpp')
| -rw-r--r-- | tracker-kinect-face/camera_kinect_ir.cpp | 270 |
1 files changed, 270 insertions, 0 deletions
diff --git a/tracker-kinect-face/camera_kinect_ir.cpp b/tracker-kinect-face/camera_kinect_ir.cpp new file mode 100644 index 00000000..edae6289 --- /dev/null +++ b/tracker-kinect-face/camera_kinect_ir.cpp @@ -0,0 +1,270 @@ +/* Copyright (c) 2019 Stéphane Lenclud + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted, provided that the above + * copyright notice and this permission notice appear in all copies. + */ + +#include "camera_kinect_ir.h" + +#ifdef OTR_HAVE_OPENCV + +//#include "frame.hpp" + +#include "compat/sleep.hpp" +#include "compat/math-imports.hpp" + +#include <opencv2/imgproc.hpp> +#include <cstdlib> + +namespace Kinect { + + static const char KKinectIRSensor[] = "Kinect V2 IR Sensor"; + + CamerasProvider::CamerasProvider() = default; + + std::unique_ptr<video::impl::camera> CamerasProvider::make_camera(const QString& name) + { + if (name.compare(KKinectIRSensor) == 0) + { + return std::make_unique<CameraKinectIr>(); + } + + return nullptr; + } + + std::vector<QString> CamerasProvider::camera_names() const + { + std::vector<QString> cameras; + cameras.push_back(KKinectIRSensor); + return cameras; + } + + bool CamerasProvider::can_show_dialog(const QString& camera_name) + { + return false; + } + + bool CamerasProvider::show_dialog(const QString& camera_name) + { + return false; + } + +// Register our camera provider thus making sure Point Tracker can use Kinect V2 IR Sensor +OTR_REGISTER_CAMERA(CamerasProvider) + + +CameraKinectIr::CameraKinectIr() +{ +} + + +CameraKinectIr::~CameraKinectIr() +{ + stop(); +} + +bool CameraKinectIr::show_dialog() +{ + return false; +} + +bool CameraKinectIr::is_open() +{ + return iInfraredFrameReader!=nullptr; +} + +/// +/// Wait until we get a first frame +/// +void CameraKinectIr::WaitForFirstFrame() +{ + bool new_frame = false; + int attempts = 200; // Kinect cold start can take a while + while (!new_frame && attempts>0) + { + new_frame = get_frame_(iMatFrame); + portable::sleep(100); + --attempts; + } +} + + + +std::tuple<const video::impl::frame&, bool> CameraKinectIr::get_frame() +{ + bool new_frame = false; + new_frame = get_frame_(iMatFrame); + + iFrame.data = iMatFrame.ptr(); + iFrame.width = 512; + iFrame.height = 424; + iFrame.stride = 0; // Auto step + iFrame.channels = 3; + return { iFrame, new_frame }; +} + +// Safe release for interfaces +template<class Interface> +inline void SafeRelease(Interface *& pInterfaceToRelease) +{ + if (pInterfaceToRelease != NULL) + { + pInterfaceToRelease->Release(); + pInterfaceToRelease = NULL; + } +} + +bool CameraKinectIr::start(const info& args) +{ + stop(); + + HRESULT hr; + + // Get and open Kinect sensor + hr = GetDefaultKinectSensor(&iKinectSensor); + if (SUCCEEDED(hr)) + { + hr = iKinectSensor->Open(); + } + + // Create infrared frame reader + if (SUCCEEDED(hr)) + { + // Initialize the Kinect and get the infrared reader + IInfraredFrameSource* pInfraredFrameSource = NULL; + + hr = iKinectSensor->Open(); + + if (SUCCEEDED(hr)) + { + hr = iKinectSensor->get_InfraredFrameSource(&pInfraredFrameSource); + } + + if (SUCCEEDED(hr)) + { + hr = pInfraredFrameSource->OpenReader(&iInfraredFrameReader); + } + + SafeRelease(pInfraredFrameSource); + } + + + if (SUCCEEDED(hr)) + { + WaitForFirstFrame(); + bool success = iMatFrame.ptr() != nullptr; + return success; + } + + stop(); + return false; +} + +void CameraKinectIr::stop() +{ + // done with infrared frame reader + SafeRelease(iInfraredFrame); + SafeRelease(iInfraredFrameReader); + + // close the Kinect Sensor + if (iKinectSensor) + { + iKinectSensor->Close(); + } + + SafeRelease(iKinectSensor); + + // Free up our memory buffer if any + iMatFrame = {}; +} + +bool CameraKinectIr::get_frame_(cv::Mat& frame) +{ + + if (!iInfraredFrameReader) + { + return false; + } + + bool success = false; + + // Release previous frame if any + SafeRelease(iInfraredFrame); + + HRESULT hr = iInfraredFrameReader->AcquireLatestFrame(&iInfraredFrame); + + if (SUCCEEDED(hr)) + { + INT64 nTime = 0; + IFrameDescription* frameDescription = NULL; + int nWidth = 0; + int nHeight = 0; + float diagonalFieldOfView = 0.0f; + UINT nBufferSize = 0; + UINT16 *pBuffer = NULL; + + hr = iInfraredFrame->get_RelativeTime(&nTime); + + if (SUCCEEDED(hr)) + { + hr = iInfraredFrame->get_FrameDescription(&frameDescription); + } + + // TODO: should not request those info for a every frame really + if (SUCCEEDED(hr)) + { + hr = frameDescription->get_Width(&nWidth); + } + + if (SUCCEEDED(hr)) + { + hr = frameDescription->get_Height(&nHeight); + } + + if (SUCCEEDED(hr)) + { + hr = frameDescription->get_DiagonalFieldOfView(&diagonalFieldOfView); + } + + if (SUCCEEDED(hr)) + { + hr = iInfraredFrame->AccessUnderlyingBuffer(&nBufferSize, &pBuffer); + } + + if (SUCCEEDED(hr)) + { + //ProcessInfrared(nTime, pBuffer, nWidth, nHeight); + + // Create an OpenCV matrix with our 16-bits IR buffer + cv::Mat raw = cv::Mat(nHeight, nWidth, CV_16UC1, pBuffer); + + // Convert that OpenCV matrix to an RGB one as this is what is expected by our point extractor + // TODO: Ideally we should implement a point extractors that works with our native buffer + // First resample to 8-bits + double min = std::numeric_limits<uint16_t>::min(); + double max = std::numeric_limits<uint16_t>::max(); + //cv::minMaxLoc(raw, &min, &max); // Should we use 16bit min and max instead? + // For scalling to have more precission in the range we are interrested in + min = max - 255; + cv::Mat raw8; + // See: https://stackoverflow.com/questions/14539498/change-type-of-mat-object-from-cv-32f-to-cv-8u/14539652 + raw.convertTo(raw8, CV_8U, 255.0 / (max - min), -255.0*min / (max - min)); + // Second convert to RGB + cv::cvtColor(raw8, frame, cv::COLOR_GRAY2BGR); + // + success = true; + } + + SafeRelease(frameDescription); + } + + + return success; +} + + + +} + +#endif |