Skip to content

Commit 69cf835

Browse files
alexbeattie42alexbeattie42
committed
Improvements for XsensDataReader
1 parent 142b6dc commit 69cf835

File tree

5 files changed

+581
-316
lines changed

5 files changed

+581
-316
lines changed

Diff for: ‎OpenSim/Common/FileAdapter.h

+4-1
Original file line numberDiff line numberDiff line change
@@ -166,9 +166,12 @@ class TableMissingHeader : public Exception {
166166
public:
167167
TableMissingHeader(const std::string& file,
168168
size_t line,
169-
const std::string& func) :
169+
const std::string& func,
170+
const std::string& message = "") :
170171
Exception(file, line, func) {
171172
std::string msg = "Table does not have metadata for 'header'.";
173+
if(!message.empty())
174+
msg += " " + message;
172175

173176
addMessage(msg);
174177
}

Diff for: ‎OpenSim/Common/Test/testXsensDataReader.cpp

+118-107
Original file line numberDiff line numberDiff line change
@@ -37,113 +37,124 @@ PacketCounter<tab>SampleTimeFine<tab>Year<tab>Month<tab>Day<tab>Second<tab>UTC_N
3737
06951<tab><tab><tab><tab><tab><tab><tab><tab><tab><tab><tab><tab><tab><tab>2.657654<tab>5.012634<tab>-7.581414<tab>0.392058<tab>0.353193<tab>-0.712047<tab>-0.114258<tab>-0.155518<tab>0.913330<tab>0.387756<tab>0.877453<tab>0.282349<tab>0.716718<tab>-0.479624<tab>0.506238<tab>0.579621<tab>0.006068<tab>-0.814863
3838
*/
3939

