From 1da2d0e71a03296a40b49316b5e23986de823a75 Mon Sep 17 00:00:00 2001 From: Stéphane Lenclud Date: Sun, 3 Feb 2019 12:28:12 +0100 Subject: Kinect: Renaming a few files. --- tracker-kinect-face/kinect_face_settings.cpp | 45 +++ tracker-kinect-face/kinect_face_settings.h | 28 ++ tracker-kinect-face/kinect_face_tracker.cpp | 569 +++++++++++++++++++++++++++ tracker-kinect-face/kinect_face_tracker.h | 113 ++++++ tracker-kinect-face/test.cpp | 45 --- tracker-kinect-face/test.h | 28 -- tracker-kinect-face/test_dialog.cpp | 2 +- tracker-kinect-face/tracker.cpp | 569 --------------------------- tracker-kinect-face/tracker.h | 113 ------ 9 files changed, 756 insertions(+), 756 deletions(-) create mode 100644 tracker-kinect-face/kinect_face_settings.cpp create mode 100644 tracker-kinect-face/kinect_face_settings.h create mode 100644 tracker-kinect-face/kinect_face_tracker.cpp create mode 100644 tracker-kinect-face/kinect_face_tracker.h delete mode 100644 tracker-kinect-face/test.cpp delete mode 100644 tracker-kinect-face/test.h delete mode 100644 tracker-kinect-face/tracker.cpp delete mode 100644 tracker-kinect-face/tracker.h (limited to 'tracker-kinect-face') diff --git a/tracker-kinect-face/kinect_face_settings.cpp b/tracker-kinect-face/kinect_face_settings.cpp new file mode 100644 index 00000000..53aada06 --- /dev/null +++ b/tracker-kinect-face/kinect_face_settings.cpp @@ -0,0 +1,45 @@ +/* Copyright (c) 2014, Stanislaw Halik + + * 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 "kinect_face_settings.h" +#include "kinect_face_tracker.h" +#include "api/plugin-api.hpp" +#include "compat/math-imports.hpp" + +#include + +#include +#include + + +test_dialog::test_dialog() +{ + ui.setupUi(this); + + connect(ui.buttonBox, &QDialogButtonBox::clicked, [this](QAbstractButton* btn) { + if (btn == ui.buttonBox->button(QDialogButtonBox::Abort)) + // NOLINTNEXTLINE + *(volatile int*)nullptr = 0; + }); + + connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); + connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); +} + +void test_dialog::doOK() +{ + //s.b->save(); + close(); +} + +void test_dialog::doCancel() +{ + close(); +} + +OPENTRACK_DECLARE_TRACKER(KinectFaceTracker, test_dialog, test_metadata) diff --git a/tracker-kinect-face/kinect_face_settings.h b/tracker-kinect-face/kinect_face_settings.h new file mode 100644 index 00000000..0f8c170b --- /dev/null +++ b/tracker-kinect-face/kinect_face_settings.h @@ -0,0 +1,28 @@ +#pragma once +#include "ui_test.h" +#include "compat/macros.hpp" +#include "api/plugin-api.hpp" + + +class test_dialog : public ITrackerDialog +{ + Q_OBJECT + + Ui::test_ui ui; +public: + test_dialog(); + void register_tracker(ITracker *) override {} + void unregister_tracker() override {} +private slots: + void doOK(); + void doCancel(); +}; + +class test_metadata : public Metadata +{ + Q_OBJECT + + QString name() { return tr("Kinect Face 0.1"); } + QIcon icon() { return QIcon(":/images/kinect.png"); } +}; + diff --git a/tracker-kinect-face/kinect_face_tracker.cpp b/tracker-kinect-face/kinect_face_tracker.cpp new file mode 100644 index 00000000..ccac8aa9 --- /dev/null +++ b/tracker-kinect-face/kinect_face_tracker.cpp @@ -0,0 +1,569 @@ + + +#include "kinect_face_tracker.h" + +#include +#include + + + +/// +bool IsValidRect(const RectI& aRect) +{ + if (aRect.Bottom != 0) + { + return true; + } + + if (aRect.Left != 0) + { + return true; + } + + if (aRect.Right != 0) + { + return true; + } + + if (aRect.Top != 0) + { + return true; + } + + return false; +} + +/// +bool IsNullVetor(const Vector4& aVector) +{ + if (aVector.w != 0) + { + return false; + } + + if (aVector.x != 0) + { + return false; + } + + if (aVector.y != 0) + { + return false; + } + + if (aVector.z != 0) + { + return false; + } + + return true; +} + +/// +bool IsNullPoint(const CameraSpacePoint& aPoint) +{ + if (aPoint.X != 0) + { + return false; + } + + if (aPoint.Y != 0) + { + return false; + } + + if (aPoint.Z != 0) + { + return false; + } + + return true; +} + + +KinectFaceTracker::KinectFaceTracker(): + m_pKinectSensor(nullptr), + m_pCoordinateMapper(nullptr), + m_pColorFrameReader(nullptr), + m_pColorRGBX(nullptr), + m_pBodyFrameReader(nullptr), + iLastFacePosition{0,0,0}, + iFacePositionCenter{ 0,0,0 }, + iFacePosition{ 0,0,0 }, + iLastFaceRotation{ 0,0,0 }, + iFaceRotationCenter{ 0,0,0 }, + iFaceRotation{ 0,0,0 } +{ + m_pFaceFrameSource = nullptr; + m_pFaceFrameReader = nullptr; + + // create heap storage for color pixel data in RGBX format + m_pColorRGBX = new RGBQUAD[cColorWidth * cColorHeight]; +} + +KinectFaceTracker::~KinectFaceTracker() +{ + if (m_pColorRGBX) + { + delete[] m_pColorRGBX; + m_pColorRGBX = nullptr; + } + + // clean up Direct2D + //SafeRelease(m_pD2DFactory); + + // done with face sources and readers + SafeRelease(m_pFaceFrameSource); + SafeRelease(m_pFaceFrameReader); + + // done with body frame reader + SafeRelease(m_pBodyFrameReader); + + // done with color frame reader + SafeRelease(m_pColorFrameReader); + + // done with coordinate mapper + SafeRelease(m_pCoordinateMapper); + + // close the Kinect Sensor + if (m_pKinectSensor) + { + m_pKinectSensor->Close(); + } + + SafeRelease(m_pKinectSensor); +} + +module_status KinectFaceTracker::start_tracker(QFrame* aFrame) +{ + t.start(); + + if (SUCCEEDED(InitializeDefaultSensor())) + { + // Setup our video preview widget + iVideoWidget = std::make_unique(aFrame); + iLayout = std::make_unique(aFrame); + iLayout->setContentsMargins(0, 0, 0, 0); + iLayout->addWidget(iVideoWidget.get()); + aFrame->setLayout(iLayout.get()); + //video_widget->resize(video_frame->width(), video_frame->height()); + aFrame->show(); + + return status_ok(); + } + + return error("Kinect init failed!"); +} + +#ifdef EMIT_NAN +# include +#endif + +bool KinectFaceTracker::center() +{ + // Mark our center + iFacePositionCenter = iFacePosition; + iFaceRotationCenter = iFaceRotation; + return true; +} + + +void KinectFaceTracker::data(double *data) +{ + const double dt = t.elapsed_seconds(); + t.start(); + + Update(); + + ExtractFaceRotationInDegrees(&iFaceRotationQuaternion, &iFaceRotation.X, &iFaceRotation.Y, &iFaceRotation.Z); + + //Check if data is valid + if (!IsNullPoint(iFacePosition) && !IsNullPoint(iFaceRotation)) + { + // We have valid tracking retain position and rotation + iLastFacePosition = iFacePosition; + iLastFaceRotation = iFaceRotation; + } + + // Feed our framework our last valid position and rotation + data[0] = (iLastFacePosition.X - iFacePositionCenter.X) * 100; // Convert to centimer to be in a range that suites OpenTrack. + data[1] = (iLastFacePosition.Y - iFacePositionCenter.Y) * 100; + data[2] = (iLastFacePosition.Z - iFacePositionCenter.Z) * 100; + + // Yaw, Picth, Roll + data[3] = -(iLastFaceRotation.X - iFaceRotationCenter.X); // Invert to be compatible with ED out-of-the-box + data[4] = (iLastFaceRotation.Y - iFaceRotationCenter.Y); + data[5] = (iLastFaceRotation.Z - iFaceRotationCenter.Z); +} + + +/// +/// Converts rotation quaternion to Euler angles +/// And then maps them to a specified range of values to control the refresh rate +/// +/// face rotation quaternion +/// rotation about the X-axis +/// rotation about the Y-axis +/// rotation about the Z-axis +void KinectFaceTracker::ExtractFaceRotationInDegrees(const Vector4* pQuaternion, float* pYaw, float* pPitch, float* pRoll) +{ + double x = pQuaternion->x; + double y = pQuaternion->y; + double z = pQuaternion->z; + double w = pQuaternion->w; + + // convert face rotation quaternion to Euler angles in degrees + double dPitch, dYaw, dRoll; + dPitch = atan2(2 * (y * z + w * x), w * w - x * x - y * y + z * z) / M_PI * 180.0; + dYaw = asin(2 * (w * y - x * z)) / M_PI * 180.0; + dRoll = atan2(2 * (x * y + w * z), w * w + x * x - y * y - z * z) / M_PI * 180.0; + + // clamp rotation values in degrees to a specified range of values to control the refresh rate + /* + double increment = c_FaceRotationIncrementInDegrees; + *pPitch = static_cast(floor((dPitch + increment/2.0 * (dPitch > 0 ? 1.0 : -1.0)) / increment) * increment); + *pYaw = static_cast(floor((dYaw + increment/2.0 * (dYaw > 0 ? 1.0 : -1.0)) / increment) * increment); + *pRoll = static_cast(floor((dRoll + increment/2.0 * (dRoll > 0 ? 1.0 : -1.0)) / increment) * increment); + */ + + *pPitch = dPitch; + *pYaw = dYaw; + *pRoll = dRoll; +} + + + +/// +/// Initializes the default Kinect sensor +/// +/// S_OK on success else the failure code +HRESULT KinectFaceTracker::InitializeDefaultSensor() +{ + HRESULT hr; + + // Get and open Kinect sensor + hr = GetDefaultKinectSensor(&m_pKinectSensor); + if (SUCCEEDED(hr)) + { + hr = m_pKinectSensor->Open(); + } + + // TODO: check if we still need that guy + if (SUCCEEDED(hr)) + { + hr = m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper); + } + + // Create color frame reader + if (SUCCEEDED(hr)) + { + UniqueInterface colorFrameSource; + hr = m_pKinectSensor->get_ColorFrameSource(colorFrameSource.PtrPtr()); + colorFrameSource.Reset(); + + if (SUCCEEDED(hr)) + { + hr = colorFrameSource->OpenReader(&m_pColorFrameReader); + } + } + + // Create body frame reader + if (SUCCEEDED(hr)) + { + UniqueInterface bodyFrameSource; + hr = m_pKinectSensor->get_BodyFrameSource(bodyFrameSource.PtrPtr()); + bodyFrameSource.Reset(); + + if (SUCCEEDED(hr)) + { + hr = bodyFrameSource->OpenReader(&m_pBodyFrameReader); + } + } + + // Create HD face frame source + if (SUCCEEDED(hr)) + { + // create the face frame source by specifying the required face frame features + hr = CreateHighDefinitionFaceFrameSource(m_pKinectSensor, &m_pFaceFrameSource); + } + + // Create HD face frame reader + if (SUCCEEDED(hr)) + { + // open the corresponding reader + hr = m_pFaceFrameSource->OpenReader(&m_pFaceFrameReader); + } + + return hr; +} + +/// +/// Main processing function +/// +void KinectFaceTracker::Update() +{ + if (!m_pColorFrameReader || !m_pBodyFrameReader) + { + return; + } + + IColorFrame* pColorFrame = nullptr; + HRESULT hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame); + + if (SUCCEEDED(hr)) + { + INT64 nTime = 0; + IFrameDescription* pFrameDescription = nullptr; + int nWidth = 0; + int nHeight = 0; + ColorImageFormat imageFormat = ColorImageFormat_None; + UINT nBufferSize = 0; + RGBQUAD *pBuffer = nullptr; + + hr = pColorFrame->get_RelativeTime(&nTime); + + if (SUCCEEDED(hr)) + { + hr = pColorFrame->get_FrameDescription(&pFrameDescription); + } + + if (SUCCEEDED(hr)) + { + hr = pFrameDescription->get_Width(&nWidth); + } + + if (SUCCEEDED(hr)) + { + hr = pFrameDescription->get_Height(&nHeight); + } + + if (SUCCEEDED(hr)) + { + hr = pColorFrame->get_RawColorImageFormat(&imageFormat); + } + + if (SUCCEEDED(hr)) + { + // Fetch color buffer + if (imageFormat == ColorImageFormat_Rgba) + { + hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize, reinterpret_cast(&pBuffer)); + } + else if (m_pColorRGBX) + { + pBuffer = m_pColorRGBX; + nBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD); + hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast(pBuffer), ColorImageFormat_Rgba); + } + else + { + hr = E_FAIL; + } + + } + + if (SUCCEEDED(hr)) + { + //DrawStreams(nTime, pBuffer, nWidth, nHeight); + ProcessFaces(); + } + + if (SUCCEEDED(hr)) + { + // Setup our image + QImage image((const unsigned char*)pBuffer, cColorWidth, cColorHeight, sizeof(RGBQUAD)*cColorWidth, QImage::Format_RGBA8888); + if (IsValidRect(iFaceBox)) + { + // Draw our face bounding box + QPainter painter(&image); + painter.setBrush(Qt::NoBrush); + painter.setPen(QPen(Qt::red, 8)); + painter.drawRect(iFaceBox.Left, iFaceBox.Top, iFaceBox.Right - iFaceBox.Left, iFaceBox.Bottom - iFaceBox.Top); + bool bEnd = painter.end(); + } + + // Update our video preview + iVideoWidget->update_image(image); + } + + SafeRelease(pFrameDescription); + } + + SafeRelease(pColorFrame); +} + + +/// +/// Updates body data +/// +/// pointer to the body data storage +/// indicates success or failure +HRESULT KinectFaceTracker::UpdateBodyData(IBody** ppBodies) +{ + HRESULT hr = E_FAIL; + + if (m_pBodyFrameReader != nullptr) + { + IBodyFrame* pBodyFrame = nullptr; + hr = m_pBodyFrameReader->AcquireLatestFrame(&pBodyFrame); + if (SUCCEEDED(hr)) + { + hr = pBodyFrame->GetAndRefreshBodyData(BODY_COUNT, ppBodies); + } + SafeRelease(pBodyFrame); + } + + return hr; +} + + + +/// +/// Processes new face frames +/// +void KinectFaceTracker::ProcessFaces() +{ + HRESULT hr; + IBody* ppBodies[BODY_COUNT] = { 0 }; + bool bHaveBodyData = SUCCEEDED(UpdateBodyData(ppBodies)); + + if (!bHaveBodyData) + { + return; + } + + // TODO: Select closest body + // Just use the first body we find + BOOLEAN tracked; + int i = 0; + while (i < BODY_COUNT) + { + hr = ppBodies[i]->get_IsTracked(&tracked); + UINT64 trackingId = 0; + + if (SUCCEEDED(hr) && tracked) + { + hr = ppBodies[i]->get_TrackingId(&trackingId); + + if (SUCCEEDED(hr)) + { + // Tell our face source to use the given body id + hr = m_pFaceFrameSource->put_TrackingId(trackingId); + break; + } + + } + + i++; + } + + + // iterate through each face reader + for (int iFace = 0; iFace < BODY_COUNT; ++iFace) + { + // retrieve the latest face frame from this reader + IHighDefinitionFaceFrame* pFaceFrame = nullptr; + hr = m_pFaceFrameReader->AcquireLatestFrame(&pFaceFrame); + + BOOLEAN bFaceTracked = false; + if (SUCCEEDED(hr) && nullptr != pFaceFrame) + { + // check if a valid face is tracked in this face frame + hr = pFaceFrame->get_IsTrackingIdValid(&bFaceTracked); + } + + if (SUCCEEDED(hr)) + { + if (bFaceTracked) + { + //OutputDebugStringA("Tracking face!\n"); + + //IFaceFrameResult* pFaceFrameResult = nullptr; + IFaceAlignment* pFaceAlignment = nullptr; + CreateFaceAlignment(&pFaceAlignment); // TODO: check return? + //D2D1_POINT_2F faceTextLayout; + + //hr = pFaceFrame->get_FaceFrameResult(&pFaceFrameResult); + + hr = pFaceFrame->GetAndRefreshFaceAlignmentResult(pFaceAlignment); + + // need to verify if pFaceFrameResult contains data before trying to access it + if (SUCCEEDED(hr) && pFaceAlignment != nullptr) + { + hr = pFaceAlignment->get_FaceBoundingBox(&iFaceBox); + //pFaceFrameResult->get_FaceBoundingBoxInColorSpace(); + + if (SUCCEEDED(hr)) + { + //hr = pFaceFrameResult->GetFacePointsInColorSpace(FacePointType::FacePointType_Count, facePoints); + hr = pFaceAlignment->get_HeadPivotPoint(&iFacePosition); + } + + if (SUCCEEDED(hr)) + { + //hr = pFaceFrameResult->get_FaceRotationQuaternion(&faceRotation); + hr = pFaceAlignment->get_FaceOrientation(&iFaceRotationQuaternion); + } + + if (SUCCEEDED(hr)) + { + //hr = pFaceFrameResult->GetFaceProperties(FaceProperty::FaceProperty_Count, faceProperties); + } + + if (SUCCEEDED(hr)) + { + //hr = GetFaceTextPositionInColorSpace(ppBodies[0], &faceTextLayout); + } + + if (SUCCEEDED(hr)) + { + // draw face frame results + //m_pDrawDataStreams->DrawFaceFrameResults(0, &faceBox, facePoints, &faceRotation, faceProperties, &faceTextLayout); + } + } + + SafeRelease(pFaceAlignment); + } + else + { + // face tracking is not valid - attempt to fix the issue + // a valid body is required to perform this step + if (bHaveBodyData) + { + // check if the corresponding body is tracked + // if this is true then update the face frame source to track this body + IBody* pBody = ppBodies[iFace]; + if (pBody != nullptr) + { + BOOLEAN bTracked = false; + hr = pBody->get_IsTracked(&bTracked); + + UINT64 bodyTId; + if (SUCCEEDED(hr) && bTracked) + { + // get the tracking ID of this body + hr = pBody->get_TrackingId(&bodyTId); + if (SUCCEEDED(hr)) + { + // update the face frame source with the tracking ID + m_pFaceFrameSource->put_TrackingId(bodyTId); + } + } + } + } + } + } + + SafeRelease(pFaceFrame); + } + + if (bHaveBodyData) + { + for (int i = 0; i < _countof(ppBodies); ++i) + { + SafeRelease(ppBodies[i]); + } + } +} + + diff --git a/tracker-kinect-face/kinect_face_tracker.h b/tracker-kinect-face/kinect_face_tracker.h new file mode 100644 index 00000000..220a126f --- /dev/null +++ b/tracker-kinect-face/kinect_face_tracker.h @@ -0,0 +1,113 @@ + + + +#include + +#include "api/plugin-api.hpp" +#include "compat/timer.hpp" +#include "compat/macros.hpp" +#include "cv/video-widget.hpp" + +// Kinect Header files +#include +#include + +#pragma once + +// @deprecated Use UniqueInterface instead. Remove it at some point. +template +inline void SafeRelease(Interface *& pInterfaceToRelease) +{ + if (pInterfaceToRelease != nullptr) + { + pInterfaceToRelease->Release(); + pInterfaceToRelease = nullptr; + } +} + +template +inline void ReleaseInterface(Interface* pInterfaceToRelease) +{ + if (pInterfaceToRelease != nullptr) + { + pInterfaceToRelease->Release(); + } +} + +// Safely use Microsoft interfaces. +template +class UniqueInterface : public std::unique_ptr)> ///**/ +{ +public: + UniqueInterface() : std::unique_ptr)>(nullptr, ReleaseInterface){} + // Access pointer, typically for creation + T** PtrPtr() { return &iPtr; }; + // Called this once the pointer was created + void Reset() { std::unique_ptr)>::reset(iPtr); } + // If ever you want to release that interface before the object is deleted + void Free() { iPtr = nullptr; Reset(); } +private: + T* iPtr = nullptr; +}; + + + +class KinectFaceTracker : public ITracker +{ +public: + KinectFaceTracker(); + ~KinectFaceTracker() override; + module_status start_tracker(QFrame* aFrame) override; + void data(double *data) override; + bool center() override; + +private: + Timer t; + + // Kinect stuff + static const int cColorWidth = 1920; + static const int cColorHeight = 1080; + + + void Update(); + HRESULT InitializeDefaultSensor(); + void ProcessFaces(); + HRESULT UpdateBodyData(IBody** ppBodies); + void ExtractFaceRotationInDegrees(const Vector4* pQuaternion, float* pPitch, float* pYaw, float* pRoll); + + // Current Kinect + IKinectSensor* m_pKinectSensor; + + // Coordinate mapper + ICoordinateMapper* m_pCoordinateMapper; + + // Color reader + IColorFrameReader* m_pColorFrameReader; + + // Body reader + IBodyFrameReader* m_pBodyFrameReader; + + // Face sources + IHighDefinitionFaceFrameSource* m_pFaceFrameSource; + + // Face readers + IHighDefinitionFaceFrameReader* m_pFaceFrameReader; + + // + RGBQUAD* m_pColorRGBX; + + RectI iFaceBox = { 0 }; + + CameraSpacePoint iLastFacePosition; + CameraSpacePoint iFacePosition; + CameraSpacePoint iFacePositionCenter; + + Vector4 iFaceRotationQuaternion; + // As Yaw, Pitch, Roll + CameraSpacePoint iLastFaceRotation; + CameraSpacePoint iFaceRotation; + CameraSpacePoint iFaceRotationCenter; + // + std::unique_ptr iVideoWidget; + std::unique_ptr iLayout; +}; diff --git a/tracker-kinect-face/test.cpp b/tracker-kinect-face/test.cpp deleted file mode 100644 index c1ada177..00000000 --- a/tracker-kinect-face/test.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/* Copyright (c) 2014, Stanislaw Halik - - * 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 "test.h" -#include "tracker.h" -#include "api/plugin-api.hpp" -#include "compat/math-imports.hpp" - -#include - -#include -#include - - -test_dialog::test_dialog() -{ - ui.setupUi(this); - - connect(ui.buttonBox, &QDialogButtonBox::clicked, [this](QAbstractButton* btn) { - if (btn == ui.buttonBox->button(QDialogButtonBox::Abort)) - // NOLINTNEXTLINE - *(volatile int*)nullptr = 0; - }); - - connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK())); - connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel())); -} - -void test_dialog::doOK() -{ - //s.b->save(); - close(); -} - -void test_dialog::doCancel() -{ - close(); -} - -OPENTRACK_DECLARE_TRACKER(KinectFaceTracker, test_dialog, test_metadata) diff --git a/tracker-kinect-face/test.h b/tracker-kinect-face/test.h deleted file mode 100644 index 0f8c170b..00000000 --- a/tracker-kinect-face/test.h +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once -#include "ui_test.h" -#include "compat/macros.hpp" -#include "api/plugin-api.hpp" - - -class test_dialog : public ITrackerDialog -{ - Q_OBJECT - - Ui::test_ui ui; -public: - test_dialog(); - void register_tracker(ITracker *) override {} - void unregister_tracker() override {} -private slots: - void doOK(); - void doCancel(); -}; - -class test_metadata : public Metadata -{ - Q_OBJECT - - QString name() { return tr("Kinect Face 0.1"); } - QIcon icon() { return QIcon(":/images/kinect.png"); } -}; - diff --git a/tracker-kinect-face/test_dialog.cpp b/tracker-kinect-face/test_dialog.cpp index 5d33555b..fadf6084 100644 --- a/tracker-kinect-face/test_dialog.cpp +++ b/tracker-kinect-face/test_dialog.cpp @@ -1,3 +1,3 @@ -#include "test.h" +#include "kinect_face_settings.h" #include "api/plugin-api.hpp" diff --git a/tracker-kinect-face/tracker.cpp b/tracker-kinect-face/tracker.cpp deleted file mode 100644 index 54a716ac..00000000 --- a/tracker-kinect-face/tracker.cpp +++ /dev/null @@ -1,569 +0,0 @@ - - -#include "tracker.h" - -#include -#include - - - -/// -bool IsValidRect(const RectI& aRect) -{ - if (aRect.Bottom != 0) - { - return true; - } - - if (aRect.Left != 0) - { - return true; - } - - if (aRect.Right != 0) - { - return true; - } - - if (aRect.Top != 0) - { - return true; - } - - return false; -} - -/// -bool IsNullVetor(const Vector4& aVector) -{ - if (aVector.w != 0) - { - return false; - } - - if (aVector.x != 0) - { - return false; - } - - if (aVector.y != 0) - { - return false; - } - - if (aVector.z != 0) - { - return false; - } - - return true; -} - -/// -bool IsNullPoint(const CameraSpacePoint& aPoint) -{ - if (aPoint.X != 0) - { - return false; - } - - if (aPoint.Y != 0) - { - return false; - } - - if (aPoint.Z != 0) - { - return false; - } - - return true; -} - - -KinectFaceTracker::KinectFaceTracker(): - m_pKinectSensor(nullptr), - m_pCoordinateMapper(nullptr), - m_pColorFrameReader(nullptr), - m_pColorRGBX(nullptr), - m_pBodyFrameReader(nullptr), - iLastFacePosition{0,0,0}, - iFacePositionCenter{ 0,0,0 }, - iFacePosition{ 0,0,0 }, - iLastFaceRotation{ 0,0,0 }, - iFaceRotationCenter{ 0,0,0 }, - iFaceRotation{ 0,0,0 } -{ - m_pFaceFrameSource = nullptr; - m_pFaceFrameReader = nullptr; - - // create heap storage for color pixel data in RGBX format - m_pColorRGBX = new RGBQUAD[cColorWidth * cColorHeight]; -} - -KinectFaceTracker::~KinectFaceTracker() -{ - if (m_pColorRGBX) - { - delete[] m_pColorRGBX; - m_pColorRGBX = nullptr; - } - - // clean up Direct2D - //SafeRelease(m_pD2DFactory); - - // done with face sources and readers - SafeRelease(m_pFaceFrameSource); - SafeRelease(m_pFaceFrameReader); - - // done with body frame reader - SafeRelease(m_pBodyFrameReader); - - // done with color frame reader - SafeRelease(m_pColorFrameReader); - - // done with coordinate mapper - SafeRelease(m_pCoordinateMapper); - - // close the Kinect Sensor - if (m_pKinectSensor) - { - m_pKinectSensor->Close(); - } - - SafeRelease(m_pKinectSensor); -} - -module_status KinectFaceTracker::start_tracker(QFrame* aFrame) -{ - t.start(); - - if (SUCCEEDED(InitializeDefaultSensor())) - { - // Setup our video preview widget - iVideoWidget = std::make_unique(aFrame); - iLayout = std::make_unique(aFrame); - iLayout->setContentsMargins(0, 0, 0, 0); - iLayout->addWidget(iVideoWidget.get()); - aFrame->setLayout(iLayout.get()); - //video_widget->resize(video_frame->width(), video_frame->height()); - aFrame->show(); - - return status_ok(); - } - - return error("Kinect init failed!"); -} - -#ifdef EMIT_NAN -# include -#endif - -bool KinectFaceTracker::center() -{ - // Mark our center - iFacePositionCenter = iFacePosition; - iFaceRotationCenter = iFaceRotation; - return true; -} - - -void KinectFaceTracker::data(double *data) -{ - const double dt = t.elapsed_seconds(); - t.start(); - - Update(); - - ExtractFaceRotationInDegrees(&iFaceRotationQuaternion, &iFaceRotation.X, &iFaceRotation.Y, &iFaceRotation.Z); - - //Check if data is valid - if (!IsNullPoint(iFacePosition) && !IsNullPoint(iFaceRotation)) - { - // We have valid tracking retain position and rotation - iLastFacePosition = iFacePosition; - iLastFaceRotation = iFaceRotation; - } - - // Feed our framework our last valid position and rotation - data[0] = (iLastFacePosition.X - iFacePositionCenter.X) * 100; // Convert to centimer to be in a range that suites OpenTrack. - data[1] = (iLastFacePosition.Y - iFacePositionCenter.Y) * 100; - data[2] = (iLastFacePosition.Z - iFacePositionCenter.Z) * 100; - - // Yaw, Picth, Roll - data[3] = -(iLastFaceRotation.X - iFaceRotationCenter.X); // Invert to be compatible with ED out-of-the-box - data[4] = (iLastFaceRotation.Y - iFaceRotationCenter.Y); - data[5] = (iLastFaceRotation.Z - iFaceRotationCenter.Z); -} - - -/// -/// Converts rotation quaternion to Euler angles -/// And then maps them to a specified range of values to control the refresh rate -/// -/// face rotation quaternion -/// rotation about the X-axis -/// rotation about the Y-axis -/// rotation about the Z-axis -void KinectFaceTracker::ExtractFaceRotationInDegrees(const Vector4* pQuaternion, float* pYaw, float* pPitch, float* pRoll) -{ - double x = pQuaternion->x; - double y = pQuaternion->y; - double z = pQuaternion->z; - double w = pQuaternion->w; - - // convert face rotation quaternion to Euler angles in degrees - double dPitch, dYaw, dRoll; - dPitch = atan2(2 * (y * z + w * x), w * w - x * x - y * y + z * z) / M_PI * 180.0; - dYaw = asin(2 * (w * y - x * z)) / M_PI * 180.0; - dRoll = atan2(2 * (x * y + w * z), w * w + x * x - y * y - z * z) / M_PI * 180.0; - - // clamp rotation values in degrees to a specified range of values to control the refresh rate - /* - double increment = c_FaceRotationIncrementInDegrees; - *pPitch = static_cast(floor((dPitch + increment/2.0 * (dPitch > 0 ? 1.0 : -1.0)) / increment) * increment); - *pYaw = static_cast(floor((dYaw + increment/2.0 * (dYaw > 0 ? 1.0 : -1.0)) / increment) * increment); - *pRoll = static_cast(floor((dRoll + increment/2.0 * (dRoll > 0 ? 1.0 : -1.0)) / increment) * increment); - */ - - *pPitch = dPitch; - *pYaw = dYaw; - *pRoll = dRoll; -} - - - -/// -/// Initializes the default Kinect sensor -/// -/// S_OK on success else the failure code -HRESULT KinectFaceTracker::InitializeDefaultSensor() -{ - HRESULT hr; - - // Get and open Kinect sensor - hr = GetDefaultKinectSensor(&m_pKinectSensor); - if (SUCCEEDED(hr)) - { - hr = m_pKinectSensor->Open(); - } - - // TODO: check if we still need that guy - if (SUCCEEDED(hr)) - { - hr = m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper); - } - - // Create color frame reader - if (SUCCEEDED(hr)) - { - UniqueInterface colorFrameSource; - hr = m_pKinectSensor->get_ColorFrameSource(colorFrameSource.PtrPtr()); - colorFrameSource.Reset(); - - if (SUCCEEDED(hr)) - { - hr = colorFrameSource->OpenReader(&m_pColorFrameReader); - } - } - - // Create body frame reader - if (SUCCEEDED(hr)) - { - UniqueInterface bodyFrameSource; - hr = m_pKinectSensor->get_BodyFrameSource(bodyFrameSource.PtrPtr()); - bodyFrameSource.Reset(); - - if (SUCCEEDED(hr)) - { - hr = bodyFrameSource->OpenReader(&m_pBodyFrameReader); - } - } - - // Create HD face frame source - if (SUCCEEDED(hr)) - { - // create the face frame source by specifying the required face frame features - hr = CreateHighDefinitionFaceFrameSource(m_pKinectSensor, &m_pFaceFrameSource); - } - - // Create HD face frame reader - if (SUCCEEDED(hr)) - { - // open the corresponding reader - hr = m_pFaceFrameSource->OpenReader(&m_pFaceFrameReader); - } - - return hr; -} - -/// -/// Main processing function -/// -void KinectFaceTracker::Update() -{ - if (!m_pColorFrameReader || !m_pBodyFrameReader) - { - return; - } - - IColorFrame* pColorFrame = nullptr; - HRESULT hr = m_pColorFrameReader->AcquireLatestFrame(&pColorFrame); - - if (SUCCEEDED(hr)) - { - INT64 nTime = 0; - IFrameDescription* pFrameDescription = nullptr; - int nWidth = 0; - int nHeight = 0; - ColorImageFormat imageFormat = ColorImageFormat_None; - UINT nBufferSize = 0; - RGBQUAD *pBuffer = nullptr; - - hr = pColorFrame->get_RelativeTime(&nTime); - - if (SUCCEEDED(hr)) - { - hr = pColorFrame->get_FrameDescription(&pFrameDescription); - } - - if (SUCCEEDED(hr)) - { - hr = pFrameDescription->get_Width(&nWidth); - } - - if (SUCCEEDED(hr)) - { - hr = pFrameDescription->get_Height(&nHeight); - } - - if (SUCCEEDED(hr)) - { - hr = pColorFrame->get_RawColorImageFormat(&imageFormat); - } - - if (SUCCEEDED(hr)) - { - // Fetch color buffer - if (imageFormat == ColorImageFormat_Rgba) - { - hr = pColorFrame->AccessRawUnderlyingBuffer(&nBufferSize, reinterpret_cast(&pBuffer)); - } - else if (m_pColorRGBX) - { - pBuffer = m_pColorRGBX; - nBufferSize = cColorWidth * cColorHeight * sizeof(RGBQUAD); - hr = pColorFrame->CopyConvertedFrameDataToArray(nBufferSize, reinterpret_cast(pBuffer), ColorImageFormat_Rgba); - } - else - { - hr = E_FAIL; - } - - } - - if (SUCCEEDED(hr)) - { - //DrawStreams(nTime, pBuffer, nWidth, nHeight); - ProcessFaces(); - } - - if (SUCCEEDED(hr)) - { - // Setup our image - QImage image((const unsigned char*)pBuffer, cColorWidth, cColorHeight, sizeof(RGBQUAD)*cColorWidth, QImage::Format_RGBA8888); - if (IsValidRect(iFaceBox)) - { - // Draw our face bounding box - QPainter painter(&image); - painter.setBrush(Qt::NoBrush); - painter.setPen(QPen(Qt::red, 8)); - painter.drawRect(iFaceBox.Left, iFaceBox.Top, iFaceBox.Right - iFaceBox.Left, iFaceBox.Bottom - iFaceBox.Top); - bool bEnd = painter.end(); - } - - // Update our video preview - iVideoWidget->update_image(image); - } - - SafeRelease(pFrameDescription); - } - - SafeRelease(pColorFrame); -} - - -/// -/// Updates body data -/// -/// pointer to the body data storage -/// indicates success or failure -HRESULT KinectFaceTracker::UpdateBodyData(IBody** ppBodies) -{ - HRESULT hr = E_FAIL; - - if (m_pBodyFrameReader != nullptr) - { - IBodyFrame* pBodyFrame = nullptr; - hr = m_pBodyFrameReader->AcquireLatestFrame(&pBodyFrame); - if (SUCCEEDED(hr)) - { - hr = pBodyFrame->GetAndRefreshBodyData(BODY_COUNT, ppBodies); - } - SafeRelease(pBodyFrame); - } - - return hr; -} - - - -/// -/// Processes new face frames -/// -void KinectFaceTracker::ProcessFaces() -{ - HRESULT hr; - IBody* ppBodies[BODY_COUNT] = { 0 }; - bool bHaveBodyData = SUCCEEDED(UpdateBodyData(ppBodies)); - - if (!bHaveBodyData) - { - return; - } - - // TODO: Select closest body - // Just use the first body we find - BOOLEAN tracked; - int i = 0; - while (i < BODY_COUNT) - { - hr = ppBodies[i]->get_IsTracked(&tracked); - UINT64 trackingId = 0; - - if (SUCCEEDED(hr) && tracked) - { - hr = ppBodies[i]->get_TrackingId(&trackingId); - - if (SUCCEEDED(hr)) - { - // Tell our face source to use the given body id - hr = m_pFaceFrameSource->put_TrackingId(trackingId); - break; - } - - } - - i++; - } - - - // iterate through each face reader - for (int iFace = 0; iFace < BODY_COUNT; ++iFace) - { - // retrieve the latest face frame from this reader - IHighDefinitionFaceFrame* pFaceFrame = nullptr; - hr = m_pFaceFrameReader->AcquireLatestFrame(&pFaceFrame); - - BOOLEAN bFaceTracked = false; - if (SUCCEEDED(hr) && nullptr != pFaceFrame) - { - // check if a valid face is tracked in this face frame - hr = pFaceFrame->get_IsTrackingIdValid(&bFaceTracked); - } - - if (SUCCEEDED(hr)) - { - if (bFaceTracked) - { - //OutputDebugStringA("Tracking face!\n"); - - //IFaceFrameResult* pFaceFrameResult = nullptr; - IFaceAlignment* pFaceAlignment = nullptr; - CreateFaceAlignment(&pFaceAlignment); // TODO: check return? - //D2D1_POINT_2F faceTextLayout; - - //hr = pFaceFrame->get_FaceFrameResult(&pFaceFrameResult); - - hr = pFaceFrame->GetAndRefreshFaceAlignmentResult(pFaceAlignment); - - // need to verify if pFaceFrameResult contains data before trying to access it - if (SUCCEEDED(hr) && pFaceAlignment != nullptr) - { - hr = pFaceAlignment->get_FaceBoundingBox(&iFaceBox); - //pFaceFrameResult->get_FaceBoundingBoxInColorSpace(); - - if (SUCCEEDED(hr)) - { - //hr = pFaceFrameResult->GetFacePointsInColorSpace(FacePointType::FacePointType_Count, facePoints); - hr = pFaceAlignment->get_HeadPivotPoint(&iFacePosition); - } - - if (SUCCEEDED(hr)) - { - //hr = pFaceFrameResult->get_FaceRotationQuaternion(&faceRotation); - hr = pFaceAlignment->get_FaceOrientation(&iFaceRotationQuaternion); - } - - if (SUCCEEDED(hr)) - { - //hr = pFaceFrameResult->GetFaceProperties(FaceProperty::FaceProperty_Count, faceProperties); - } - - if (SUCCEEDED(hr)) - { - //hr = GetFaceTextPositionInColorSpace(ppBodies[0], &faceTextLayout); - } - - if (SUCCEEDED(hr)) - { - // draw face frame results - //m_pDrawDataStreams->DrawFaceFrameResults(0, &faceBox, facePoints, &faceRotation, faceProperties, &faceTextLayout); - } - } - - SafeRelease(pFaceAlignment); - } - else - { - // face tracking is not valid - attempt to fix the issue - // a valid body is required to perform this step - if (bHaveBodyData) - { - // check if the corresponding body is tracked - // if this is true then update the face frame source to track this body - IBody* pBody = ppBodies[iFace]; - if (pBody != nullptr) - { - BOOLEAN bTracked = false; - hr = pBody->get_IsTracked(&bTracked); - - UINT64 bodyTId; - if (SUCCEEDED(hr) && bTracked) - { - // get the tracking ID of this body - hr = pBody->get_TrackingId(&bodyTId); - if (SUCCEEDED(hr)) - { - // update the face frame source with the tracking ID - m_pFaceFrameSource->put_TrackingId(bodyTId); - } - } - } - } - } - } - - SafeRelease(pFaceFrame); - } - - if (bHaveBodyData) - { - for (int i = 0; i < _countof(ppBodies); ++i) - { - SafeRelease(ppBodies[i]); - } - } -} - - diff --git a/tracker-kinect-face/tracker.h b/tracker-kinect-face/tracker.h deleted file mode 100644 index 220a126f..00000000 --- a/tracker-kinect-face/tracker.h +++ /dev/null @@ -1,113 +0,0 @@ - - - -#include - -#include "api/plugin-api.hpp" -#include "compat/timer.hpp" -#include "compat/macros.hpp" -#include "cv/video-widget.hpp" - -// Kinect Header files -#include -#include - -#pragma once - -// @deprecated Use UniqueInterface instead. Remove it at some point. -template -inline void SafeRelease(Interface *& pInterfaceToRelease) -{ - if (pInterfaceToRelease != nullptr) - { - pInterfaceToRelease->Release(); - pInterfaceToRelease = nullptr; - } -} - -template -inline void ReleaseInterface(Interface* pInterfaceToRelease) -{ - if (pInterfaceToRelease != nullptr) - { - pInterfaceToRelease->Release(); - } -} - -// Safely use Microsoft interfaces. -template -class UniqueInterface : public std::unique_ptr)> ///**/ -{ -public: - UniqueInterface() : std::unique_ptr)>(nullptr, ReleaseInterface){} - // Access pointer, typically for creation - T** PtrPtr() { return &iPtr; }; - // Called this once the pointer was created - void Reset() { std::unique_ptr)>::reset(iPtr); } - // If ever you want to release that interface before the object is deleted - void Free() { iPtr = nullptr; Reset(); } -private: - T* iPtr = nullptr; -}; - - - -class KinectFaceTracker : public ITracker -{ -public: - KinectFaceTracker(); - ~KinectFaceTracker() override; - module_status start_tracker(QFrame* aFrame) override; - void data(double *data) override; - bool center() override; - -private: - Timer t; - - // Kinect stuff - static const int cColorWidth = 1920; - static const int cColorHeight = 1080; - - - void Update(); - HRESULT InitializeDefaultSensor(); - void ProcessFaces(); - HRESULT UpdateBodyData(IBody** ppBodies); - void ExtractFaceRotationInDegrees(const Vector4* pQuaternion, float* pPitch, float* pYaw, float* pRoll); - - // Current Kinect - IKinectSensor* m_pKinectSensor; - - // Coordinate mapper - ICoordinateMapper* m_pCoordinateMapper; - - // Color reader - IColorFrameReader* m_pColorFrameReader; - - // Body reader - IBodyFrameReader* m_pBodyFrameReader; - - // Face sources - IHighDefinitionFaceFrameSource* m_pFaceFrameSource; - - // Face readers - IHighDefinitionFaceFrameReader* m_pFaceFrameReader; - - // - RGBQUAD* m_pColorRGBX; - - RectI iFaceBox = { 0 }; - - CameraSpacePoint iLastFacePosition; - CameraSpacePoint iFacePosition; - CameraSpacePoint iFacePositionCenter; - - Vector4 iFaceRotationQuaternion; - // As Yaw, Pitch, Roll - CameraSpacePoint iLastFaceRotation; - CameraSpacePoint iFaceRotation; - CameraSpacePoint iFaceRotationCenter; - // - std::unique_ptr iVideoWidget; - std::unique_ptr iLayout; -}; -- cgit v1.2.3