Skip to content

Commit 097410f

Browse files
authored
Merge pull request #3977 from gateway240/uniform-time
Uniform Time Sampling in XsenseDataReader and ADPMDataReader
2 parents c3e3ace + 544810c commit 097410f

File tree

5 files changed

+151
-21
lines changed

5 files changed

+151
-21
lines changed

CHANGELOG.md

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -52,7 +52,7 @@ v4.6
5252
- Make `InverseKinematicsSolver` methods to query for specific marker or orientation-sensor errors more robust to invalid names or names not
5353
in the intersection of names in the model and names in the provided referece/data. Remove methods that are index based from public interface.(#3951)
5454
- Replace usages of `OpenSim::make_unique` with `std::make_unique` and remove wrapper function now that C++14 is used in OpenSim (#3979).
55-
55+
- Add utility method `createVectorLinspaceInterval` for the `std::vector` type and add unit tests. Utilize the new utility method to fix a bug (#3976) in creating the uniformly sampled time interval from the experimental data sampling frequency in `APDMDataReader` and `XsensDataReader` (#3977).
5656

5757

5858
v4.5.1

OpenSim/Common/APDMDataReader.cpp

Lines changed: 3 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
#include <fstream>
2+
#include "CommonUtilities.h"
23
#include "Simbody.h"
34
#include "Exception.h"
45
#include "FileAdapter.h"
@@ -58,8 +59,6 @@ APDMDataReader::extendRead(const std::string& fileName) const {
5859
SimTK::Matrix_<SimTK::Vec3> linearAccelerationData{ last_size, n_imus };
5960
SimTK::Matrix_<SimTK::Vec3> magneticHeadingData{ last_size, n_imus };
6061
SimTK::Matrix_<SimTK::Vec3> angularVelocityData{ last_size, n_imus };
61-
std::vector<double> times;
62-
times.resize(last_size);
6362
// We support two formats, they contain similar data but headers are different
6463
std::string line;
6564
// Line 1
@@ -134,8 +133,6 @@ APDMDataReader::extendRead(const std::string& fileName) const {
134133

135134
// For all tables, will create row, stitch values from different sensors then append
136135
bool done = false;
137-
double time = 0.0;
138-
double timeIncrement = 1 / dataRate;
139136
int rowNumber = 0;
140137
while (!done){
141138
// Make vectors one per table
@@ -172,22 +169,17 @@ APDMDataReader::extendRead(const std::string& fileName) const {
172169
OpenSim::IO::stod(nextRow[orientationsIndex[imu_index] + 3]));
173170
}
174171
// append to the tables
175-
times[rowNumber] = time;
176172
if (foundLinearAccelerationData)
177173
linearAccelerationData[rowNumber] = accel_row_vector;
178174
if (foundMagneticHeadingData)
179175
magneticHeadingData[rowNumber] = magneto_row_vector;
180176
if (foundAngularVelocityData)
181177
angularVelocityData[rowNumber] = gyro_row_vector;
182178
rotationsData[rowNumber] = orientation_row_vector;
183-
// We could get some indication of time from file or generate time based on rate
184-
// Here we use the latter mechanism.
185-
time += timeIncrement;
186179
rowNumber++;
187180
if (std::remainder(rowNumber, last_size) == 0) {
188181
// resize all Data/Matrices, double the size while keeping data
189182
int newSize = last_size*2;
190-
times.resize(newSize);
191183
// Repeat for Data matrices in use
192184
if (foundLinearAccelerationData) linearAccelerationData.resizeKeep(newSize, n_imus);
193185
if (foundMagneticHeadingData) magneticHeadingData.resizeKeep(newSize, n_imus);
@@ -197,7 +189,8 @@ APDMDataReader::extendRead(const std::string& fileName) const {
197189
}
198190
}
199191
// Trim Matrices in use to actual data and move into tables
200-
times.resize(rowNumber);
192+
const double timeIncrement = 1.0 / dataRate;
193+
const auto times = createVectorLinspaceInterval(rowNumber, 0.0, timeIncrement);
201194
// Repeat for Data matrices in use and create Tables from them or size 0 for empty
202195
linearAccelerationData.resizeKeep(foundLinearAccelerationData? rowNumber : 0,
203196
n_imus);

OpenSim/Common/CommonUtilities.h

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,10 +25,13 @@
2525

2626
#include "osimCommonDLL.h"
2727
#include "Assertion.h"
28+
#include <algorithm>
29+
#include <cmath>
2830
#include <functional>
2931
#include <iostream>
3032
#include <memory>
3133
#include <mutex>
34+
#include <numeric>
3235
#include <stack>
3336
#include <condition_variable>
3437

@@ -73,6 +76,43 @@ class OSIMCOMMON_API FileRemover {
7376
OSIMCOMMON_API
7477
SimTK::Vector createVectorLinspace(int length, double start, double end);
7578

79+
/**
80+
* @brief Creates a vector of uniformly spaced values with a known interval.
81+
*
82+
* This function generates a vector of length `length`, where each element
83+
* is calculated based on the starting value and the specified step size.
84+
* The elements are computed as:
85+
*
86+
* output[i] = start + i * step_size
87+
*
88+
* for i = 0, 1, 2, ..., length-1.
89+
*
90+
* @tparam T The type of the elements in the output vector. This can be any
91+
* numeric type (e.g., int, float, double).
92+
* @param length The number of elements in the output vector.
93+
* @param start The starting value of the sequence.
94+
* @param step_size The difference between consecutive elements in the output vector.
95+
* @return A std::vector<T> containing `length` elements, uniformly spaced
96+
* starting from `start` with a step size of `step_size`.
97+
*
98+
* @example
99+
* std::vector<double> vec = createVectorLinspaceInterval(5, 0.0, 2.0);
100+
* // vec will contain: [0.0, 2.0, 4.0, 6.0, 8.0]
101+
*/
102+
/// @ingroup commonutil
103+
template <typename T>
104+
std::vector<T> createVectorLinspaceInterval(
105+
const int length, const T start, const T step_size) {
106+
std::vector<int> ivec(length);
107+
std::iota(ivec.begin(), ivec.end(), 0); // ivec will become: [0..length]
108+
std::vector<T> output(ivec.size());
109+
std::transform(ivec.begin(), ivec.end(), output.begin(),
110+
[step_size, start](int value) {
111+
return static_cast<T>(std::fma(static_cast<T>(value), step_size, start));
112+
});
113+
return output;
114+
};
115+
76116
#ifndef SWIG
77117
/// Create a SimTK::Vector using modern C++ syntax.
78118
/// @ingroup commonutil
Lines changed: 102 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,102 @@
1+
/* -------------------------------------------------------------------------- *
2+
* OpenSim: testCommonUtilities.cpp *
3+
* -------------------------------------------------------------------------- *
4+
* The OpenSim API is a toolkit for musculoskeletal modeling and simulation. *
5+
* See http://opensim.stanford.edu and the NOTICE file for more information. *
6+
* OpenSim is developed at Stanford University and supported by the US *
7+
* National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA *
8+
* through the Warrior Web program. *
9+
* *
10+
* Copyright (c) 2005-2024 Stanford University and the Authors *
11+
* Author(s): Alexander Beattie *
12+
* *
13+
* Licensed under the Apache License, Version 2.0 (the "License"); you may *
14+
* not use this file except in compliance with the License. You may obtain a *
15+
* copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
16+
* *
17+
* Unless required by applicable law or agreed to in writing, software *
18+
* distributed under the License is distributed on an "AS IS" BASIS, *
19+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
20+
* See the License for the specific language governing permissions and *
21+
* limitations under the License. *
22+
* -------------------------------------------------------------------------- */
23+
24+
#include <OpenSim/Common/CommonUtilities.h>
25+
26+
#include <algorithm> // std::transform
27+
#include <limits> // std::numeric_limits
28+
#include <vector>
29+
#include <numeric> // std::iota
30+
#include <cmath> // std::fma
31+
32+
#include <catch2/catch_all.hpp>
33+
34+
using namespace OpenSim;
35+
36+
constexpr double tol = std::numeric_limits<double>::epsilon() * 10;
37+
38+
TEST_CASE("createVectorLinspaceInterval produces correct output for integers", "[createVectorLinspaceInterval]") {
39+
auto result = createVectorLinspaceInterval<int>(5, 0, 2);
40+
std::vector<int> expected = {0, 2, 4, 6, 8};
41+
REQUIRE_THAT(result, Catch::Matchers::Equals(expected));
42+
}
43+
44+
TEST_CASE("createVectorLinspaceInterval produces correct output for floats", "[createVectorLinspaceInterval]") {
45+
auto result = createVectorLinspaceInterval<float>(5, 1.0f, 0.5f);
46+
std::vector<float> expected = {1.0f, 1.5f, 2.0f, 2.5f, 3.0f};
47+
REQUIRE_THAT(result, Catch::Matchers::Equals(expected));
48+
}
49+
50+
TEST_CASE("createVectorLinspaceInterval handles negative step size", "[createVectorLinspaceInterval]") {
51+
auto result = createVectorLinspaceInterval<double>(5, 10.0, -2.0);
52+
std::vector<double> expected = {10.0, 8.0, 6.0, 4.0, 2.0};
53+
REQUIRE_THAT(result, Catch::Matchers::Equals(expected));
54+
}
55+
56+
TEST_CASE("createVectorLinspaceInterval handles zero length", "[createVectorLinspaceInterval]") {
57+
auto result = createVectorLinspaceInterval<double>(0, 0.0, 1.0);
58+
std::vector<double> expected = {};
59+
REQUIRE_THAT(result, Catch::Matchers::Equals(expected));
60+
}
61+
62+
TEST_CASE("createVectorLinspaceInterval handles large step size", "[createVectorLinspaceInterval]") {
63+
auto result = createVectorLinspaceInterval<int>(5, 0, 100);
64+
std::vector<int> expected = {0, 100, 200, 300, 400};
65+
REQUIRE_THAT(result, Catch::Matchers::Equals(expected));
66+
}
67+
68+
TEST_CASE("createVectorLinspaceInterval handles large length", "[createVectorLinspaceInterval]") {
69+
auto result = createVectorLinspaceInterval<double>(100, 0.0, 0.1);
70+
std::vector<double> expected(100);
71+
std::iota(expected.begin(), expected.end(), 0.0);
72+
std::transform(expected.begin(), expected.end(), expected.begin(),
73+
[](double value) { return value * 0.1; });
74+
REQUIRE_THAT(result, Catch::Matchers::Equals(expected));
75+
}
76+
77+
TEST_CASE("createVectorLinspaceInterval produces correct output for small spacing with 10 elements", "[createVectorLinspaceInterval]") {
78+
auto result = createVectorLinspaceInterval<double>(10, 0.0, 0.001);
79+
std::vector<double> expected = {0.0, 0.001, 0.002, 0.003, 0.004, 0.005, 0.006, 0.007, 0.008, 0.009};
80+
// Check that the sizes of the vectors are the same
81+
REQUIRE(result.size() == expected.size());
82+
83+
// Check that each result is within the specified tolerance of the expected value
84+
for (size_t i = 0; i < result.size(); ++i) {
85+
REQUIRE_THAT(result[i], Catch::Matchers::WithinAbs(expected[i], tol));
86+
}
87+
}
88+
89+
TEST_CASE("createVectorLinspaceInterval produces correct output for small spacing", "[createVectorLinspaceInterval]") {
90+
std::vector<double> expected(100);
91+
std::iota(expected.begin(), expected.end(), 0.0);
92+
std::transform(expected.begin(), expected.end(), expected.begin(),
93+
[](double value) { return value * 0.001; });
94+
auto result = createVectorLinspaceInterval<double>(100, 0.0, 0.001);
95+
// Check that the sizes of the vectors are the same
96+
REQUIRE(result.size() == expected.size());
97+
98+
// Check that each result is within the specified tolerance of the expected value
99+
for (size_t i = 0; i < result.size(); ++i) {
100+
REQUIRE_THAT(result[i], Catch::Matchers::WithinAbs(expected[i], tol));
101+
}
102+
}

OpenSim/Common/XsensDataReader.cpp

Lines changed: 5 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
#include <fstream>
2+
#include "CommonUtilities.h"
23
#include "Simbody.h"
34
#include "Exception.h"
45
#include "FileAdapter.h"
@@ -32,8 +33,6 @@ XsensDataReader::extendRead(const std::string& folderName) const {
3233
SimTK::Matrix_<SimTK::Vec3> linearAccelerationData{ last_size, n_imus };
3334
SimTK::Matrix_<SimTK::Vec3> magneticHeadingData{ last_size, n_imus };
3435
SimTK::Matrix_<SimTK::Vec3> angularVelocityData{ last_size, n_imus };
35-
std::vector<double> times;
36-
times.resize(last_size);
3736

3837
std::string prefix = _settings.get_trial_prefix();
3938
std::map<std::string, std::string> headersKeyValuePairs;
@@ -96,8 +95,6 @@ XsensDataReader::extendRead(const std::string& folderName) const {
9695
// For all tables, will create row, stitch values from different files then append,time and timestep
9796
// are based on the first file
9897
bool done = false;
99-
double time = 0.0;
100-
double timeIncrement = 1 / dataRate;
10198
int rowNumber = 0;
10299
while (!done){
103100
// Make vectors one per table
@@ -109,7 +106,7 @@ XsensDataReader::extendRead(const std::string& folderName) const {
109106
magneto_row_vector{ n_imus, SimTK::Vec3(SimTK::NaN) };
110107
TimeSeriesTableVec3::RowVector
111108
gyro_row_vector{ n_imus, SimTK::Vec3(SimTK::NaN) };
112-
// Cycle through the filles collating values
109+
// Cycle through the files collating values
113110
int imu_index = 0;
114111
for (std::vector<std::ifstream*>::iterator it = imuStreams.begin();
115112
it != imuStreams.end();
@@ -146,20 +143,17 @@ XsensDataReader::extendRead(const std::string& folderName) const {
146143
if (done)
147144
break;
148145
// append to the tables
149-
times[rowNumber] = time;
150146
if (foundLinearAccelerationData)
151147
linearAccelerationData[rowNumber] = accel_row_vector;
152148
if (foundMagneticHeadingData)
153149
magneticHeadingData[rowNumber] = magneto_row_vector;
154150
if (foundAngularVelocityData)
155151
angularVelocityData[rowNumber] = gyro_row_vector;
156152
rotationsData[rowNumber] = orientation_row_vector;
157-
time += timeIncrement;
158153
rowNumber++;
159154
if (std::remainder(rowNumber, last_size) == 0) {
160155
// resize all Data/Matrices, double the size while keeping data
161156
int newSize = last_size*2;
162-
times.resize(newSize);
163157
// Repeat for Data matrices in use
164158
if (foundLinearAccelerationData) linearAccelerationData.resizeKeep(newSize, n_imus);
165159
if (foundMagneticHeadingData) magneticHeadingData.resizeKeep(newSize, n_imus);
@@ -168,8 +162,9 @@ XsensDataReader::extendRead(const std::string& folderName) const {
168162
last_size = newSize;
169163
}
170164
}
171-
// Trim Matrices in use to actual data and move into tables
172-
times.resize(rowNumber);
165+
const double timeIncrement = 1.0 / dataRate;
166+
const auto times = createVectorLinspaceInterval(rowNumber, 0.0, timeIncrement);
167+
173168
// Repeat for Data matrices in use and create Tables from them or size 0 for empty
174169
linearAccelerationData.resizeKeep(foundLinearAccelerationData? rowNumber : 0,
175170
n_imus);

0 commit comments

Comments
 (0)