From c705176f3430dc4cf95f8449502f81d56e389e7c Mon Sep 17 00:00:00 2001 From: AlexBeattie42 <30098201+alexbeattie42@users.noreply.github.com> Date: Thu, 5 Dec 2024 09:53:35 +0200 Subject: [PATCH] Add isUniform to CommonUtilities and Update Comments --- OpenSim/Common/CommonUtilities.h | 96 ++++++++++++++++++++++++++++--- OpenSim/Common/TableUtilities.cpp | 45 +-------------- OpenSim/Common/TableUtilities.h | 46 ++++++++++++--- 3 files changed, 127 insertions(+), 60 deletions(-) diff --git a/OpenSim/Common/CommonUtilities.h b/OpenSim/Common/CommonUtilities.h index 795e22cf1e..5c31a0abfd 100644 --- a/OpenSim/Common/CommonUtilities.h +++ b/OpenSim/Common/CommonUtilities.h @@ -25,10 +25,13 @@ #include "osimCommonDLL.h" #include "Assertion.h" +#include +#include #include #include #include #include +#include #include #include @@ -88,6 +91,81 @@ OSIMCOMMON_API SimTK::Vector createVector(std::initializer_list elements); #endif +/** + * @brief Checks if the elements of a vector are uniformly spaced. + * + * This function determines whether the elements in the provided vector are + * uniformly spaced within a specified tolerance. It calculates the mean step + * size between adjacent elements and checks if the absolute difference between + * each step and the mean step is within the defined tolerance. If the vector + * contains only two elements, it is considered uniform by default. Specifically + * this verifies that the spacing between consecutive elements in numeric vector + * x does not deviate from the mean spacing by more than 4*eps(max(abs(x))), + * provided that the mean spacing is greater than that tolerance. + * + * @tparam T The type of the elements in the vector. Must support arithmetic + * operations and the std::abs function. + * + * @param x A constant reference to a vector of elements of type T. The vector + * should contain at least one element. + * + * @return A pair containing: + * - A boolean indicating whether the elements are uniformly spaced + * (true) or not (false). + * - The calculated step size if the elements are uniform, or the + * minimum step size found if they are not uniform. If the elements are uniform, + * this value will be the mean step size. + * + * @note The function uses a tolerance based on the maximum absolute value of + * the first and last elements in the vector, scaled by machine epsilon. If the + * vector is empty or contains only one element, the behavior is undefined. + * @note The function implementation draws inspiration from Matlab's `isuniform`. + * See https://mathworks.com/help/matlab/ref/isuniform.html for more details. + * details + */ +/// @ingroup commonutil +OSIMCOMMON_API +template std::pair isUniform(const std::vector& x) { + + // Initialize step as NaN + T step = std::numeric_limits::quiet_NaN(); + bool tf = false; + + T maxElement = std::max(std::abs(x.front()), std::abs(x.back())); + T tol = 4 * std::numeric_limits::epsilon() * maxElement; + size_t numSpaces = x.size() - 1; + T span = x.back() - x.front(); + const T mean_step = + (std::isfinite(span)) + ? span / numSpaces + : (x.back() / numSpaces - x.front() / numSpaces); + + T stepAbs = std::abs(mean_step); + if (stepAbs < tol) { + tol = (stepAbs < std::numeric_limits::epsilon() * maxElement) + ? std::numeric_limits::epsilon() * maxElement + : stepAbs; + } + std::vector results(x.size()); + std::adjacent_difference(x.begin(), x.end(), results.begin()); + // First value from adjacent_difference is the first input so it is skipped + tf = std::all_of( + results.begin() + 1, results.end(), [&mean_step, &tol](T val) { + return std::abs(val - mean_step) <= tol; + }); + + if (!tf && x.size() == 2) { + tf = true; // Handle special case for two elements + } + if (tf) { + step = mean_step; + } else { + step = *std::min_element(results.begin() + 1, results.end()); + } + + return {tf, step}; +} + /// Linearly interpolate y(x) at new values of x. The optional 'ignoreNaNs' /// argument will ignore any NaN values contained in the input vectors and /// create the interpolant from the non-NaN values only. Note that this option @@ -182,24 +260,24 @@ template class ThreadsafeJar { OSIMCOMMON_API SimTK::Matrix computeKNearestNeighbors(const SimTK::Matrix& x, const SimTK::Matrix& y, int k = 1); -/// Use non-negative matrix factorization to decompose an matrix A (NxM) for a -/// selected number of factors 'K' into two matrices W (NxK) and H (KxM) such -/// that A = W * H. The alternating least squares (ALS) algorithm is used to -/// solve for W and H by minimizing the Frobenius norm of the error between A +/// Use non-negative matrix factorization to decompose an matrix A (NxM) for a +/// selected number of factors 'K' into two matrices W (NxK) and H (KxM) such +/// that A = W * H. The alternating least squares (ALS) algorithm is used to +/// solve for W and H by minimizing the Frobenius norm of the error between A /// and W * H. The matrices W and H are scaled assuming that the rows of H /// have magnitudes as if all elements in H were equal to 0.5, which prevents -/// individual factors from being very large or very small. The algorithm -/// terminates when the change in the error norm is less than the specified +/// individual factors from being very large or very small. The algorithm +/// terminates when the change in the error norm is less than the specified /// tolerance or the maximum number of iterations is reached. /// /// @returns The final Frobenius norm of the error between A and W * H. /// /// Reference /// --------- -/// Berry, M. W., et al. (2007). Algorithms and Applications for Approximate -/// Nonnegative Matrix Factorization. Computational Statistics & Data Analysis, +/// Berry, M. W., et al. (2007). Algorithms and Applications for Approximate +/// Nonnegative Matrix Factorization. Computational Statistics & Data Analysis, /// 52(1), 155-173. doi:10.1016/j.csda.2006.11.006. -OSIMCOMMON_API double factorizeMatrixNonNegative(const SimTK::Matrix& A, +OSIMCOMMON_API double factorizeMatrixNonNegative(const SimTK::Matrix& A, int numFactors, int maxIterations, double tolerance, SimTK::Matrix& W, SimTK::Matrix& H); diff --git a/OpenSim/Common/TableUtilities.cpp b/OpenSim/Common/TableUtilities.cpp index e07d684299..8dc8da2c11 100644 --- a/OpenSim/Common/TableUtilities.cpp +++ b/OpenSim/Common/TableUtilities.cpp @@ -122,48 +122,6 @@ int TableUtilities::findStateLabelIndexInternal(const std::string* begin, return -1; } -template -std::pair isUniform(const std::vector& x) { - - // Initialize step as NaN - T step = std::numeric_limits::quiet_NaN(); - bool tf = false; - - T maxElement = std::max(std::abs(x.front()), std::abs(x.back())); - T tol = 4 * std::numeric_limits::epsilon() * maxElement; - size_t numSpaces = x.size() - 1; - T span = x.back() - x.front(); - const T mean_step = - (std::isfinite(span)) - ? span / numSpaces - : (x.back() / numSpaces - x.front() / numSpaces); - - T stepAbs = std::abs(mean_step); - if (stepAbs < tol) { - tol = (stepAbs < std::numeric_limits::epsilon() * maxElement) - ? std::numeric_limits::epsilon() * maxElement - : stepAbs; - } - std::vector results(x.size()); - std::adjacent_difference(x.begin(), x.end(), results.begin()); - // First value from adjacent_difference is the first input so it is skipped - tf = std::all_of( - results.begin() + 1, results.end(), [&mean_step, &tol](T val) { - return std::abs(val - mean_step) <= tol; - }); - - if (!tf && x.size() == 2) { - tf = true; // Handle special case for two elements - } - if (tf) { - step = mean_step; - } else { - step = *std::min_element(results.begin()+1, results.end()); - } - - return {tf, step}; -} - void TableUtilities::filterLowpass( TimeSeriesTable& table, double cutoffFreq, bool padData) { OPENSIM_THROW_IF(cutoffFreq < 0, Exception, @@ -177,10 +135,9 @@ void TableUtilities::filterLowpass( const auto& time = table.getIndependentColumn(); - double dtMin = SimTK::Infinity; const auto uniform = isUniform(time); const auto &uniformlySampled = uniform.first; - dtMin = uniform.second; + const auto &dtMin = uniform.second; OPENSIM_THROW_IF( dtMin < SimTK::Eps, Exception, "Storage cannot be resampled."); diff --git a/OpenSim/Common/TableUtilities.h b/OpenSim/Common/TableUtilities.h index 93eed91343..1808246097 100644 --- a/OpenSim/Common/TableUtilities.h +++ b/OpenSim/Common/TableUtilities.h @@ -60,12 +60,44 @@ class OSIMCOMMON_API TableUtilities { static int findStateLabelIndex( const std::vector& labels, const std::string& desired); - /// Lowpass filter the data in a TimeSeriesTable at a provided cutoff - /// frequency. If padData is true, then the data is first padded with pad() - /// using numRowsToPrependAndAppend = table.getNumRows() / 2. - /// The filtering is performed with Signal::LowpassIIR() - static void filterLowpass(TimeSeriesTable& table, - double cutoffFreq, bool padData = false); + /** + * @brief Applies a lowpass filter to the data in a TimeSeriesTable at a + * specified cutoff frequency. + * + * This function first checks if the provided cutoff frequency is + * non-negative. If the `padData` parameter is set to true, the data is + * mirror padded using the `pad()` function. + * The amount of padding is half the number of rows in the table on each side. + * In other words, using numRowsToPrependAndAppend = table.getNumRows() / 2. + * This will make the resulting data twice as long as the original and may + * include "negative" time if the original independent (time) column began at 0. + * + * The function then verifies that the number of rows in the table is at + * least 4, as filtering requires a minimum amount of data. It retrieves the + * independent time column and checks if the time samples are uniformly + * spaced. If the time intervals are not uniform, the data is resampled to + * ensure consistent sampling before applying the lowpass filter. + * See `CommonUtilities.h` for more information on how the uniform sampling + * is calculated to determine if the data should be resampled. + * + * The filtering is performed using the `Signal::LowpassIIR()` method, which + * processes each dependent column of the table with the specified cutoff + * frequency and the determined sampling interval. + * + * @param table A reference to the TimeSeriesTable containing the data to be + * filtered. + * @param cutoffFreq The cutoff frequency for the lowpass filter. Must be + * non-negative. + * @param padData A boolean indicating whether to pad the data before + * filtering. + * + * @throws Exception if the cutoff frequency is negative, if the number of + * rows is less than 4, or if the time intervals are not suitable for + * resampling. + * + */ + static void filterLowpass( + TimeSeriesTable& table, double cutoffFreq, bool padData = false); /// Pad each column by the number of rows specified. The padded data is /// obtained by reflecting and negating the data in the table. @@ -99,7 +131,7 @@ class OSIMCOMMON_API TableUtilities { static TimeSeriesTable resampleWithIntervalBounded( const TimeSeriesTable& in, double interval); - // Utility to convert TimeSeriesTable of Rotations to a + // Utility to convert TimeSeriesTable of Rotations to a // corresponding TimeSeriesTableVec3 of BodyFixedXYZ Euler angles static TimeSeriesTable_ convertRotationsToEulerAngles( const TimeSeriesTable_& rotTable);