Kinect support for Point Tracker (#914)

Kinect tracker now provides IR camera for Point Tracker to use.
Closes #914

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