72 lines
2.1 KiB
C
72 lines
2.1 KiB
C
|
// 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)
|
||
|
|
{
|
||
|
|
// Remove nans
|
||
|
|
if (v != v)
|
||
|
|
return;
|
||
|
|
|
||
|
|
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;
|
||
|
|
};
|
||
|
|
};
|