Material for the Azure Kinect Reactor presentation

reactor2019/README.md

@@ -0,0 +1,3 @@
+# Reactor 2019
+
+Content presented at the Microsoft Reactor for the Azure Kinect DK Workshop in October 2019.

reactor2019/bodytracking/jump_analysis_sample/DigitalSignalProcessing.cpp

@@ -0,0 +1,146 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#include "DigitalSignalProcessing.h"
+
+#include <cmath>
+#include <limits>
+
+std::vector<float> DSP::MovingAverage(const std::vector<float>& signal, size_t numOfPoints)
+{
+    if (numOfPoints > signal.size())
+    {
+        return std::vector<float>();
+    }
+
+    if (signal.size() == 1)
+    {
+        return signal;
+    }
+    std::vector<float> cumSum(signal.size());
+
+    std::vector<float> window(numOfPoints);
+    std::vector<float> filteredSignal(signal.size());
+
+    int windowIndex = 0;
+
+    for (size_t i = 0; i < signal.size(); i++)
+    {
+        window[windowIndex] = signal[i] / numOfPoints;
+        float currentMean = 0.0f;
+        for (size_t j = 0; j < numOfPoints; j++)
+        {
+            currentMean = currentMean + window[j];
+        }
+        filteredSignal[i] = currentMean;
+        windowIndex = (windowIndex + 1) % numOfPoints;
+    }
+    return filteredSignal;
+}
+
+std::vector<float> DSP::FirstDerivate(const std::vector<float>& signal)
+{
+    std::vector<float> outputSignal(signal.size() - 1);
+
+    for (size_t i = 1; i < signal.size(); i++)
+    {
+        outputSignal[i - 1] = signal[i] - signal[i - 1];
+    }
+    return outputSignal;
+}
+
+std::vector<float> DSP::DivideTwoArrays(std::vector<float>& dividend, std::vector<float>& divisor)
+{
+    if (dividend.size() != divisor.size())
+    {
+        return std::vector<float>();
+    }
+
+    std::vector<float> result(dividend.size());
+
+    for (size_t i = 0; i < dividend.size(); i++)
+    {
+        if (divisor[i] == 0)
+        {
+            result[i] = 0;
+        }
+        else
+        {
+            result[i] = dividend[i] / divisor[i];
+        }
+    }
+    return result;
+}
+
+IndexValueTuple DSP::FindMaximum(const std::vector<float>& signal, size_t minIdx, size_t maxIdx)
+{
+    if (minIdx > signal.size() || minIdx > signal.size() || minIdx > maxIdx)
+    {
+        return IndexValueTuple();
+    }
+    float maxValue = std::numeric_limits<float>::min();
+    int maxIndex = 0;
+
+    for (size_t i = minIdx; i < maxIdx; i++)
+    {
+        if (signal[i] > maxValue)
+        {
+            maxValue = signal[i];
+            maxIndex = static_cast<int>(i);
+        }
+    }
+    return { maxIndex, maxValue };
+}
+
+IndexValueTuple DSP::FindMinimum(const std::vector<float>& signal, size_t minIdx, size_t maxIdx)
+{
+    if (minIdx > signal.size() || minIdx > signal.size() || minIdx > maxIdx)
+    {
+        return IndexValueTuple();
+    }
+    float minValue = std::numeric_limits<float>::max();
+    int minIndex = 0;
+
+    for (size_t i = minIdx; i < maxIdx; i++)
+    {
+        if (signal[i] < minValue)
+        {
+            minValue = signal[i];
+            minIndex = static_cast<int>(i);
+        }
+    }
+    return { minIndex, minValue };
+}
+
+float DSP::Angle(k4a_float3_t A, k4a_float3_t B, k4a_float3_t C)
+{
+    k4a_float3_t AbVector;
+    k4a_float3_t BcVector;
+
+    AbVector.xyz.x = B.xyz.x - A.xyz.x;
+    AbVector.xyz.y = B.xyz.y - A.xyz.y;
+    AbVector.xyz.z = B.xyz.z - A.xyz.z;
+
+    BcVector.xyz.x = C.xyz.x - B.xyz.x;
+    BcVector.xyz.y = C.xyz.y - B.xyz.y;
+    BcVector.xyz.z = C.xyz.z - B.xyz.z;
+
+    float AbNorm = (float)sqrt(AbVector.xyz.x * AbVector.xyz.x + AbVector.xyz.y * AbVector.xyz.y + AbVector.xyz.z * AbVector.xyz.z);
+    float BcNorm = (float)sqrt(BcVector.xyz.x * BcVector.xyz.x + BcVector.xyz.y * BcVector.xyz.y + BcVector.xyz.z * BcVector.xyz.z);
+
+    k4a_float3_t AbVectorNorm;
+    k4a_float3_t BcVectorNorm;
+
+    AbVectorNorm.xyz.x = AbVector.xyz.x / AbNorm;
+    AbVectorNorm.xyz.y = AbVector.xyz.y / AbNorm;
+    AbVectorNorm.xyz.z = AbVector.xyz.z / AbNorm;
+
+    BcVectorNorm.xyz.x = BcVector.xyz.x / BcNorm;
+    BcVectorNorm.xyz.y = BcVector.xyz.y / BcNorm;
+    BcVectorNorm.xyz.z = BcVector.xyz.z / BcNorm;
+
+    float result = AbVectorNorm.xyz.x * BcVectorNorm.xyz.x + AbVectorNorm.xyz.y * BcVectorNorm.xyz.y + AbVectorNorm.xyz.z * BcVectorNorm.xyz.z;
+
+    result = (float)std::acos(result) * 180.0f / 3.1415926535897f;
+    return result;
+}

