diff --git a/CHANGELOG.md b/CHANGELOG.md index aba594e239..00c1f541d8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,9 @@ This is not a comprehensive list of changes but rather a hand-curated collection v4.6 ===== - Move BoolLike class outside of template Array class for the `VS 17.14` linker (#4081) +- Improvements for the `XsensDataReader`. Add a configuration option to XSensDataReaderSettings to specify a known data rate (sampling frequency). Automatically detect the delimiter used in the file. Support the new Xsens export rotations formats (Rotation Matrix, Quaternion, or Euler angles) values from Xsens files. Update the parser to handle the path separator for data_folder and fix a memory leak. Verify integrity and uniformity of all files. Add tests with additional new and old Xsens formats. (#4063) + + v4.5.2 diff --git a/OpenSim/Common/CommonUtilities.cpp b/OpenSim/Common/CommonUtilities.cpp index 5c64140c98..7235c258d0 100644 --- a/OpenSim/Common/CommonUtilities.cpp +++ b/OpenSim/Common/CommonUtilities.cpp @@ -31,6 +31,7 @@ #include #include #include +#include #include @@ -69,6 +70,32 @@ std::string OpenSim::getFormattedDateTime( return ss.str(); } +std::string OpenSim::detectDelimiter( + const std::string& input, const std::vector& delimiters) { + + std::unordered_map counts; + + // Count occurrences of common delimiters in the input string + std::transform(delimiters.begin(), delimiters.end(), + std::inserter(counts, counts.end()), [&input](const std::string& d) { + std::size_t count = 0; + std::size_t pos = 0; + // Find all occurrences of delimiter in input string + while ((pos = input.find(d, pos)) != std::string::npos) { + ++count; + pos += d.length(); // Move past the current delimiter + } + return std::pair(d, count); + }); + + // Find the delimiter with the highest frequency + auto maxElem = std::max_element(counts.begin(), counts.end(), + [](const auto& a, const auto& b) { return a.second < b.second; }); + + // If a delimiter is found, return it, otherwise return an empty string + return (maxElem != counts.end() && maxElem->second > 0) ? maxElem->first : ""; +} + SimTK::Vector OpenSim::createVectorLinspace( int length, double start, double end) { SimTK::Vector v(length); diff --git a/OpenSim/Common/CommonUtilities.h b/OpenSim/Common/CommonUtilities.h index 8e1bf8d2ca..bb17799d84 100644 --- a/OpenSim/Common/CommonUtilities.h +++ b/OpenSim/Common/CommonUtilities.h @@ -27,14 +27,15 @@ #include "Assertion.h" #include #include +#include #include #include #include #include #include #include -#include #include +#include #include @@ -121,6 +122,25 @@ OSIMCOMMON_API SimTK::Vector createVector(std::initializer_list elements); #endif +/** + * @brief Detects the most likely string delimiter in an input string. + * + * This function identifies the most frequent delimiter from a predefined list + * of common delimiters based on occurrences in the input string. + * + * @param input The string to analyze. + * @param delimiters A vector of candidate delimiters. + * + * @return: The most likely delimiter (as an std::string), + * or an empty string if none was found. + * + * @note Supports single-character delimiters. + */ + /// @ingroup commonutil +OSIMCOMMON_API +std::string detectDelimiter( + const std::string& input, const std::vector& delimiters); + /** * @brief Checks if the elements of a vector are uniformly spaced. * diff --git a/OpenSim/Common/FileAdapter.h b/OpenSim/Common/FileAdapter.h index 92c0834bbb..1ba9e53d92 100644 --- a/OpenSim/Common/FileAdapter.h +++ b/OpenSim/Common/FileAdapter.h @@ -166,9 +166,12 @@ class TableMissingHeader : public Exception { public: TableMissingHeader(const std::string& file, size_t line, - const std::string& func) : + const std::string& func, + const std::string& message = "") : Exception(file, line, func) { std::string msg = "Table does not have metadata for 'header'."; + if(!message.empty()) + msg += " " + message; addMessage(msg); } diff --git a/OpenSim/Common/Test/testCommonUtilities.cpp b/OpenSim/Common/Test/testCommonUtilities.cpp index 0182ad9510..5a0c091203 100644 --- a/OpenSim/Common/Test/testCommonUtilities.cpp +++ b/OpenSim/Common/Test/testCommonUtilities.cpp @@ -261,3 +261,49 @@ TEST_CASE("isUniform tests with createVectorLinspace(SimTK::Vector) and createVe REQUIRE_THAT(result_simtk.second, Catch::Matchers::WithinAbs(rate, tol)); } } + +TEST_CASE("detectDelimiter") { + const std::vector delimiters = {",", ";", "|", "\t", ":", " "}; + SECTION("Comma") { + std::string input = "a,b,c,d"; + auto delim = detectDelimiter(input, delimiters); + REQUIRE(delim == ","); + } + + SECTION("Pipe") { + std::string input = "a|b|c|d"; + auto delim = detectDelimiter(input, delimiters); + REQUIRE(delim == "|"); + } + + SECTION("Tab") { + std::string input = "a\tb\tc\td"; + auto delim = detectDelimiter(input, delimiters); + REQUIRE(delim == "\t"); + } + + SECTION("Semicolon") { + std::string input = "a;b;c;d"; + auto delim = detectDelimiter(input, delimiters); + REQUIRE(delim == ";"); + } + + SECTION("Space") { + std::string input = "a b c d"; + auto delim = detectDelimiter(input, delimiters); + REQUIRE(delim == " "); + } + + SECTION("No Valid Delimiter") { + std::string input = "abcd"; + auto delim = detectDelimiter(input, delimiters); + REQUIRE(delim == ""); + } + + SECTION("Delimiter Exclusion") { + std::vector small_delimiters = {",", ";"}; + std::string input = "a|b|c|d"; + auto delim = detectDelimiter(input, small_delimiters); + REQUIRE(delim == ""); + } +} diff --git a/OpenSim/Common/Test/testXsensDataReader.cpp b/OpenSim/Common/Test/testXsensDataReader.cpp index 07e914452d..4044f58f9e 100644 --- a/OpenSim/Common/Test/testXsensDataReader.cpp +++ b/OpenSim/Common/Test/testXsensDataReader.cpp @@ -37,113 +37,297 @@ PacketCounterSampleTimeFineYearMonthDaySecondUTC_N 069512.6576545.012634-7.5814140.3920580.353193-0.712047-0.114258-0.1555180.9133300.3877560.8774530.2823490.716718-0.4796240.5062380.5796210.006068-0.814863 */ -TEST_CASE("XsensDataReader") -{ - XsensDataReaderSettings readerSettings; - std::vector imu_names{ "shank", "thigh" }; - std::vector file_names{ "000_00B421AF", "000_00B4227B" }; - // Programmatically add items to Map, write to xml - for (int index = 0; index < (int)imu_names.size(); ++index) { - ExperimentalSensor nextSensor(file_names[index], imu_names[index]); - readerSettings.append_ExperimentalSensors(nextSensor); +TEST_CASE("XsensDataReader") { + SECTION("Full Set Basic Processing") { + XsensDataReaderSettings readerSettings; + std::vector imu_names{"shank", "thigh"}; + std::vector file_names{"000_00B421AF", "000_00B4227B"}; + // Programmatically add items to Map, write to xml + for (int index = 0; index < (int)imu_names.size(); ++index) { + ExperimentalSensor nextSensor(file_names[index], imu_names[index]); + readerSettings.append_ExperimentalSensors(nextSensor); + } + readerSettings.updProperty_trial_prefix() = "MT_012005D6_031-"; + readerSettings.print("reader2xml.xml"); + // read xml we wrote into a new XsensDataReader to readTrial + XsensDataReader reconstructFromXML( + XsensDataReaderSettings("reader2xml.xml")); + DataAdapter::OutputTables tables = reconstructFromXML.read("./"); + std::string folder = readerSettings.get_data_folder(); + std::string trial = readerSettings.get_trial_prefix(); + // Write tables to sto files + // Accelerations + const TimeSeriesTableVec3& accelTableTyped = + reconstructFromXML.getLinearAccelerationsTable(tables); + STOFileAdapterVec3::write( + accelTableTyped, folder + trial + "accelerations.sto"); + const SimTK::RowVectorView_& rvv = + accelTableTyped.getRowAtIndex(0); + SimTK::Vec3 fromTable = accelTableTyped.getRowAtIndex(0)[0]; + SimTK::Vec3 fromFile = SimTK::Vec3{3.030769, 5.254238, -7.714005}; + double tolerance = SimTK::Eps; + REQUIRE(fromTable.size() == fromFile.size()); + for (int i = 0; i < fromTable.size(); ++i) { + REQUIRE_THAT(fromTable[i], + Catch::Matchers::WithinAbs(fromFile[i], tolerance)); + } + // test last row as well to make sure all data is read correctly, + // size is as expected + int numRows = accelTableTyped.getIndependentColumn().size(); + fromTable = accelTableTyped.getRowAtIndex(numRows - 1)[0]; + fromFile = SimTK::Vec3{2.657654, 5.012634, -7.581414}; + REQUIRE(fromTable.size() == fromFile.size()); + for (int i = 0; i < fromTable.size(); ++i) { + REQUIRE_THAT(fromTable[i], + Catch::Matchers::WithinAbs(fromFile[i], tolerance)); + } + // Magenometer + const TimeSeriesTableVec3& magTableTyped = + reconstructFromXML.getMagneticHeadingTable(tables); + STOFileAdapterVec3::write( + magTableTyped, folder + trial + "magnetometers.sto"); + fromTable = magTableTyped.getRowAtIndex(0)[0]; + fromFile = SimTK::Vec3{-0.045410, -0.266113, 0.897217}; + REQUIRE(fromTable.size() == fromFile.size()); + for (int i = 0; i < fromTable.size(); ++i) { + REQUIRE_THAT(fromTable[i], + Catch::Matchers::WithinAbs(fromFile[i], tolerance)); + } + // ASSERT_EQUAL(fromTable, fromFile, tolerance); + // Gyro + const TimeSeriesTableVec3& gyroTableTyped = + reconstructFromXML.getAngularVelocityTable(tables); + STOFileAdapterVec3::write(gyroTableTyped, folder + trial + "gyros.sto"); + fromTable = gyroTableTyped.getRowAtIndex(0)[0]; + fromFile = SimTK::Vec3{0.005991, -0.032133, 0.022713}; + REQUIRE(fromTable.size() == fromFile.size()); + for (int i = 0; i < fromTable.size(); ++i) { + REQUIRE_THAT(fromTable[i], + Catch::Matchers::WithinAbs(fromFile[i], tolerance)); + } + // Orientation + const TimeSeriesTableQuaternion& quatTableTyped = + reconstructFromXML.getOrientationsTable(tables); + STOFileAdapterQuaternion::write( + quatTableTyped, folder + trial + "quaternions.sto"); + SimTK::Quaternion quat = quatTableTyped.getRowAtIndex(0)[1]; + // Convert back to orientation matrix and compare with gold standard + // data in file + //-0.4448980.8955420.008444 + // 0.3339340.1571320.929407 + // 0.8309960.416311-0.368959 + std::vector rotationVectorInFile{-0.444898, 0.895542, 0.008444, + 0.333934, 0.157132, 0.929407, 0.830996, 0.416311, -0.368959}; + SimTK::Rotation rot; + rot.setRotationFromQuaternion(quat); + tolerance = + 1e-6; // empirically determined due to conversion back and forth + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + // Matrix is stored column major + REQUIRE_THAT(rotationVectorInFile[i * 3 + j], + Catch::Matchers::WithinAbs(rot[j][i], tolerance)); + } + } + } + SECTION("Only Orientation") { + // Now test the case where only orientation data is available, rest is + // missing + XsensDataReaderSettings readOrientationsOnly; + ExperimentalSensor nextSensor("000_00B421ED", "test"); + readOrientationsOnly.append_ExperimentalSensors(nextSensor); + readOrientationsOnly.updProperty_trial_prefix() = + "MT_012005D6-000_sit_to_stand-"; + DataAdapter::OutputTables tables2 = + XsensDataReader(readOrientationsOnly).read("./"); + const auto accelTable2 = + tables2.at(XsensDataReader::LinearAccelerations); + CHECK(accelTable2->getNumRows() == 0); + CHECK(tables2.at(IMUDataReader::MagneticHeading)->getNumRows() == 0); + CHECK(tables2.at(IMUDataReader::AngularVelocity)->getNumRows() == 0); + // Now a file with extra comment in header as reported by user, has 3 + // rows + XsensDataReaderSettings readerSettings3; + ExperimentalSensor nextSensor2("test1_00B421E6", "test"); + readerSettings3.append_ExperimentalSensors(nextSensor2); + DataAdapter::OutputTables tables3 = + XsensDataReader(readerSettings3).read("./"); + auto accelTable3 = tables3.at(XsensDataReader::LinearAccelerations); + CHECK(accelTable3->getNumRows() == 3); + } + SECTION("Exported with Current (2022-2025) Format") { + // Now a file with latest format + XsensDataReaderSettings readerSettings4; + ExperimentalSensor nextSensor4("MT_01200454_000-000_00B40DE4", "test"); + readerSettings4.append_ExperimentalSensors(nextSensor4); + DataAdapter::OutputTables tables4 = + XsensDataReader(readerSettings4).read("./"); + auto accelTable4 = tables4.at(XsensDataReader::LinearAccelerations); + CHECK(accelTable4->getNumRows() == 4); } - readerSettings.updProperty_trial_prefix() = "MT_012005D6_031-"; - readerSettings.print("reader2xml.xml"); - // read xml we wrote into a new XsensDataReader to readTrial - XsensDataReader reconstructFromXML(XsensDataReaderSettings("reader2xml.xml")); - DataAdapter::OutputTables tables = reconstructFromXML.read("./"); - std::string folder = readerSettings.get_data_folder(); - std::string trial = readerSettings.get_trial_prefix(); - // Write tables to sto files - // Accelerations - const TimeSeriesTableVec3& accelTableTyped = - reconstructFromXML.getLinearAccelerationsTable(tables); - STOFileAdapterVec3::write(accelTableTyped, folder + trial + "accelerations.sto"); - const SimTK::RowVectorView_& rvv = accelTableTyped.getRowAtIndex(0); - SimTK::Vec3 fromTable = accelTableTyped.getRowAtIndex(0)[0]; - SimTK::Vec3 fromFile = SimTK::Vec3{ 3.030769, 5.254238, -7.714005 }; - double tolerance = SimTK::Eps; - ASSERT_EQUAL(fromTable, fromFile, tolerance); - // test last row as well to make sure all data is read correctly, - // size is as expected - size_t numRows = accelTableTyped.getIndependentColumn().size(); - fromTable = accelTableTyped.getRowAtIndex(numRows - 1)[0]; - fromFile = SimTK::Vec3{ 2.657654, 5.012634, -7.581414 }; - ASSERT_EQUAL(fromTable, fromFile, tolerance); - // Magenometer - const TimeSeriesTableVec3& magTableTyped = - reconstructFromXML.getMagneticHeadingTable(tables); - STOFileAdapterVec3::write(magTableTyped, folder + trial + "magnetometers.sto"); - fromTable = magTableTyped.getRowAtIndex(0)[0]; - fromFile = SimTK::Vec3{ -0.045410, -0.266113, 0.897217 }; - ASSERT_EQUAL(fromTable, fromFile, tolerance); - // Gyro - const TimeSeriesTableVec3& gyroTableTyped = - reconstructFromXML.getAngularVelocityTable(tables); - STOFileAdapterVec3::write(gyroTableTyped, folder + trial + "gyros.sto"); - fromTable = gyroTableTyped.getRowAtIndex(0)[0]; - fromFile = SimTK::Vec3{ 0.005991, -0.032133, 0.022713 }; - ASSERT_EQUAL(fromTable, fromFile, tolerance); - // Orientation - const TimeSeriesTableQuaternion& quatTableTyped = - reconstructFromXML.getOrientationsTable(tables); - STOFileAdapterQuaternion::write(quatTableTyped, folder + trial + "quaternions.sto"); - SimTK::Quaternion quat = quatTableTyped.getRowAtIndex(0)[1]; - // Convert back to orientation matrix and compare with gold standard data in file - //-0.4448980.8955420.008444 - // 0.3339340.1571320.929407 - // 0.8309960.416311-0.368959 - std::vector rotationVectorInFile{ -0.444898,0.895542,0.008444, - 0.333934,0.157132,0.929407, - 0.830996,0.416311,-0.368959 }; - SimTK::Rotation rot; - rot.setRotationFromQuaternion(quat); - tolerance = 1e-6; // empirically determined due to conversion back and forth - for (int i = 0; i < 3; ++i){ - for (int j = 0; j < 3; ++j) { - // Matrix is stored column major - ASSERT_EQUAL(rotationVectorInFile[i * 3 + j], rot[j][i], tolerance); + SECTION("Exported from MTManager2020.0.2") { + // Now a file exported from MTManager2020.0.2 + XsensDataReaderSettings readerSettings5; + ExperimentalSensor nextSensor5("MT_01200312-002-000_00B474BA", "test"); + readerSettings5.append_ExperimentalSensors(nextSensor5); + XsensDataReader reader5(readerSettings5); + DataAdapter::OutputTables tables5 = reader5.read("./"); + auto accelTable5 = tables5.at(XsensDataReader::LinearAccelerations); + CHECK(accelTable5->getNumRows() == 5); + } + SECTION("Exported from MTManager4.6") { + + XsensDataReaderSettings readerSettings; + const std::vector expSens = { + OpenSim::ExperimentalSensor("_00B42D4D", "femur_l_imu"), + OpenSim::ExperimentalSensor("_00B42D4E", "calcn_l_imu")}; + for (const auto& sensor : expSens) { + readerSettings.append_ExperimentalSensors(sensor); } + readerSettings.updProperty_trial_prefix() = "MT46_01-000"; + + XsensDataReader reader(readerSettings); + DataAdapter::OutputTables tables = reader.read("./"); + auto accelTable = tables.at(XsensDataReader::LinearAccelerations); + REQUIRE(accelTable->getNumRows() == 5); + + auto gyroscopeTable = tables.at(XsensDataReader::AngularVelocity); + REQUIRE(gyroscopeTable->getNumRows() == 5); + + auto orientationTable = tables.at(XsensDataReader::Orientations); + REQUIRE(orientationTable->getNumRows() == 5); + } +} + +TEST_CASE("XsensDataReader MT Manager 2022 Data Formats") { + SECTION("Acceleration Only") { + XsensDataReaderSettings readerSettings; + readerSettings.append_ExperimentalSensors( + OpenSim::ExperimentalSensor("_00B42D4B", "head_imu")); + readerSettings.updProperty_trial_prefix() = "MT2022_ACC"; + readerSettings.set_sampling_rate(60.0); + + XsensDataReader reader(readerSettings); + DataAdapter::OutputTables tables = reader.read("./"); + auto accelTable = tables.at(XsensDataReader::LinearAccelerations); + REQUIRE(accelTable->getNumRows() == 5); + const TimeSeriesTableVec3& accelTableTyped = + reader.getLinearAccelerationsTable(tables); + REQUIRE(accelTableTyped.getTableMetaData() + .getValueForKey("DataRate") + .getValue() == "60.000000"); + + const auto& times = accelTableTyped.getIndependentColumn(); + double lastTimestamp = times[times.size() - 1]; // last timestamp + double expectedLastTimestamp = + (accelTableTyped.getNumRows() - 1) / 60.0; + REQUIRE_THAT(lastTimestamp, + Catch::Matchers::WithinAbs(expectedLastTimestamp, SimTK::Eps)); + } + SECTION("Acceleration, Gyroscope, All Rotation Formats, Tab separated, " + "NaN") { + XsensDataReaderSettings readerSettings; + readerSettings.append_ExperimentalSensors( + OpenSim::ExperimentalSensor("_00B42D4B", "head_imu")); + readerSettings.updProperty_trial_prefix() = + "MT2022_ACC_GYRO_ROT_ALL_TAB_NAN"; + + XsensDataReader reader(readerSettings); + DataAdapter::OutputTables tables = reader.read("./"); + auto accelTable = tables.at(XsensDataReader::LinearAccelerations); + REQUIRE(accelTable->getNumRows() == 5); + + auto gyroscopeTable = tables.at(XsensDataReader::AngularVelocity); + REQUIRE(gyroscopeTable->getNumRows() == 5); + + auto orientationTable = tables.at(XsensDataReader::Orientations); + REQUIRE(orientationTable->getNumRows() == 5); + + const TimeSeriesTableVec3& accelTableTyped = + reader.getLinearAccelerationsTable(tables); + const SimTK::RowVectorView_& rvv = + accelTableTyped.getRowAtIndex(0); + SimTK::Vec3 fromTable = accelTableTyped.getRowAtIndex(0)[0]; + SimTK::Vec3 fromFile = SimTK::Vec3{SimTK::NaN, -0.040975, 9.922524}; + double tolerance = SimTK::Eps; + + REQUIRE(fromTable.size() == fromFile.size()); + REQUIRE(std::isnan(fromTable[0])); + REQUIRE_THAT(fromTable[1], + Catch::Matchers::WithinAbs(fromFile[1], tolerance)); + REQUIRE_THAT(fromTable[2], + Catch::Matchers::WithinAbs(fromFile[2], tolerance)); + } + SECTION("Acceleration, Gyroscope, Euler Angles, Tab separated") { + XsensDataReaderSettings readerSettings; + readerSettings.append_ExperimentalSensors( + OpenSim::ExperimentalSensor("_00B42D4F", "radius_r_imu")); + readerSettings.updProperty_trial_prefix() = + "MT2022_ACC_GYRO_ROT_EULER_TAB"; + + XsensDataReader reader(readerSettings); + DataAdapter::OutputTables tables = reader.read("./"); + auto accelTable = tables.at(XsensDataReader::LinearAccelerations); + REQUIRE(accelTable->getNumRows() == 5); + + auto gyroscopeTable = tables.at(XsensDataReader::AngularVelocity); + REQUIRE(gyroscopeTable->getNumRows() == 5); + + auto orientationTable = tables.at(XsensDataReader::Orientations); + REQUIRE(orientationTable->getNumRows() == 5); + } + SECTION("Acceleration, Gyroscope, Rotation Matrix, Comma separated") { + XsensDataReaderSettings readerSettings; + readerSettings.append_ExperimentalSensors( + OpenSim::ExperimentalSensor("_00B42D4B", "head_imu")); + readerSettings.updProperty_trial_prefix() = + "MT2022_ACC_GYRO_ROT_MAT_COMMA"; + + XsensDataReader reader(readerSettings); + DataAdapter::OutputTables tables = reader.read("./"); + auto accelTable = tables.at(XsensDataReader::LinearAccelerations); + REQUIRE(accelTable->getNumRows() == 5); + + auto gyroscopeTable = tables.at(XsensDataReader::AngularVelocity); + REQUIRE(gyroscopeTable->getNumRows() == 5); + + auto orientationTable = tables.at(XsensDataReader::Orientations); + REQUIRE(orientationTable->getNumRows() == 5); + } + SECTION("Acceleration, Gyroscope, Rotation Matrix, Semicolon separated") { + XsensDataReaderSettings readerSettings; + readerSettings.append_ExperimentalSensors( + OpenSim::ExperimentalSensor("_00B42D4B", "head_imu")); + readerSettings.updProperty_trial_prefix() = + "MT2022_ACC_GYRO_ROT_MAT_SEMICOLON"; + + XsensDataReader reader(readerSettings); + DataAdapter::OutputTables tables = reader.read("./"); + auto accelTable = tables.at(XsensDataReader::LinearAccelerations); + REQUIRE(accelTable->getNumRows() == 5); + + auto gyroscopeTable = tables.at(XsensDataReader::AngularVelocity); + REQUIRE(gyroscopeTable->getNumRows() == 5); + + auto orientationTable = tables.at(XsensDataReader::Orientations); + REQUIRE(orientationTable->getNumRows() == 5); + } + SECTION("Acceleration, Gyroscope, Rotation Matrix, Space separated") { + XsensDataReaderSettings readerSettings; + readerSettings.append_ExperimentalSensors( + OpenSim::ExperimentalSensor("_00B42D4B", "head_imu")); + readerSettings.updProperty_trial_prefix() = + "MT2022_ACC_GYRO_ROT_MAT_SPACE"; + + XsensDataReader reader(readerSettings); + DataAdapter::OutputTables tables = reader.read("./"); + auto accelTable = tables.at(XsensDataReader::LinearAccelerations); + REQUIRE(accelTable->getNumRows() == 5); + + auto gyroscopeTable = tables.at(XsensDataReader::AngularVelocity); + REQUIRE(gyroscopeTable->getNumRows() == 5); + + auto orientationTable = tables.at(XsensDataReader::Orientations); + REQUIRE(orientationTable->getNumRows() == 5); } - // Now test the case where only orientation data is available, rest is missing - XsensDataReaderSettings readOrientationsOnly; - ExperimentalSensor nextSensor("000_00B421ED", "test"); - readOrientationsOnly.append_ExperimentalSensors(nextSensor); - readOrientationsOnly.updProperty_trial_prefix() = "MT_012005D6-000_sit_to_stand-"; - DataAdapter::OutputTables tables2 = - XsensDataReader(readOrientationsOnly).read("./"); - const TimeSeriesTableVec3& accelTable2 = - reconstructFromXML.getLinearAccelerationsTable(tables2); - ASSERT(accelTable2.getNumRows() ==0); - ASSERT(tables2.at(IMUDataReader::MagneticHeading)->getNumRows() == 0); - ASSERT(tables2.at(IMUDataReader::AngularVelocity)->getNumRows() == 0); - // Now a file with extra comment in header as reported by user, has 3 rows - XsensDataReaderSettings readerSettings3; - ExperimentalSensor nextSensor2("test1_00B421E6", "test"); - readerSettings3.append_ExperimentalSensors(nextSensor2); - DataAdapter::OutputTables tables3 = - XsensDataReader(readerSettings3).read("./"); - auto accelTable3 = tables3.at(XsensDataReader::LinearAccelerations); - ASSERT(accelTable3->getNumRows() == 3); - // - // Now a file with latest format - XsensDataReaderSettings readerSettings4; - ExperimentalSensor nextSensor4("MT_01200454_000-000_00B40DE4", "test"); - readerSettings4.append_ExperimentalSensors(nextSensor4); - DataAdapter::OutputTables tables4 = - XsensDataReader(readerSettings4).read("./"); - auto accelTable4 = tables4.at(XsensDataReader::LinearAccelerations); - ASSERT(accelTable4->getNumRows() == 4); - - // Now a file exported from MTManager2020.0.2 - XsensDataReaderSettings readerSettings5; - ExperimentalSensor nextSensor5("MT_01200312-002-000_00B474BA", "test"); - readerSettings5.append_ExperimentalSensors(nextSensor5); - XsensDataReader reader5(readerSettings5); - DataAdapter::OutputTables tables5 = reader5.read("./"); - auto accelTable5 = tables5.at(XsensDataReader::LinearAccelerations); - ASSERT(accelTable5->getNumRows() == 5); - //const TimeSeriesTableQuaternion& quatTable5 = - // reader5.getOrientationsTable(tables5); - // STOFileAdapterQuaternion::write(quatTable5, "2020-0-2-quaternions.sto"); } diff --git a/OpenSim/Common/XsensDataReader.cpp b/OpenSim/Common/XsensDataReader.cpp index 599e99eb2f..d29dc58dec 100644 --- a/OpenSim/Common/XsensDataReader.cpp +++ b/OpenSim/Common/XsensDataReader.cpp @@ -1,10 +1,17 @@ -#include +#include "XsensDataReader.h" + #include "CommonUtilities.h" -#include "Simbody.h" #include "Exception.h" #include "FileAdapter.h" +#include "Simbody.h" #include "TimeSeriesTable.h" -#include "XsensDataReader.h" +#include +#include +#include +#include +#include +#include +#include namespace OpenSim { @@ -12,182 +19,396 @@ XsensDataReader* XsensDataReader::clone() const { return new XsensDataReader{*this}; } -DataAdapter::OutputTables -XsensDataReader::extendRead(const std::string& folderName) const { +typedef struct XsensDataReader::XsensIMU { + std::string name; + size_t data_size; + std::string rotation_format; + std::map comments; + std::map header; + std::vector acc; + std::vector gyr; + std::vector mag; + std::vector quat; + std::vector euler; + std::vector rot_mat; +} XsensIMU; + +DataAdapter::OutputTables XsensDataReader::extendRead( + const std::string& folderName) const { + + // Valid delimiters for Xsens files + const std::vector delimiters = {",", ";", "|", "\t", ":", " "}; + + // Valid headers for data file, header vector is in order + const std::unordered_map> imu_h = { + {"acc", {"Acc_X", "Acc_Y", "Acc_Z"}}, + {"gyr", {"Gyr_X", "Gyr_Y", "Gyr_Z"}}, + {"mag", {"Mag_X", "Mag_Y", "Mag_Z"}}, + {"quat", {"Quat_q0", "Quat_q1", "Quat_q2", "Quat_q3"}}, + {"euler", {"Roll", "Pitch", "Yaw"}}, + {"rot_mat", {"Mat[1][1]", "Mat[2][1]", "Mat[3][1]", "Mat[1][2]", + "Mat[2][2]", "Mat[3][2]", "Mat[1][3]", + "Mat[2][3]", "Mat[3][3]"}}}; - std::vector imuStreams; - std::vector labels; // files specified by prefix + file name exist - double dataRate = SimTK::NaN; - - int accIndex = -1; - int gyroIndex = -1; - int magIndex = -1; - int rotationsIndex = -1; - - int n_imus = _settings.getProperty_ExperimentalSensors().size(); - int last_size = 1024; - // Will read data into pre-allocated Matrices in-memory rather than appendRow - // on the fly to avoid the overhead of - SimTK::Matrix_ rotationsData{ last_size, n_imus }; - SimTK::Matrix_ linearAccelerationData{ last_size, n_imus }; - SimTK::Matrix_ magneticHeadingData{ last_size, n_imus }; - SimTK::Matrix_ angularVelocityData{ last_size, n_imus }; - - std::string prefix = _settings.get_trial_prefix(); - std::map headersKeyValuePairs; + double dataRate = _settings.get_sampling_rate(); + const std::string extension = _settings.get_trial_extension(); + const std::string prefix = _settings.get_trial_prefix(); + + const int n_imus = _settings.getProperty_ExperimentalSensors().size(); + + // Prepare all the file handles + // format: name, filename + std::vector> imuFiles; for (int index = 0; index < n_imus; ++index) { std::string prefix = _settings.get_trial_prefix(); - const ExperimentalSensor& nextItem = _settings.get_ExperimentalSensors(index); - auto fileName = folderName + prefix + nextItem.getName() +".txt"; - auto* nextStream = new std::ifstream{ fileName }; - OPENSIM_THROW_IF(!nextStream->good(), - FileDoesNotExist, - fileName); - // Add imu name to labels - labels.push_back(nextItem.get_name_in_model()); - // Add corresponding stream to imuStreams - imuStreams.push_back(nextStream); - - // Skip lines to get to data - std::string line; - auto commentLine = true; - std::getline(*nextStream, line); - auto isCommentLine = [](std::string aline) { - return aline.substr(0, 2) == "//"; - }; - std::vector tokens; - while (commentLine) { - // Comment lines of arbitrary number on the form // "key":"value" - tokens =FileAdapter::tokenize(line.substr(2), ":"); //Skip leading 2 chars tokenize on ':' - // Will populate map from first file/imu only, assume they all have same format - if (tokens.size() == 2 && index == 0) { - // Put values in map - headersKeyValuePairs[tokens[0]] = tokens[1]; - } - std::getline(*nextStream, line); - commentLine = isCommentLine(line); - } - // Find indices for Acc_{X,Y,Z}, Gyr_{X,Y,Z}, - // Mag_{X,Y,Z}, Mat on first non-comment line - tokens = FileAdapter::tokenize(line, "\t"); - - if (accIndex == -1) accIndex = find_index(tokens, "Acc_X"); - if (gyroIndex == -1) gyroIndex = find_index(tokens, "Gyr_X"); - if (magIndex == -1) magIndex = find_index(tokens, "Mag_X"); - if (rotationsIndex == -1) rotationsIndex = find_index(tokens, "Mat[1][1]"); + const ExperimentalSensor& nextItem = + _settings.get_ExperimentalSensors(index); + const std::filesystem::path fileName = + std::filesystem::path(folderName) / + (prefix + nextItem.getName() + extension); + + // Add corresponding pair to imuStreams + const auto& p = + std::make_pair(nextItem.get_name_in_model(), fileName.string()); + imuFiles.push_back(p); } - // Compute data rate based on key/value pair if available - std::map::iterator it = - headersKeyValuePairs.find("Update Rate"); - if (it != headersKeyValuePairs.end()) - dataRate = OpenSim::IO::stod(it->second); - else - dataRate = 40.0; // Need confirmation from XSens as later files don't specify rate - // internally keep track of what data was found in input files - bool foundLinearAccelerationData = (accIndex != -1); - bool foundMagneticHeadingData = (magIndex != -1); - bool foundAngularVelocityData = (gyroIndex != -1); - - // If no Orientation data is available we'll abort completely - OPENSIM_THROW_IF((rotationsIndex == -1), TableMissingHeader); - - // For all tables, will create row, stitch values from different files then append,time and timestep - // are based on the first file - bool done = false; - int rowNumber = 0; - while (!done){ - // Make vectors one per table - TimeSeriesTableQuaternion::RowVector - orientation_row_vector{ n_imus, SimTK::Quaternion() }; - TimeSeriesTableVec3::RowVector - accel_row_vector{ n_imus, SimTK::Vec3(SimTK::NaN) }; - TimeSeriesTableVec3::RowVector - magneto_row_vector{ n_imus, SimTK::Vec3(SimTK::NaN) }; - TimeSeriesTableVec3::RowVector - gyro_row_vector{ n_imus, SimTK::Vec3(SimTK::NaN) }; - // Cycle through the files collating values - int imu_index = 0; - for (std::vector::iterator it = imuStreams.begin(); - it != imuStreams.end(); - ++it, ++imu_index) { - std::ifstream* nextStream = *it; - // parse gyro info from imuStream - std::vector nextRow = FileAdapter::getNextLine(*nextStream, "\t\r"); - if (nextRow.empty()) { - done = true; - break; - } - if (foundLinearAccelerationData) - accel_row_vector[imu_index] = SimTK::Vec3(OpenSim::IO::stod(nextRow[accIndex]), - OpenSim::IO::stod(nextRow[accIndex + 1]), OpenSim::IO::stod(nextRow[accIndex + 2])); - if (foundMagneticHeadingData) - magneto_row_vector[imu_index] = SimTK::Vec3(OpenSim::IO::stod(nextRow[magIndex]), - OpenSim::IO::stod(nextRow[magIndex + 1]), OpenSim::IO::stod(nextRow[magIndex + 2])); - if (foundAngularVelocityData) - gyro_row_vector[imu_index] = SimTK::Vec3(OpenSim::IO::stod(nextRow[gyroIndex]), - OpenSim::IO::stod(nextRow[gyroIndex + 1]), OpenSim::IO::stod(nextRow[gyroIndex + 2])); - // Create Mat33 then convert into Quaternion - SimTK::Mat33 imu_matrix{ SimTK::NaN }; - int matrix_entry_index = 0; - for (int mcol = 0; mcol < 3; mcol++) { - for (int mrow = 0; mrow < 3; mrow++) { - imu_matrix[mrow][mcol] = OpenSim::IO::stod(nextRow[rotationsIndex + matrix_entry_index]); - matrix_entry_index++; + + std::vector imus(imuFiles.size()); + + // Read each IMU file + std::transform(imuFiles.begin(), imuFiles.end(), imus.begin(), + [&imu_h, &delimiters](const auto& p) { + // Open File + const std::string& fileName = p.second; + std::ifstream stream{fileName}; + OPENSIM_THROW_IF(!stream.good(), FileDoesNotExist, fileName); + + // Build IMU Sensor struct + XsensIMU imu; + imu.name = p.first; + + const auto isCommentLine = [](std::string aline) { + return aline.substr(0, 2) == "//"; + }; + std::string line; + std::vector tokens; + while (std::getline(stream, line) && isCommentLine(line)) { + // Comment lines of arbitrary number on the form + // // "key":"value" + // Skip leading 2 chars tokenize on ':' + tokens = FileAdapter::tokenize(line.substr(2), ":"); + if (tokens.size() == 2) { + imu.comments.insert({tokens[0], tokens[1]}); + } } - } - // Convert imu_matrix to Quaternion - SimTK::Rotation imu_rotation{ imu_matrix }; - orientation_row_vector[imu_index] = imu_rotation.convertRotationToQuaternion(); - } - if (done) - break; - // append to the tables - if (foundLinearAccelerationData) - linearAccelerationData[rowNumber] = accel_row_vector; - if (foundMagneticHeadingData) - magneticHeadingData[rowNumber] = magneto_row_vector; - if (foundAngularVelocityData) - angularVelocityData[rowNumber] = gyro_row_vector; - rotationsData[rowNumber] = orientation_row_vector; - rowNumber++; - if (std::remainder(rowNumber, last_size) == 0) { - // resize all Data/Matrices, double the size while keeping data - int newSize = last_size*2; - // Repeat for Data matrices in use - if (foundLinearAccelerationData) linearAccelerationData.resizeKeep(newSize, n_imus); - if (foundMagneticHeadingData) magneticHeadingData.resizeKeep(newSize, n_imus); - if (foundAngularVelocityData) angularVelocityData.resizeKeep(newSize, n_imus); - rotationsData.resizeKeep(newSize, n_imus); - last_size = newSize; - } - } - const double timeIncrement = 1.0 / dataRate; - const auto times = createVectorLinspaceInterval(rowNumber, 0.0, timeIncrement); - - // Repeat for Data matrices in use and create Tables from them or size 0 for empty - linearAccelerationData.resizeKeep(foundLinearAccelerationData? rowNumber : 0, - n_imus); - magneticHeadingData.resizeKeep(foundMagneticHeadingData? rowNumber : 0, - n_imus); - angularVelocityData.resizeKeep(foundAngularVelocityData? rowNumber :0, - n_imus); - rotationsData.resizeKeep(rowNumber, n_imus); + const auto delim = OpenSim::detectDelimiter(line,delimiters); + OPENSIM_THROW_IF(delim == "", TableMissingHeader, + "No delimiter found for: " + imu.name + + " Please ensure that the data file is valid!"); + const std::string delimiter = delim; + // This is the header + // Find indices for Acc_{X,Y,Z}, Gyr_{X,Y,Z}, + // Mag_{X,Y,Z}, Mat on first non-comment line + tokens = FileAdapter::tokenize(line, delimiter); + // Process each header in the line + for (const auto& token : tokens) { + size_t tokenIndex = std::distance(tokens.begin(), + std::find(tokens.begin(), tokens.end(), token)); + imu.header.insert({token, tokenIndex}); + } + + const auto is_group_complete = + [&](const std::vector& search_set, + const std::map& + found_headers) -> bool { + return std::all_of(search_set.begin(), search_set.end(), + [&](const auto& p) { + return found_headers.find(p) != + found_headers.end(); + }); + }; + + const auto& acc_h = imu_h.at("acc"); + const auto& gyr_h = imu_h.at("gyr"); + const auto& mag_h = imu_h.at("mag"); + const auto& quat_h = imu_h.at("quat"); + const auto& euler_h = imu_h.at("euler"); + const auto& rot_mat_h = imu_h.at("rot_mat"); + + // internally keep track of what data was found in input + const bool found_linear_acceleration = + is_group_complete(acc_h, imu.header); + const bool found_magnetic_heading = + is_group_complete(mag_h, imu.header); + const bool found_angular_velocity = + is_group_complete(gyr_h, imu.header); + const bool found_quat = is_group_complete(quat_h, imu.header); + const bool found_euler = is_group_complete(euler_h, imu.header); + const bool found_rot_mat = + is_group_complete(rot_mat_h, imu.header); + + // Determine the rotation format to be used in the order: + // 1. Quaternion => (best for OpenSim) + // 2. Rotation Matrix + // 3. Euler Angles + if (found_quat) { + imu.rotation_format = "rot_quaternion"; + } else if (found_rot_mat) { + imu.rotation_format = "rot_matrix"; + } else if (found_euler) { + imu.rotation_format = "rot_euler"; + } + + std::vector next_row; + size_t row_count = 0; + while (!(next_row = FileAdapter::getNextLine( + stream, delimiter + "\r")) + .empty()) { + // parse info from imuStream + if (found_linear_acceleration) + imu.acc.push_back(SimTK::Vec3( + OpenSim::IO::stod( + next_row[imu.header.at(acc_h[0])]), + OpenSim::IO::stod( + next_row[imu.header.at(acc_h[1])]), + OpenSim::IO::stod( + next_row[imu.header.at(acc_h[2])]))); + if (found_magnetic_heading) + imu.mag.push_back(SimTK::Vec3( + OpenSim::IO::stod( + next_row[imu.header.at(mag_h[0])]), + OpenSim::IO::stod( + next_row[imu.header.at(mag_h[1])]), + OpenSim::IO::stod( + next_row[imu.header.at(mag_h[2])]))); + if (found_angular_velocity) + imu.gyr.push_back(SimTK::Vec3( + OpenSim::IO::stod( + next_row[imu.header.at(gyr_h[0])]), + OpenSim::IO::stod( + next_row[imu.header.at(gyr_h[1])]), + OpenSim::IO::stod( + next_row[imu.header.at(gyr_h[2])]))); + if (found_quat) { + imu.quat.push_back(SimTK::Quaternion( + OpenSim::IO::stod( + next_row[imu.header.at(quat_h[0])]), + OpenSim::IO::stod( + next_row[imu.header.at(quat_h[1])]), + OpenSim::IO::stod( + next_row[imu.header.at(quat_h[2])]), + OpenSim::IO::stod( + next_row[imu.header.at(quat_h[3])]))); + } + if (found_euler) { + imu.euler.push_back(SimTK::Rotation( + SimTK::BodyOrSpaceType::BodyRotationSequence, + OpenSim::IO::stod( + next_row[imu.header.at(euler_h[0])]), + SimTK::XAxis, + OpenSim::IO::stod( + next_row[imu.header.at(euler_h[1])]), + SimTK::YAxis, + OpenSim::IO::stod( + next_row[imu.header.at(euler_h[2])]), + SimTK::ZAxis)); + } + if (found_rot_mat) { + // Create Mat33 then convert into Quaternion + SimTK::Mat33 imu_matrix{SimTK::NaN}; + imu_matrix[0][0] = OpenSim::IO::stod( + next_row[imu.header.at(rot_mat_h[0])]); + imu_matrix[1][0] = OpenSim::IO::stod( + next_row[imu.header.at(rot_mat_h[1])]); + imu_matrix[2][0] = OpenSim::IO::stod( + next_row[imu.header.at(rot_mat_h[2])]); + + imu_matrix[0][1] = OpenSim::IO::stod( + next_row[imu.header.at(rot_mat_h[3])]); + imu_matrix[1][1] = OpenSim::IO::stod( + next_row[imu.header.at(rot_mat_h[4])]); + imu_matrix[2][1] = OpenSim::IO::stod( + next_row[imu.header.at(rot_mat_h[5])]); + + imu_matrix[0][2] = OpenSim::IO::stod( + next_row[imu.header.at(rot_mat_h[6])]); + imu_matrix[1][2] = OpenSim::IO::stod( + next_row[imu.header.at(rot_mat_h[7])]); + imu_matrix[2][2] = OpenSim::IO::stod( + next_row[imu.header.at(rot_mat_h[8])]); + + imu.rot_mat.push_back(imu_matrix); + } + row_count++; + } + imu.data_size = row_count; + return imu; + }); + + // Ensure all files have the same size, update rate, and rotation + // format + std::vector line_lengths(imus.size()); + std::vector update_rates(imus.size()); + std::vector rotation_formats(imus.size()); + + std::transform(imus.begin(), imus.end(), line_lengths.begin(), + [](const auto& imu) { return imu.data_size; }); + + std::transform(imus.begin(), imus.end(), update_rates.begin(), + [&dataRate](const auto& imu) { + const auto it = imu.comments.find("Update Rate"); + return (it != imu.comments.end()) + ? OpenSim::IO::stod(it->second) + : dataRate; + }); + + std::transform(imus.begin(), imus.end(), rotation_formats.begin(), + [](const auto& imu) { return imu.rotation_format; }); + + // Check for uniformity + OPENSIM_THROW_IF( + std::adjacent_find(line_lengths.begin(), line_lengths.end(), + std::not_equal_to<>()) != line_lengths.end(), + IOError, + "All data files in the trial do not have the same line length!"); + + OPENSIM_THROW_IF( + std::adjacent_find(update_rates.begin(), update_rates.end(), + std::not_equal_to<>()) != update_rates.end(), + IOError, + "All data files in the trial do not have the same update rate!"); + + OPENSIM_THROW_IF( + std::adjacent_find(rotation_formats.begin(), rotation_formats.end(), + std::not_equal_to<>()) != rotation_formats.end(), + IOError, + "All data files in the trial do not have the same rotation " + "format!"); + + // These have all been checked for uniformity + const size_t n_lines = line_lengths[0]; + const double sampling_rate = update_rates[0]; + const std::string rotation_format = rotation_formats[0]; + + OPENSIM_THROW_IF( + (n_lines > static_cast(std::numeric_limits::max())), + IOError, "Too many lines present in the data files!"); + // Will read data into pre-allocated Matrices in-memory rather than + // appendRow on the fly to avoid the overhead of + SimTK::Matrix_ rotationsData{ + static_cast(n_lines), n_imus}; + SimTK::Matrix_ linearAccelerationData{ + static_cast(n_lines), n_imus}; + SimTK::Matrix_ magneticHeadingData{ + static_cast(n_lines), n_imus}; + SimTK::Matrix_ angularVelocityData{ + static_cast(n_lines), n_imus}; + + const bool has_acc = std::all_of(imus.begin(), imus.end(), + [&n_lines](const auto& imu) { return imu.acc.size() == n_lines; }); + const bool has_gyr = std::all_of(imus.begin(), imus.end(), + [&n_lines](const auto& imu) { return imu.gyr.size() == n_lines; }); + const bool has_mag = std::all_of(imus.begin(), imus.end(), + [&n_lines](const auto& imu) { return imu.mag.size() == n_lines; }); + + // We use a vector of row indices to transform over the rows (i.e., the time + // steps) + std::vector row_indices(n_lines); + std::iota(row_indices.begin(), row_indices.end(), + 0); // Fill with 0, 1, 2, ..., n_lines-1 + + // For all tables, will create the row by stitching values from all read + // files + std::transform(row_indices.begin(), row_indices.end(), row_indices.begin(), + [&imus, &n_imus, &rotation_format, &n_lines, &has_acc, &has_gyr, + &has_mag, &linearAccelerationData, &magneticHeadingData, + &angularVelocityData, &rotationsData](int j) { + TimeSeriesTableQuaternion::RowVector orientation_row_vector( + n_imus, SimTK::Quaternion()); + TimeSeriesTableVec3::RowVector accel_row_vector( + n_imus, SimTK::Vec3(SimTK::NaN)); + TimeSeriesTableVec3::RowVector magneto_row_vector( + n_imus, SimTK::Vec3(SimTK::NaN)); + TimeSeriesTableVec3::RowVector gyro_row_vector( + n_imus, SimTK::Vec3(SimTK::NaN)); + const bool has_quat = std::all_of( + imus.begin(), imus.end(), [&n_lines](const auto& imu) { + return imu.quat.size() == n_lines; + }); + const bool has_euler = std::all_of( + imus.begin(), imus.end(), [&n_lines](const auto& imu) { + return imu.euler.size() == n_lines; + }); + const bool has_rot_mat = std::all_of( + imus.begin(), imus.end(), [&n_lines](const auto& imu) { + return imu.rot_mat.size() == n_lines; + }); + if (has_acc) { + std::transform(imus.begin(), imus.end(), + std::begin(accel_row_vector), + [j](const auto& imu) { return imu.acc[j]; }); + linearAccelerationData[j] = accel_row_vector; + } + + if (has_mag) { + std::transform(imus.begin(), imus.end(), + std::begin(magneto_row_vector), + [j](const auto& imu) { return imu.mag[j]; }); + magneticHeadingData[j] = magneto_row_vector; + } + + if (has_gyr) { + std::transform(imus.begin(), imus.end(), + std::begin(gyro_row_vector), + [j](const auto& imu) { return imu.gyr[j]; }); + angularVelocityData[j] = gyro_row_vector; + } + + if (rotation_format == "rot_quaternion" && has_quat) { + std::transform(imus.begin(), imus.end(), + std::begin(orientation_row_vector), + [j](const auto& imu) { return imu.quat[j]; }); + } else if (rotation_format == "rot_euler" && has_euler) { + std::transform(imus.begin(), imus.end(), + std::begin(orientation_row_vector), + [j](const auto& imu) { + return imu.euler[j] + .convertRotationToQuaternion(); + }); + } else if (rotation_format == "rot_matrix" && has_rot_mat) { + std::transform(imus.begin(), imus.end(), + std::begin(orientation_row_vector), + [j](const auto& imu) { + SimTK::Rotation imu_rotation{imu.rot_mat[j]}; + return imu_rotation + .convertRotationToQuaternion(); + }); + } + // Store the rows in the matrices + rotationsData[j] = orientation_row_vector; + + return j; // Return the index required by std::transform + }); + const double timeIncrement = 1.0 / sampling_rate; + const auto times = createVectorLinspaceInterval( + static_cast(n_lines), 0.0, timeIncrement); + // Zero data matrices if the data is not found + if (!has_acc) { linearAccelerationData.resize(0, n_imus); } + if (!has_mag) { magneticHeadingData.resize(0, n_imus); } + if (!has_gyr) { angularVelocityData.resize(0, n_imus); } // Now create the tables from matrices - // Create 4 tables for Rotations, LinearAccelerations, AngularVelocity, MagneticHeading - // Tables could be empty if data is not present in file(s) - DataAdapter::OutputTables tables = createTablesFromMatrices(dataRate, labels, times, - rotationsData, linearAccelerationData, magneticHeadingData, angularVelocityData); + // Create 4 tables for Rotations, LinearAccelerations, + // AngularVelocity, MagneticHeading Tables could be empty if data is + // not present in file(s) + std::vector labels; + std::transform(imus.begin(), imus.end(), std::back_inserter(labels), + [](auto const& p) { return p.name; }); + DataAdapter::OutputTables tables = createTablesFromMatrices(sampling_rate, + labels, times, rotationsData, linearAccelerationData, + magneticHeadingData, angularVelocityData); return tables; } -int XsensDataReader::find_index(std::vector& tokens, const std::string& keyToMatch) { - int returnIndex = -1; - std::vector::iterator it = std::find(tokens.begin(), tokens.end(), keyToMatch); - if (it != tokens.end()) - returnIndex = static_cast(std::distance(tokens.begin(), it)); - return returnIndex; -} - -} +} // namespace OpenSim diff --git a/OpenSim/Common/XsensDataReader.h b/OpenSim/Common/XsensDataReader.h index 5175fbd916..5a4c1e5b64 100644 --- a/OpenSim/Common/XsensDataReader.h +++ b/OpenSim/Common/XsensDataReader.h @@ -23,16 +23,17 @@ * -------------------------------------------------------------------------- */ // For definition of Exceptions to be used -#include "Object.h" -#include "ExperimentalSensor.h" #include "DataAdapter.h" -#include "XsensDataReaderSettings.h" -#include "TimeSeriesTable.h" +#include "ExperimentalSensor.h" #include "IMUDataReader.h" +#include "Object.h" +#include "TimeSeriesTable.h" +#include "XsensDataReaderSettings.h" + /** @file -* This file defines class for reading data files from IMU maker Xsens and -* producing in memory tables to be consumed by the OpenSim tools/pipelines -*/ + * This file defines class for reading data files from IMU maker Xsens and + * producing in memory tables to be consumed by the OpenSim tools/pipelines + */ namespace OpenSim { @@ -50,53 +51,56 @@ class OSIMCOMMON_API XsensDataReader : public IMUDataReader { virtual ~XsensDataReader() = default; XsensDataReader* clone() const override; + protected: - /** Typically, Xsens can export a trial as one .mtb file (binary that we - can't parse) or as collection of ASCII text files that are tab delimited, - one per sensor. All of these files have a common prefix that indicates the + /** Typically, Xsens can export a trial as one .mtb file (binary that we + can't parse) or as collection of ASCII text files that are tab delimited, + one per sensor. All of these files have a common prefix that indicates the trial, and a suffix that indicates the sensor (fixed across trials). - Example: MT_012005D6_031-000_00B421AF.txt, MT_012005D6_031-000_00B4227B.txt + Example: MT_012005D6_031-000_00B421AF.txt, MT_012005D6_031-000_00B4227B.txt for trial MT_012005D6_031-000_and sensors 00B421AF, 00B4227B respectively. - The function below reads all files in the given folder name, with common - prefix (same trial). It produces a list of tables depending on the contents - of the files read. - - One table for Orientations, + The function below reads all files in the given folder name, with common + prefix (same trial). It produces a list of tables depending on the contents + of the files read. + - One table for Orientations, - one table for LinearAccelerations - - one table for MagneticHeading data, - - one table for AngularVelocity data. - If data is missing, an empty table is returned. + - one table for MagneticHeading data, + - one table for AngularVelocity data. + If data is missing, an empty table is returned. */ - DataAdapter::OutputTables extendRead(const std::string& folderName) const override; + DataAdapter::OutputTables extendRead( + const std::string& folderName) const override; /** Implements writing functionality, not implemented. */ virtual void extendWrite(const DataAdapter::InputTables& tables, - const std::string& sinkName) const override {}; + const std::string& sinkName) const override {}; + public: /** - * Method to get const reference to the internal XsensDataReaderSettings object - * maintained by this reader. + * Method to get const reference to the internal XsensDataReaderSettings + * object maintained by this reader. */ - const XsensDataReaderSettings& getSettings() const { - return _settings; - } - /** - * Method to get writable reference to the internal XsensDataReaderSettings object - * maintained by this reader, to allow modification after construction. - */ - XsensDataReaderSettings& updSettings() { - return _settings; - } - private: + const XsensDataReaderSettings& getSettings() const { return _settings; } /** - * Find index of searchString in tokens + * Method to get writable reference to the internal XsensDataReaderSettings + * object maintained by this reader, to allow modification after + * construction. */ - static int find_index(std::vector& tokens, const std::string& searchString); + XsensDataReaderSettings& updSettings() { return _settings; } + +private: /** - * This data member encapsulates all the serializable settings for the Reader; + * This data member encapsulates all the serializable settings for + * the Reader; */ XsensDataReaderSettings _settings; + + /** + * This data member encapsulates all the data for a single Xsens IMU + */ + struct XsensIMU; }; -} // OpenSim namespace +} // namespace OpenSim #endif // OPENSIM_XSENS_DATA_READER_H_ diff --git a/OpenSim/Common/XsensDataReaderSettings.h b/OpenSim/Common/XsensDataReaderSettings.h index ab1fae35be..7992546f47 100644 --- a/OpenSim/Common/XsensDataReaderSettings.h +++ b/OpenSim/Common/XsensDataReaderSettings.h @@ -41,6 +41,17 @@ OpenSim_DECLARE_CONCRETE_OBJECT(XsensDataReaderSettings, Object); "Name of folder containing data files."); OpenSim_DECLARE_PROPERTY(trial_prefix, std::string, "Name of trial (Common prefix of txt files representing trial)."); + OpenSim_DECLARE_PROPERTY(trial_extension, std::string, + "File extension for the trial files. Defaults to \".txt\""); + OpenSim_DECLARE_PROPERTY(sampling_rate, double, + "Sampling Rate (frequency in Hz) for the trials. Xsens MT Manager sets this value when the data is recorded. " + "This parser defaults to 40Hz if no value is provided. " + "Newer versions of the Xsens software do not specify the recording sampling rate. " + "If you know the sampling rate of your data and it is not included in the header of the IMUs, input the value here. " + "The default behavior for MTw Awinda sensors (and possibly others) is to set the sampling rate " + "at one value less than the maximum (e.g., 40 Hz for 11-20 sensors)." + "This value is used in calculating the time interval (1/frequency) for the resultant tables. " + "See issue (#3956) for details."); OpenSim_DECLARE_LIST_PROPERTY(ExperimentalSensors, ExperimentalSensor, "List of Experimental sensors and desired associated names in resulting tables"); @@ -61,6 +72,8 @@ OpenSim_DECLARE_CONCRETE_OBJECT(XsensDataReaderSettings, Object); void constructProperties() { constructProperty_data_folder(""); constructProperty_trial_prefix(""); + constructProperty_trial_extension(".txt"); + constructProperty_sampling_rate(40.0); constructProperty_ExperimentalSensors(); } }; diff --git a/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_00B42D4B.txt b/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_00B42D4B.txt new file mode 100755 index 0000000000..7210dfaaea --- /dev/null +++ b/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_00B42D4B.txt @@ -0,0 +1,18 @@ +// General information: +// MT Manager version: 2022.2.0 +// XDA version: 2022.2.0 build 7381 rev 124627 built on 2023-07-19 +// Device information: +// DeviceId: 00B42D4B +// ProductCode: MTW2-3A7G6 +// Firmware Version: 4.4.0 +// Hardware Version: 1.1.0 +// Device settings: +// Filter Profile: human(46.1) +// Option Flags: Orientation Smoother Disabled, Position/Velocity Smoother Disabled, Continuous Zero Rotation Update Disabled, AHS Disabled, ICC Disabled +// Coordinate system: ENU +PacketCounter SampleTimeFine Year Month Day Second Acc_X Acc_Y Acc_Z +51400 NaN NaN NaN NaN NaN 0.074225 -0.040975 9.922524 +51401 NaN NaN NaN NaN NaN 0.057993 0.007234 9.938693 +51402 NaN NaN NaN NaN NaN 0.040451 0.014429 9.928879 +51403 NaN NaN NaN NaN NaN 0.045482 -0.009840 9.967645 +51404 NaN NaN NaN NaN NaN 0.061840 -0.013683 9.956912 diff --git a/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_GYRO_ROT_ALL_TAB_NAN_00B42D4B.txt b/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_GYRO_ROT_ALL_TAB_NAN_00B42D4B.txt new file mode 100755 index 0000000000..3e1896414d --- /dev/null +++ b/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_GYRO_ROT_ALL_TAB_NAN_00B42D4B.txt @@ -0,0 +1,18 @@ +// General information: +// MT Manager version: 2022.2.0 +// XDA version: 2022.2.0 build 7381 rev 124627 built on 2023-07-19 +// Device information: +// DeviceId: 00B42D4B +// ProductCode: MTW2-3A7G6 +// Firmware Version: 4.4.0 +// Hardware Version: 1.1.0 +// Device settings: +// Filter Profile: human(46.1) +// Option Flags: Orientation Smoother Disabled, Position/Velocity Smoother Disabled, Continuous Zero Rotation Update Disabled, AHS Disabled, ICC Disabled +// Coordinate system: ENU +PacketCounter SampleTimeFine Year Month Day Second Acc_X Acc_Y Acc_Z Gyr_X Gyr_Y Gyr_Z Quat_q0 Quat_q1 Quat_q2 Quat_q3 Roll Pitch Yaw Mat[1][1] Mat[2][1] Mat[3][1] Mat[1][2] Mat[2][2] Mat[3][2] Mat[1][3] Mat[2][3] Mat[3][3] +51400 NaN NaN NaN NaN NaN NaN -0.040975 9.922524 0.003078 0.004280 0.002706 -0.529130 0.004539 0.002454 0.848525 -0.036609 -0.590193 -116.105418 -0.440001 -0.897938 0.010301 0.897983 -0.440030 -0.000639 0.005106 0.008969 0.999947 +51401 NaN NaN NaN NaN NaN 0.057993 0.007234 9.938693 0.001590 0.004310 0.002149 -0.529132 0.004542 0.002450 0.848524 -0.037216 -0.590159 -116.105209 -0.439997 -0.897940 0.010300 0.897984 -0.440027 -0.000650 0.005116 0.008963 0.999947 +51402 NaN NaN NaN NaN NaN 0.040451 0.014429 9.928879 0.003244 0.003104 0.002185 -0.529135 0.004544 0.002463 0.848522 -0.036075 -0.591142 -116.104805 -0.439991 -0.897943 0.010317 0.897988 -0.440020 -0.000630 0.005105 0.008988 0.999947 +51403 NaN NaN NaN NaN NaN 0.045482 -0.009840 9.967645 0.002354 0.003944 0.001659 -0.529133 0.004545 0.002465 0.848523 -0.035920 -0.591445 -116.105053 -0.439995 -0.897941 0.010322 0.897986 -0.440024 -0.000627 0.005105 0.008994 0.999947 +51404 NaN NaN NaN NaN NaN 0.061840 -0.013683 9.956912 0.002574 0.006178 -0.001358 -0.529111 0.004528 0.002460 0.848537 -0.035352 -0.589479 -116.108018 -0.440042 -0.897918 0.010288 0.897963 -0.440070 -0.000617 0.005082 0.008967 0.999947 diff --git a/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_GYRO_ROT_EULER_TAB_00B42D4F.txt b/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_GYRO_ROT_EULER_TAB_00B42D4F.txt new file mode 100755 index 0000000000..f7cb0c8e81 --- /dev/null +++ b/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_GYRO_ROT_EULER_TAB_00B42D4F.txt @@ -0,0 +1,18 @@ +// General information: +// MT Manager version: 2022.2.0 +// XDA version: 2022.2.0 build 7381 rev 124627 built on 2023-07-19 +// Device information: +// DeviceId: 00B42D4F +// ProductCode: MTW2-3A7G6 +// Firmware Version: 4.4.0 +// Hardware Version: 1.1.0 +// Device settings: +// Filter Profile: human(46.1) +// Option Flags: Orientation Smoother Disabled, Position/Velocity Smoother Disabled, Continuous Zero Rotation Update Disabled, AHS Disabled, ICC Disabled +// Coordinate system: ENU +PacketCounter SampleTimeFine Year Month Day Second Acc_X Acc_Y Acc_Z Gyr_X Gyr_Y Gyr_Z Roll Pitch Yaw +51400 NaN NaN NaN NaN NaN 0.069663 -0.012828 9.978389 0.001960 -0.000371 0.005926 -0.254385 0.210872 -137.485101 +51401 NaN NaN NaN NaN NaN 0.026610 0.010900 10.002911 0.003273 -0.000530 0.004671 -0.253865 0.210758 -137.485455 +51402 NaN NaN NaN NaN NaN 0.045357 0.036667 9.985499 0.003273 -0.000530 0.004671 -0.252935 0.210528 -137.485373 +51403 NaN NaN NaN NaN NaN 0.041319 0.010342 10.003773 0.003272 -0.000530 0.004671 -0.252083 0.210424 -137.485681 +51404 NaN NaN NaN NaN NaN 0.056388 0.007488 9.990137 0.005991 0.001733 0.001990 -0.248674 0.212412 -137.488454 diff --git a/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_GYRO_ROT_MAT_COMMA_00B42D4B.txt b/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_GYRO_ROT_MAT_COMMA_00B42D4B.txt new file mode 100755 index 0000000000..35288eca06 --- /dev/null +++ b/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_GYRO_ROT_MAT_COMMA_00B42D4B.txt @@ -0,0 +1,18 @@ +// General information: +// MT Manager version: 2022.2.0 +// XDA version: 2022.2.0 build 7381 rev 124627 built on 2023-07-19 +// Device information: +// DeviceId: 00B42D4B +// ProductCode: MTW2-3A7G6 +// Firmware Version: 4.4.0 +// Hardware Version: 1.1.0 +// Device settings: +// Filter Profile: human(46.1) +// Option Flags: Orientation Smoother Disabled, Position/Velocity Smoother Disabled, Continuous Zero Rotation Update Disabled, AHS Disabled, ICC Disabled +// Coordinate system: ENU +PacketCounter,SampleTimeFine,Year,Month,Day,Second,Acc_X,Acc_Y,Acc_Z,Gyr_X,Gyr_Y,Gyr_Z,Mat[1][1],Mat[2][1],Mat[3][1],Mat[1][2],Mat[2][2],Mat[3][2],Mat[1][3],Mat[2][3],Mat[3][3] +51400,NaN,NaN,NaN,NaN,NaN,0.074225,-0.040975,9.922524,0.003078,0.004280,0.002706,-0.440001,-0.897938,0.010301,0.897983,-0.440030,-0.000639,0.005106,0.008969,0.999947 +51401,NaN,NaN,NaN,NaN,NaN,0.057993,0.007234,9.938693,0.001590,0.004310,0.002149,-0.439997,-0.897940,0.010300,0.897984,-0.440027,-0.000650,0.005116,0.008963,0.999947 +51402,NaN,NaN,NaN,NaN,NaN,0.040451,0.014429,9.928879,0.003244,0.003104,0.002185,-0.439991,-0.897943,0.010317,0.897988,-0.440020,-0.000630,0.005105,0.008988,0.999947 +51403,NaN,NaN,NaN,NaN,NaN,0.045482,-0.009840,9.967645,0.002354,0.003944,0.001659,-0.439995,-0.897941,0.010322,0.897986,-0.440024,-0.000627,0.005105,0.008994,0.999947 +51404,NaN,NaN,NaN,NaN,NaN,0.061840,-0.013683,9.956912,0.002574,0.006178,-0.001358,-0.440042,-0.897918,0.010288,0.897963,-0.440070,-0.000617,0.005082,0.008967,0.999947 diff --git a/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_GYRO_ROT_MAT_SEMICOLON_00B42D4B.txt b/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_GYRO_ROT_MAT_SEMICOLON_00B42D4B.txt new file mode 100755 index 0000000000..10ed206eef --- /dev/null +++ b/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_GYRO_ROT_MAT_SEMICOLON_00B42D4B.txt @@ -0,0 +1,18 @@ +// General information: +// MT Manager version: 2022.2.0 +// XDA version: 2022.2.0 build 7381 rev 124627 built on 2023-07-19 +// Device information: +// DeviceId: 00B42D4B +// ProductCode: MTW2-3A7G6 +// Firmware Version: 4.4.0 +// Hardware Version: 1.1.0 +// Device settings: +// Filter Profile: human(46.1) +// Option Flags: Orientation Smoother Disabled, Position/Velocity Smoother Disabled, Continuous Zero Rotation Update Disabled, AHS Disabled, ICC Disabled +// Coordinate system: ENU +PacketCounter;SampleTimeFine;Year;Month;Day;Second;Acc_X;Acc_Y;Acc_Z;Gyr_X;Gyr_Y;Gyr_Z;Mat[1][1];Mat[2][1];Mat[3][1];Mat[1][2];Mat[2][2];Mat[3][2];Mat[1][3];Mat[2][3];Mat[3][3] +51400;;;;;;0.074225;-0.040975;9.922524;0.003078;0.004280;0.002706;-0.440001;-0.897938;0.010301;0.897983;-0.440030;-0.000639;0.005106;0.008969;0.999947 +51401;;;;;;0.057993;0.007234;9.938693;0.001590;0.004310;0.002149;-0.439997;-0.897940;0.010300;0.897984;-0.440027;-0.000650;0.005116;0.008963;0.999947 +51402;;;;;;0.040451;0.014429;9.928879;0.003244;0.003104;0.002185;-0.439991;-0.897943;0.010317;0.897988;-0.440020;-0.000630;0.005105;0.008988;0.999947 +51403;;;;;;0.045482;-0.009840;9.967645;0.002354;0.003944;0.001659;-0.439995;-0.897941;0.010322;0.897986;-0.440024;-0.000627;0.005105;0.008994;0.999947 +51404;;;;;;0.061840;-0.013683;9.956912;0.002574;0.006178;-0.001358;-0.440042;-0.897918;0.010288;0.897963;-0.440070;-0.000617;0.005082;0.008967;0.999947 diff --git a/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_GYRO_ROT_MAT_SPACE_00B42D4B.txt b/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_GYRO_ROT_MAT_SPACE_00B42D4B.txt new file mode 100755 index 0000000000..9b00cf2d35 --- /dev/null +++ b/OpenSim/Tests/shared/IMUData/Xsens/MT2022_ACC_GYRO_ROT_MAT_SPACE_00B42D4B.txt @@ -0,0 +1,18 @@ +// General information: +// MT Manager version: 2022.2.0 +// XDA version: 2022.2.0 build 7381 rev 124627 built on 2023-07-19 +// Device information: +// DeviceId: 00B42D4B +// ProductCode: MTW2-3A7G6 +// Firmware Version: 4.4.0 +// Hardware Version: 1.1.0 +// Device settings: +// Filter Profile: human(46.1) +// Option Flags: Orientation Smoother Disabled, Position/Velocity Smoother Disabled, Continuous Zero Rotation Update Disabled, AHS Disabled, ICC Disabled +// Coordinate system: ENU +PacketCounter SampleTimeFine Year Month Day Second Acc_X Acc_Y Acc_Z Gyr_X Gyr_Y Gyr_Z Mat[1][1] Mat[2][1] Mat[3][1] Mat[1][2] Mat[2][2] Mat[3][2] Mat[1][3] Mat[2][3] Mat[3][3] +51400 0.074225 -0.040975 9.922524 0.003078 0.004280 0.002706 -0.440001 -0.897938 0.010301 0.897983 -0.440030 -0.000639 0.005106 0.008969 0.999947 +51401 0.057993 0.007234 9.938693 0.001590 0.004310 0.002149 -0.439997 -0.897940 0.010300 0.897984 -0.440027 -0.000650 0.005116 0.008963 0.999947 +51402 0.040451 0.014429 9.928879 0.003244 0.003104 0.002185 -0.439991 -0.897943 0.010317 0.897988 -0.440020 -0.000630 0.005105 0.008988 0.999947 +51403 0.045482 -0.009840 9.967645 0.002354 0.003944 0.001659 -0.439995 -0.897941 0.010322 0.897986 -0.440024 -0.000627 0.005105 0.008994 0.999947 +51404 0.061840 -0.013683 9.956912 0.002574 0.006178 -0.001358 -0.440042 -0.897918 0.010288 0.897963 -0.440070 -0.000617 0.005082 0.008967 0.999947 diff --git a/OpenSim/Tests/shared/IMUData/Xsens/MT46_01-000_00B42D4D.txt b/OpenSim/Tests/shared/IMUData/Xsens/MT46_01-000_00B42D4D.txt new file mode 100755 index 0000000000..79f8ccc17b --- /dev/null +++ b/OpenSim/Tests/shared/IMUData/Xsens/MT46_01-000_00B42D4D.txt @@ -0,0 +1,10 @@ +// Start Time: Unknown +// Update Rate: 100.0Hz +// Filter Profile: human (46.1) +// Firmware Version: 4.4.0 +PacketCounter SampleTimeFine Year Month Day Second Acc_X Acc_Y Acc_Z Gyr_X Gyr_Y Gyr_Z Quat_q0 Quat_q1 Quat_q2 Quat_q3 +04097 9.645108 0.128211 -1.834658 0.024471 0.002012 0.007768 0.414280 -0.578860 -0.508282 -0.484709 +04098 9.622599 0.176720 -1.831047 0.022140 0.001999 0.005884 0.415598 -0.578145 -0.509091 -0.483584 +04099 9.647392 0.223973 -1.839441 0.019333 0.001887 -0.000289 0.415658 -0.578099 -0.509134 -0.483541 +04100 9.630348 0.249730 -1.810994 0.013515 0.001037 0.005899 0.415720 -0.578081 -0.509145 -0.483498 +04101 9.630374 0.249007 -1.810955 0.000714 0.000150 0.006434 0.415738 -0.578096 -0.509128 -0.483483 diff --git a/OpenSim/Tests/shared/IMUData/Xsens/MT46_01-000_00B42D4E.txt b/OpenSim/Tests/shared/IMUData/Xsens/MT46_01-000_00B42D4E.txt new file mode 100755 index 0000000000..f9012a7dae --- /dev/null +++ b/OpenSim/Tests/shared/IMUData/Xsens/MT46_01-000_00B42D4E.txt @@ -0,0 +1,10 @@ +// Start Time: Unknown +// Update Rate: 100.0Hz +// Filter Profile: human (46.1) +// Firmware Version: 4.4.0 +PacketCounter SampleTimeFine Year Month Day Second Acc_X Acc_Y Acc_Z Gyr_X Gyr_Y Gyr_Z Quat_q0 Quat_q1 Quat_q2 Quat_q3 +04097 4.707445 4.697718 7.265665 0.003258 -0.005507 -0.002823 -0.131224 -0.289821 -0.217187 -0.922829 +04098 4.667387 4.677692 7.284879 -0.002577 0.002491 -0.001265 -0.129709 -0.289078 -0.217478 -0.923208 +04099 4.680539 4.658568 7.292087 0.003437 -0.004121 -0.001997 -0.129718 -0.289097 -0.217494 -0.923197 +04100 4.680738 4.658828 7.291793 0.001885 -0.003128 -0.000795 -0.129721 -0.289117 -0.217502 -0.923188 +04101 4.700939 4.669100 7.281907 0.001193 -0.003765 -0.002854 -0.129737 -0.289132 -0.217509 -0.923180