40-
TEST_CASE("XsensDataReader")
41-
{
42-
XsensDataReaderSettings readerSettings;
43-
std::vector<std::string> imu_names{ "shank", "thigh" };
44-
std::vector<std::string> file_names{ "000_00B421AF", "000_00B4227B" };
45-
// Programmatically add items to Map, write to xml
46-
for (int index = 0; index < (int)imu_names.size(); ++index) {
47-
ExperimentalSensor nextSensor(file_names[index], imu_names[index]);
48-
readerSettings.append_ExperimentalSensors(nextSensor);
49-
}
50-
readerSettings.updProperty_trial_prefix() = "MT_012005D6_031-";
51-
readerSettings.print("reader2xml.xml");
52-
// read xml we wrote into a new XsensDataReader to readTrial
53-
XsensDataReader reconstructFromXML(XsensDataReaderSettings("reader2xml.xml"));
54-
DataAdapter::OutputTables tables = reconstructFromXML.read("./");
55-
std::string folder = readerSettings.get_data_folder();
56-
std::string trial = readerSettings.get_trial_prefix();
57-
// Write tables to sto files
58-
// Accelerations
59-
const TimeSeriesTableVec3& accelTableTyped =
60-
reconstructFromXML.getLinearAccelerationsTable(tables);
61-
STOFileAdapterVec3::write(accelTableTyped, folder + trial + "accelerations.sto");
62-
const SimTK::RowVectorView_<SimTK::Vec3>& rvv = accelTableTyped.getRowAtIndex(0);
63-
SimTK::Vec3 fromTable = accelTableTyped.getRowAtIndex(0)[0];
64-
SimTK::Vec3 fromFile = SimTK::Vec3{ 3.030769, 5.254238, -7.714005 };
65-
double tolerance = SimTK::Eps;
66-
ASSERT_EQUAL(fromTable, fromFile, tolerance);
67-
// test last row as well to make sure all data is read correctly,
68-
// size is as expected
69-
size_t numRows = accelTableTyped.getIndependentColumn().size();
70-
fromTable = accelTableTyped.getRowAtIndex(numRows - 1)[0];
71-
fromFile = SimTK::Vec3{ 2.657654, 5.012634, -7.581414 };
72-
ASSERT_EQUAL(fromTable, fromFile, tolerance);
73-
// Magenometer
74-
const TimeSeriesTableVec3& magTableTyped =
75-
reconstructFromXML.getMagneticHeadingTable(tables);
76-
STOFileAdapterVec3::write(magTableTyped, folder + trial + "magnetometers.sto");
77-
fromTable = magTableTyped.getRowAtIndex(0)[0];
78-
fromFile = SimTK::Vec3{ -0.045410, -0.266113, 0.897217 };
79-
ASSERT_EQUAL(fromTable, fromFile, tolerance);
80-
// Gyro
81-
const TimeSeriesTableVec3& gyroTableTyped =
82-
reconstructFromXML.getAngularVelocityTable(tables);
83-
STOFileAdapterVec3::write(gyroTableTyped, folder + trial + "gyros.sto");
84-
fromTable = gyroTableTyped.getRowAtIndex(0)[0];
85-
fromFile = SimTK::Vec3{ 0.005991, -0.032133, 0.022713 };
86-
ASSERT_EQUAL(fromTable, fromFile, tolerance);
87-
// Orientation
88-
const TimeSeriesTableQuaternion& quatTableTyped =
89-
reconstructFromXML.getOrientationsTable(tables);
90-
STOFileAdapterQuaternion::write(quatTableTyped, folder + trial + "quaternions.sto");
91-
SimTK::Quaternion quat = quatTableTyped.getRowAtIndex(0)[1];
92-
// Convert back to orientation matrix and compare with gold standard data in file
93-
//-0.444898<tab>0.895542<tab>0.008444
94-
// 0.333934<tab>0.157132<tab>0.929407
95-
// 0.830996<tab>0.416311<tab>-0.368959
96-
std::vector<double> rotationVectorInFile{ -0.444898,0.895542,0.008444,
97-
0.333934,0.157132,0.929407,
98-
0.830996,0.416311,-0.368959 };
99-
SimTK::Rotation rot;
100-
rot.setRotationFromQuaternion(quat);
101-
tolerance = 1e-6; // empirically determined due to conversion back and forth
102-
for (int i = 0; i < 3; ++i){
103-
for (int j = 0; j < 3; ++j) {
104-
// Matrix is stored column major
105-
ASSERT_EQUAL(rotationVectorInFile[i * 3 + j], rot[j][i], tolerance);
40+
TEST_CASE("XsensDataReader") {
41+
SECTION("Full Set Basic Processing") {
42+
XsensDataReaderSettings readerSettings;
43+
std::vector<std::string> imu_names{"shank", "thigh"};
44+
std::vector<std::string> file_names{"000_00B421AF", "000_00B4227B"};
45+
// Programmatically add items to Map, write to xml
46+
for (int index = 0; index < (int)imu_names.size(); ++index) {
47+
ExperimentalSensor nextSensor(file_names[index], imu_names[index]);
48+
readerSettings.append_ExperimentalSensors(nextSensor);
49+
}
50+
readerSettings.updProperty_trial_prefix() = "MT_012005D6_031-";
51+
readerSettings.print("reader2xml.xml");
52+
// read xml we wrote into a new XsensDataReader to readTrial
53+
XsensDataReader reconstructFromXML(
54+
XsensDataReaderSettings("reader2xml.xml"));
55+
DataAdapter::OutputTables tables = reconstructFromXML.read("./");
56+
std::string folder = readerSettings.get_data_folder();
57+
std::string trial = readerSettings.get_trial_prefix();
58+
// Write tables to sto files
59+
// Accelerations
60+
const TimeSeriesTableVec3& accelTableTyped =
61+
reconstructFromXML.getLinearAccelerationsTable(tables);
62+
STOFileAdapterVec3::write(
63+
accelTableTyped, folder + trial + "accelerations.sto");
64+
const SimTK::RowVectorView_<SimTK::Vec3>& rvv =
65+
accelTableTyped.getRowAtIndex(0);
66+
SimTK::Vec3 fromTable = accelTableTyped.getRowAtIndex(0)[0];
67+
SimTK::Vec3 fromFile = SimTK::Vec3{3.030769, 5.254238, -7.714005};
68+
double tolerance = SimTK::Eps;
69+
ASSERT_EQUAL(fromTable, fromFile, tolerance);
70+
// test last row as well to make sure all data is read correctly,
71+
// size is as expected
72+
size_t numRows = accelTableTyped.getIndependentColumn().size();
73+
fromTable = accelTableTyped.getRowAtIndex(numRows - 1)[0];
74+
fromFile = SimTK::Vec3{2.657654, 5.012634, -7.581414};
75+
ASSERT_EQUAL(fromTable, fromFile, tolerance);
76+
// Magenometer
77+
const TimeSeriesTableVec3& magTableTyped =
78+
reconstructFromXML.getMagneticHeadingTable(tables);
79+
STOFileAdapterVec3::write(
80+
magTableTyped, folder + trial + "magnetometers.sto");
81+
fromTable = magTableTyped.getRowAtIndex(0)[0];
82+
fromFile = SimTK::Vec3{-0.045410, -0.266113, 0.897217};
83+
ASSERT_EQUAL(fromTable, fromFile, tolerance);
84+
// Gyro
85+
const TimeSeriesTableVec3& gyroTableTyped =
86+
reconstructFromXML.getAngularVelocityTable(tables);
87+
STOFileAdapterVec3::write(gyroTableTyped, folder + trial + "gyros.sto");
88+
fromTable = gyroTableTyped.getRowAtIndex(0)[0];
89+
fromFile = SimTK::Vec3{0.005991, -0.032133, 0.022713};
90+
ASSERT_EQUAL(fromTable, fromFile, tolerance);
91+
// Orientation
92+
const TimeSeriesTableQuaternion& quatTableTyped =
93+
reconstructFromXML.getOrientationsTable(tables);
94+
STOFileAdapterQuaternion::write(
95+
quatTableTyped, folder + trial + "quaternions.sto");
96+
SimTK::Quaternion quat = quatTableTyped.getRowAtIndex(0)[1];
97+
// Convert back to orientation matrix and compare with gold standard
98+
// data in file
99+
//-0.444898<tab>0.895542<tab>0.008444
100+
// 0.333934<tab>0.157132<tab>0.929407
101+
// 0.830996<tab>0.416311<tab>-0.368959
102+
std::vector<double> rotationVectorInFile{-0.444898, 0.895542, 0.008444,
103+
0.333934, 0.157132, 0.929407, 0.830996, 0.416311, -0.368959};
104+
SimTK::Rotation rot;
105+
rot.setRotationFromQuaternion(quat);
106+
tolerance =
107+
1e-6; // empirically determined due to conversion back and forth
108+
for (int i = 0; i < 3; ++i) {
109+
for (int j = 0; j < 3; ++j) {
110+
// Matrix is stored column major
111+
ASSERT_EQUAL(
112+
rotationVectorInFile[i * 3 + j], rot[j][i], tolerance);
113+
}
106114
}
107115
}
108-
// Now test the case where only orientation data is available, rest is missing
109-
XsensDataReaderSettings readOrientationsOnly;
110-
ExperimentalSensor nextSensor("000_00B421ED", "test");
111-
readOrientationsOnly.append_ExperimentalSensors(nextSensor);
112-
readOrientationsOnly.updProperty_trial_prefix() = "MT_012005D6-000_sit_to_stand-";
113-
DataAdapter::OutputTables tables2 =
114-
XsensDataReader(readOrientationsOnly).read("./");
115-
const TimeSeriesTableVec3& accelTable2 =
116-
reconstructFromXML.getLinearAccelerationsTable(tables2);
117-
ASSERT(accelTable2.getNumRows() ==0);
118-
ASSERT(tables2.at(IMUDataReader::MagneticHeading)->getNumRows() == 0);
119-
ASSERT(tables2.at(IMUDataReader::AngularVelocity)->getNumRows() == 0);
120-
// Now a file with extra comment in header as reported by user, has 3 rows
121-
XsensDataReaderSettings readerSettings3;
122-
ExperimentalSensor nextSensor2("test1_00B421E6", "test");
123-
readerSettings3.append_ExperimentalSensors(nextSensor2);
124-
DataAdapter::OutputTables tables3 =
125-
XsensDataReader(readerSettings3).read("./");
126-
auto accelTable3 = tables3.at(XsensDataReader::LinearAccelerations);
127-
ASSERT(accelTable3->getNumRows() == 3);
128-
//
129-
// Now a file with latest format
130-
XsensDataReaderSettings readerSettings4;
131-
ExperimentalSensor nextSensor4("MT_01200454_000-000_00B40DE4", "test");
132-
readerSettings4.append_ExperimentalSensors(nextSensor4);
133-
DataAdapter::OutputTables tables4 =
134-
XsensDataReader(readerSettings4).read("./");
135-
auto accelTable4 = tables4.at(XsensDataReader::LinearAccelerations);
136-
ASSERT(accelTable4->getNumRows() == 4);
137-
138-
// Now a file exported from MTManager2020.0.2
139-
XsensDataReaderSettings readerSettings5;
140-
ExperimentalSensor nextSensor5("MT_01200312-002-000_00B474BA", "test");
141-
readerSettings5.append_ExperimentalSensors(nextSensor5);
142-
XsensDataReader reader5(readerSettings5);
143-
DataAdapter::OutputTables tables5 = reader5.read("./");
144-
auto accelTable5 = tables5.at(XsensDataReader::LinearAccelerations);
145-
ASSERT(accelTable5->getNumRows() == 5);
146-
//const TimeSeriesTableQuaternion& quatTable5 =
147-
// reader5.getOrientationsTable(tables5);
148-
// STOFileAdapterQuaternion::write(quatTable5, "2020-0-2-quaternions.sto");
116+
SECTION("Only Orientation") {
117+
// Now test the case where only orientation data is available, rest is
118+
// missing
119+
XsensDataReaderSettings readOrientationsOnly;
120+
ExperimentalSensor nextSensor("000_00B421ED", "test");
121+
readOrientationsOnly.append_ExperimentalSensors(nextSensor);
122+
readOrientationsOnly.updProperty_trial_prefix() =
123+
"MT_012005D6-000_sit_to_stand-";
124+
DataAdapter::OutputTables tables2 =
125+
XsensDataReader(readOrientationsOnly).read("./");
126+
const auto accelTable2 = tables2.at(XsensDataReader::LinearAccelerations);
127+
ASSERT(accelTable2->getNumRows() == 0);
128+
ASSERT(tables2.at(IMUDataReader::MagneticHeading)->getNumRows() == 0);
129+
ASSERT(tables2.at(IMUDataReader::AngularVelocity)->getNumRows() == 0);
130+
// Now a file with extra comment in header as reported by user, has 3
131+
// rows
132+
XsensDataReaderSettings readerSettings3;
133+
ExperimentalSensor nextSensor2("test1_00B421E6", "test");
134+
readerSettings3.append_ExperimentalSensors(nextSensor2);
135+
DataAdapter::OutputTables tables3 =
136+
XsensDataReader(readerSettings3).read("./");
137+
auto accelTable3 = tables3.at(XsensDataReader::LinearAccelerations);
138+
ASSERT(accelTable3->getNumRows() == 3);
139+
}
140+
SECTION("Exported with Current (2022-2025) Format") {
141+
// Now a file with latest format
142+
XsensDataReaderSettings readerSettings4;
143+
ExperimentalSensor nextSensor4("MT_01200454_000-000_00B40DE4", "test");
144+
readerSettings4.append_ExperimentalSensors(nextSensor4);
145+
DataAdapter::OutputTables tables4 =
146+
XsensDataReader(readerSettings4).read("./");
147+
auto accelTable4 = tables4.at(XsensDataReader::LinearAccelerations);
148+
ASSERT(accelTable4->getNumRows() == 4);
149+
}
150+
SECTION("Exported from MTManager2020.0.2") {
151+
// Now a file exported from MTManager2020.0.2
152+
XsensDataReaderSettings readerSettings5;
153+
ExperimentalSensor nextSensor5("MT_01200312-002-000_00B474BA", "test");
154+
readerSettings5.append_ExperimentalSensors(nextSensor5);
155+
XsensDataReader reader5(readerSettings5);
156+
DataAdapter::OutputTables tables5 = reader5.read("./");
157+
auto accelTable5 = tables5.at(XsensDataReader::LinearAccelerations);
158+
ASSERT(accelTable5->getNumRows() == 5);
159+
}
149160
}

0 commit comments

Comments
 (0)