reactor2019/bodytracking/jump_analysis_sample/DigitalSignalProcessing.h

@@ -0,0 +1,67 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#pragma once
+
+#include <chrono>
+#include <vector>
+
+#include <k4abttypes.h>
+
+struct IndexValueTuple
+{
+    int Index = -1;
+    float Value = 0.f;
+};
+
+namespace DSP
+{
+    std::vector<float> MovingAverage(const std::vector<float>& signal, size_t numOfPoints);
+
+    std::vector<float> FirstDerivate(const std::vector<float>& signal);
+
+    std::vector<float> DivideTwoArrays(std::vector<float>& dividend, std::vector<float>& divisor);
+
+    IndexValueTuple FindMaximum(const std::vector<float>& signal, size_t minIdx, size_t maxIdx);
+
+    IndexValueTuple FindMinimum(const std::vector<float>& signal, size_t minIdx, size_t maxIdx);
+
+    float Angle(k4a_float3_t A, k4a_float3_t B, k4a_float3_t C);
+
+    class RollingWindow
+    {
+    public:
+        RollingWindow(int windowSize)
+        {
+            m_window.resize(windowSize, 0);
+            m_circularIndex = windowSize - 1;
+        }
+        void Update(const std::chrono::microseconds& timestamp, float v)
+        {
+            m_circularIndex = (m_circularIndex + 1) % m_window.size();
+
+            m_movingAverageDelta = (v - m_window[m_circularIndex]) / m_window.size();
+            m_movingAverage += m_movingAverageDelta;
+            m_window[m_circularIndex] = v;
+
+            if (m_circularIndex == m_window.size() - 1)
+            {
+                m_filled = true;
+            }
+            m_timestampDelta = timestamp - m_timestamp;
+            m_timestamp = timestamp;
+        }
+        bool IsValid() const { return m_filled; }
+        float GetMovingAverage() const { return m_movingAverage; }
+        float GetMovingAverageVelocity() const { return m_movingAverageDelta / m_timestampDelta.count(); }
+
+    private:
+        std::vector<float> m_window;
+        float m_movingAverage = 0;
+        float m_movingAverageDelta = 0;
+        size_t m_circularIndex = 0;
+        bool m_filled = false;
+        std::chrono::microseconds m_timestamp;
+        std::chrono::microseconds m_timestampDelta;
+    };
+};

reactor2019/bodytracking/jump_analysis_sample/HandRaisedDetector.cpp

@@ -0,0 +1,41 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#include "HandRaisedDetector.h"
+
+using namespace std::chrono;
+
+void HandRaisedDetector::UpdateData(k4abt_body_t selectedBody, uint64_t currentTimestampUsec)
+{
+    k4a_float3_t leftWristJoint = selectedBody.skeleton.joints[K4ABT_JOINT_WRIST_LEFT].position;
+    k4a_float3_t rightWristJoint = selectedBody.skeleton.joints[K4ABT_JOINT_WRIST_RIGHT].position;
+    k4a_float3_t headJoint = selectedBody.skeleton.joints[K4ABT_JOINT_HEAD].position;
+
+    // Notice: y direction is pointing towards the ground! So jointA.y < jointB.y means jointA is higher than jointB
+    bool bothHandsAreRaised = leftWristJoint.xyz.y < headJoint.xyz.y &&
+                               rightWristJoint.xyz.y < headJoint.xyz.y;
+
+    microseconds currentTimestamp(currentTimestampUsec);
+    if (m_previousTimestamp == microseconds::zero())
+    {
+        m_previousTimestamp = currentTimestamp;
+        m_handRaisedTimeSpan = microseconds::zero();
+    }
+
+    if (!m_bothHandsAreRaised && bothHandsAreRaised)
+    {
+        // Start accumulating the hand raising time
+        m_handRaisedTimeSpan += currentTimestamp - m_previousTimestamp;
+        if (m_handRaisedTimeSpan > m_stableTime)
+        {
+            m_bothHandsAreRaised = bothHandsAreRaised;
+        }
+    }
+    else if (!bothHandsAreRaised)
+    {
+        // Stop the time accumulation immediately when hands are put down
+        m_bothHandsAreRaised = false;
+        m_previousTimestamp = microseconds::zero();
+        m_handRaisedTimeSpan = microseconds::zero();
+    }
+}

reactor2019/bodytracking/jump_analysis_sample/HandRaisedDetector.h

@@ -0,0 +1,21 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#pragma once
+
+#include <k4abttypes.h>
+#include <chrono>
+
+class HandRaisedDetector
+{
+public:
+    void UpdateData(k4abt_body_t selectedBody, uint64_t currentTimestampUsec);
+
+    bool AreBothHandsRaised() { return m_bothHandsAreRaised; }
+
+private:
+    bool m_bothHandsAreRaised = false;
+    std::chrono::microseconds m_handRaisedTimeSpan = std::chrono::microseconds::zero();
+    std::chrono::microseconds m_previousTimestamp = std::chrono::microseconds::zero();
+    const std::chrono::seconds m_stableTime = std::chrono::seconds(2);
+};