Kinect support for Point Tracker (#914)

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

3 files changed, 355 insertions, 0 deletions

diff --git a/tracker-kinect-face/CMakeLists.txt b/tracker-kinect-face/CMakeLists.txt
index 8eb064e7..7d8f828a 100644
--- a/tracker-kinect-face/CMakeLists.txt
+++ b/tracker-kinect-face/CMakeLists.txt
@@ -40,5 +40,16 @@ if (WIN32 AND opentrack-intel)
 		  DESTINATION "${opentrack-hier-pfx}"
 		  PERMISSIONS ${opentrack-perms-exec}
 		)
+
+        # Optional OpenCV support
+        # Needed for Point Tracker to support Kinect V2 IR Sensor
+        find_package(OpenCV QUIET)
+        if(OpenCV_FOUND)
+            add_definitions(-DOTR_HAVE_OPENCV)
+            target_include_directories(${self} SYSTEM PUBLIC ${OpenCV_INCLUDE_DIRS})
+            target_link_libraries(${self} opencv_imgproc opentrack-cv opencv_core opentrack-video)
+        endif()
+
+
 	endif()
 endif()
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
diff --git a/tracker-kinect-face/camera_kinect_ir.h b/tracker-kinect-face/camera_kinect_ir.h
new file mode 100644
index 00000000..f38c342a
--- /dev/null
+++ b/tracker-kinect-face/camera_kinect_ir.h
@@ -0,0 +1,74 @@
+/* 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.
+ */
+
+#pragma once
+
+#ifdef OTR_HAVE_OPENCV
+
+#include <Kinect.h>
+
+//#include "pt-api.hpp"
+#include "compat/timer.hpp"
+#include "video/camera.hpp"
+
+
+#include <memory>
+
+#include <opencv2/core.hpp>
+#include <opencv2/videoio.hpp>
+
+#include <QString>
+
+namespace Kinect {
+
+    struct CamerasProvider : video::impl::camera_
+    {
+        CamerasProvider();
+        std::vector<QString> camera_names() const override;
+        std::unique_ptr<video::impl::camera> make_camera(const QString& name) override;
+        bool can_show_dialog(const QString& camera_name) override;
+        bool show_dialog(const QString& camera_name) override;
+    };
+
+
+///
+/// Implement our camera interface using Kinect V2 SDK IR Sensor.
+///
+struct CameraKinectIr final : video::impl::camera
+{
+    CameraKinectIr();
+    virtual ~CameraKinectIr();
+
+
+    [[nodiscard]] bool start(const info& args) override;
+    void stop() override;
+    bool is_open() override;
+    std::tuple<const video::impl::frame&, bool> get_frame() override;
+    [[nodiscard]] bool show_dialog() override;
+
+private:
+    bool get_frame_(cv::Mat& frame);
+    void WaitForFirstFrame();
+
+private:
+    // Current Kinect
+    IKinectSensor* iKinectSensor = nullptr;
+
+    // Infrared reader
+    IInfraredFrameReader*  iInfraredFrameReader = nullptr;
+
+    // Frame needs to stay alive while we access the data buffer
+    IInfraredFrame* iInfraredFrame = nullptr;
+
+    video::frame iFrame;
+    cv::Mat iMatFrame;
+};
+
+}
+
+
+#endif