Merge pull request #3978 from gateway240/lowpass-resample

Lowpass Filter Improved Uniform Sampling Check

CHANGELOG.md

@@ -56,6 +56,8 @@ v4.6
 - Fix Point Kinematics Reporter variable and initialization error and add unit tests (#3966)
 - `OpenSim::ContactHalfSpace`, `OpenSim::ContactMesh`, and `OpenSim::ContactSphere` now check their associated `Appearance`'s `is_visible` flag when deciding whether to emit their associated decorations (#3993).
 - The message associated with `OpenSim::PropertyException` now includes the full absolute path to the component that threw the exception (#3987).
+- Add an improved uniform sampling check for `std::vector` containers to `CommonUtilities` and use the implemented method in the `TableUtilities::filterLowpass` method (#3968, #3978).
+
 
 v4.5.1
 ======

OpenSim/Common/CommonUtilities.cpp

@@ -72,8 +72,9 @@ std::string OpenSim::getFormattedDateTime(
 SimTK::Vector OpenSim::createVectorLinspace(
         int length, double start, double end) {
     SimTK::Vector v(length);
+    const double step_size = (end - start) / static_cast<double>((length - 1));
     for (int i = 0; i < length; ++i) {
-        v[i] = start + i * (end - start) / (length - 1);
+        v[i] = std::fma(i, step_size, start);
     }
     return v;
 }

OpenSim/Common/CommonUtilities.h

@@ -34,6 +34,7 @@
 #include <numeric>
 #include <stack>
 #include <condition_variable>
+#include <utility>
 
 #include <SimTKcommon/internal/BigMatrix.h>
 
@@ -120,6 +121,88 @@ OSIMCOMMON_API
 SimTK::Vector createVector(std::initializer_list<SimTK::Real> 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 positive step size found if they are not uniform. If the elements are
+ * uniform, this value will be the mean step size. If the input is a one element
+ * vector, the step size will be NaN since a valid step size cannot be
+ * calculated with only 1 element.
+ *
+ * @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.
+ */
+/// @ingroup commonutil
+template <typename T> 
+std::pair<bool, T> isUniform(const std::vector<T>& x) {
+
+    // Initialize step as NaN
+    T step = std::numeric_limits<T>::quiet_NaN();
+    bool tf = false;
+
+    T maxElement = std::max(std::abs(x.front()), std::abs(x.back()));
+    T tol = 4 * std::numeric_limits<T>::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<T>::epsilon() * maxElement)
+                      ? std::numeric_limits<T>::epsilon() * maxElement
+                      : stepAbs;
+    }
+    std::vector<T> 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 {
+         // Use std::remove_if to filter out non-positive numbers from the adjacent difference
+        auto end = std::remove_if(results.begin(), results.end(), [](T n) { return n <= 0; });
+        // Now find the minimum element among the positive numbers
+        if (end != results.begin()) { // Check if there are any positive numbers
+            step = *std::min_element(results.begin(), 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

OpenSim/Common/TableUtilities.cpp

@@ -29,6 +29,9 @@
 #include "PiecewiseLinearFunction.h"
 #include "Signal.h"
 #include "Storage.h"
+#include <algorithm>
+#include <numeric>
+#include <vector>
 
 using namespace OpenSim;
 
@@ -132,18 +135,16 @@ void TableUtilities::filterLowpass(
 
     const auto& time = table.getIndependentColumn();
 
-    double dtMin = SimTK::Infinity;
-    for (int irow = 1; irow < numRows; ++irow) {
-        double dt = time[irow] - time[irow - 1];
-        if (dt < dtMin) dtMin = dt;
-    }
+    const auto uniform = isUniform(time);
+    const auto &uniformlySampled = uniform.first;
+    const auto &dtMin = uniform.second;
+
     OPENSIM_THROW_IF(
             dtMin < SimTK::Eps, Exception, "Storage cannot be resampled.");
 
-    double dtAvg = (time.back() - time.front()) / (numRows - 1);
-
     // Resample if the sampling interval is not uniform.
-    if (dtAvg - dtMin > SimTK::Eps) {
+    if (!uniformlySampled) {
+        log_warn("Table not uniformly sampled! Resampling with interval: {}", dtMin);
         table = resampleWithInterval(table, dtMin);
     }
 

OpenSim/Common/TableUtilities.h

@@ -60,12 +60,43 @@ class OSIMCOMMON_API TableUtilities {
     static int findStateLabelIndex(
             const std::vector<std::string>& 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.

OpenSim/Common/Test/testCommonUtilities.cpp

@@ -100,3 +100,164 @@ TEST_CASE("createVectorLinspaceInterval produces correct output for small spacin
         REQUIRE_THAT(result[i], Catch::Matchers::WithinAbs(expected[i], tol));
     }
 }
+TEST_CASE("isUniform tests") {
+    SECTION("Basic uniform spacing") {
+        std::vector<double> vec1 = {1.0, 2.0, 3.0, 4.0, 5.0};
+        auto result = isUniform(vec1);
+        REQUIRE(result.first == true);
+        REQUIRE_THAT(result.second, Catch::Matchers::WithinAbs(1.0, tol));
+
+        std::vector<double> vec2 = {0.0, 1.0, 2.0, 3.0};
+        result = isUniform(vec2);
+        REQUIRE(result.first == true);
+        REQUIRE_THAT(result.second, Catch::Matchers::WithinAbs(1.0, tol));
+    }
+
+    SECTION("Non-uniform spacing") {
+        std::vector<double> vec2 = {0.0, 0.5, 0.11, 0.16, 0.19, 0.24};
+        auto result = isUniform(vec2);
+
+        REQUIRE(result.first == false); // Should not be uniformly spaced
+        // Verify that the second value returned is the minimum step size found
+        REQUIRE_THAT(result.second, Catch::Matchers::WithinAbs(0.03, tol));
+
+        std::vector<double> vec3 = {1.0, 2.0, 3.5, 4.0};
+        result = isUniform(vec3);
+        REQUIRE(result.first == false);
+        REQUIRE_THAT(result.second, Catch::Matchers::WithinAbs(0.5, tol));
+
+        std::vector<double> vec4 = {1.0, 2.0, 3.0, 4.0, 5.1};
+        result = isUniform(vec4);
+        REQUIRE(result.first == false);
+        REQUIRE_THAT(result.second, Catch::Matchers::WithinAbs(1.0, tol));
+    }
+
+    SECTION("Non-uniform spacing with increase decreasing and negative values") {
+        // Smallest element is 42.7 - 22.9 = 19.8
+        std::vector<double> vec = {100.1, -27.2, 357.2, 0.16, 22.9, 42.7};
+        auto result = isUniform(vec);
+
+        REQUIRE(result.first == false); // Should not be uniformly spaced
+        // Verify that the second value returned is the minimum step size found
+        REQUIRE_THAT(result.second, Catch::Matchers::WithinAbs(19.8, tol));
+    }
+
+    SECTION("Uniform spacing with machine epsilon spacing") {
+        double epsilon = std::numeric_limits<double>::epsilon();
+        std::vector<double> vec5 = {1.0, 1.0 + epsilon, 1.0 + 2 * epsilon, 1.0 + 3 * epsilon};
+        auto result = isUniform(vec5);
+        REQUIRE(result.first == true);
+        REQUIRE_THAT(result.second, Catch::Matchers::WithinAbs(epsilon, tol));
+    }
+
+    SECTION("Uniform spacing with floating-point inaccuracies") {
+        {
+            std::vector<double> vec = {678.0599999999999, 678.0700000000001, 678.08};
+            auto result = isUniform(vec);
+            const double maxElement = std::max(std::abs(vec.front()), std::abs(vec.back()));
+            REQUIRE(result.first == true);
+            INFO("Spacing Value: " << result.second );
+            REQUIRE_THAT(result.second, Catch::Matchers::WithinAbs(0.01, maxElement*tol));
+        }
+
+        {
+            std::vector<double> vec = {
+                23.02499999999976, 23.04999999999976, 23.07499999999976, 23.09999999999976,
+                23.12499999999975, 23.14999999999975, 23.17499999999975, 23.19999999999975,
+                23.22499999999975, 23.24999999999975, 23.27499999999975, 23.29999999999974,
+                23.32499999999974, 23.34999999999974, 23.37499999999974, 23.39999999999974,
+                23.42499999999974, 23.44999999999974, 23.47499999999973, 23.49999999999973,
+                23.52499999999973, 23.54999999999973, 23.57499999999973, 23.59999999999973,
+                23.62499999999973, 23.64999999999973, 23.67499999999972, 23.69999999999972,
+                23.72499999999972, 23.74999999999972, 23.77499999999972, 23.79999999999972,
+                23.82499999999972, 23.84999999999971, 23.87499999999971, 23.89999999999971,
+                23.92499999999971, 23.94999999999971, 23.97499999999971, 23.99999999999971,
+                24.0249999999997, 24.0499999999997, 24.0749999999997, 24.0999999999997,
+                24.1249999999997, 24.1499999999997, 24.1749999999997, 24.19999999999969,
+                24.22499999999969, 24.24999999999969, 24.27499999999969
+            };
+            auto result = isUniform(vec);
+            REQUIRE(result.first == true);
+            // Should be sampling rate 1/40 = 0.025 
+            REQUIRE_THAT(result.second, Catch::Matchers::WithinAbs(0.025, tol));
+        }
+    }
+
+    SECTION("Edge cases") {
+        std::vector<double> vec7 = {1.0, 1.0};
+        auto result = isUniform(vec7);
+        REQUIRE(result.first == true);
+        REQUIRE_THAT(result.second, Catch::Matchers::WithinAbs(0.0, tol));
+
+        std::vector<double> vec8 = {1.0};
+        result = isUniform(vec8);
+        REQUIRE(result.first == true);
+        // There isn't any spacing for a 1 element vector
+        REQUIRE(std::isnan(result.second));
+    }
+}
+
+TEST_CASE("isUniform tests with createVectorLinspace(SimTK::Vector) and createVectorLinspaceInterval (std::vector) Combo") {
+    {
+        const int numEl = 100000;
+        const double rate = 1.0 / 40.0; // Expected step size
+        const double last_num = 2510.65; // Manually computed
+        const double start = 10.675;
+
+        const double step_size = (last_num - start) / (numEl - 1);
+        REQUIRE_THAT(rate, Catch::Matchers::WithinAbs(step_size, tol));
+
+        std::vector<double> values_std = createVectorLinspaceInterval(numEl, start, rate);
+        SimTK::Vector values_simtk = createVectorLinspace(numEl, start, last_num);
+        // Convert to std::vector
+        std::vector<double> std_vector_from_simtk(values_simtk.size());
+        std::copy_n(values_simtk.getContiguousScalarData(), numEl, std_vector_from_simtk.data());
+
+        // Check that the last two elements are the same
+        REQUIRE_THAT(values_std.back(), Catch::Matchers::WithinAbs(last_num, tol));
+        REQUIRE_THAT(std_vector_from_simtk.back(), Catch::Matchers::WithinAbs(last_num, 1000*tol));
+
+        // Check that the rest of the vector matches ==> they should
+        for (size_t i = 0; i < values_std.size(); ++i) {
+            REQUIRE_THAT(values_simtk[i], Catch::Matchers::WithinAbs(values_std[i], 1000*tol));
+        }
+
+        auto result_std = isUniform(values_std);
+        REQUIRE(result_std.first == true);
+        REQUIRE_THAT(result_std.second, Catch::Matchers::WithinAbs(rate, tol));
+
+        auto result_simtk = isUniform(std_vector_from_simtk);
+        REQUIRE(result_simtk.first == true);
+        REQUIRE_THAT(result_simtk.second, Catch::Matchers::WithinAbs(rate, tol));           
+    }
+
+    {
+        const int numEl = 5281;
+        const double rate = 1.0 / 60.0; // Expected step size
+        const double last_num = 80.362; // Manually computed
+        const double start = -7.638;
+
+        std::vector<double> values_std = createVectorLinspaceInterval(numEl, start, rate);
+        SimTK::Vector values_simtk = createVectorLinspace(numEl, start, last_num);
+        // Convert to std::vector
+        std::vector<double> std_vector_from_simtk(values_simtk.size());
+        std::copy_n(values_simtk.getContiguousScalarData(), numEl, std_vector_from_simtk.data());
+
+        // Check that the last two elements are the same
+        REQUIRE_THAT(values_std.back(), Catch::Matchers::WithinAbs(last_num, tol));
+        REQUIRE_THAT(std_vector_from_simtk.back(), Catch::Matchers::WithinAbs(last_num, tol));
+
+        // Check that the rest of the vector matches ==> they should
+        for (size_t i = 0; i < values_std.size(); ++i) {
+            REQUIRE_THAT(values_simtk[i], Catch::Matchers::WithinAbs(values_std[i], tol));
+        }
+
+        auto result_std = isUniform(values_std);
+        REQUIRE(result_std.first == true);
+        REQUIRE_THAT(result_std.second, Catch::Matchers::WithinAbs(rate, tol));
+
+        auto result_simtk = isUniform(std_vector_from_simtk);
+        REQUIRE(result_simtk.first == true);
+        REQUIRE_THAT(result_simtk.second, Catch::Matchers::WithinAbs(rate, tol));
+    }
+}