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/testXsensDataReader.cpp b/OpenSim/Common/Test/testXsensDataReader.cpp index 07e914452d..a944f0647a 100644 --- a/OpenSim/Common/Test/testXsensDataReader.cpp +++ b/OpenSim/Common/Test/testXsensDataReader.cpp @@ -37,113 +37,124 @@ 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); - } - 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); +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; + 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); + } } } - // 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"); + 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); + 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); + } + 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); + ASSERT(accelTable4->getNumRows() == 4); + } + 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); + ASSERT(accelTable5->getNumRows() == 5); + } } diff --git a/OpenSim/Common/XsensDataReader.cpp b/OpenSim/Common/XsensDataReader.cpp index 599e99eb2f..7d2b961ae6 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,403 @@ 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 headers for data file in order + const std::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 delimiter = _settings.get_delimiter(); + const std::string extension = _settings.get_trial_extension(); + const std::string rotation_representation = + _settings.get_rotation_representation(); + 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(), + [&delimiter, &imu_h, &rotation_representation](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); + + // 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); + + int rotation_reps_found = + found_quat + found_euler + found_rot_mat; + + // Determine the rotation format to be used + if (rotation_reps_found > 1) { + imu.rotation_format = rotation_representation; + } else { + if (found_quat) { imu.rotation_format = "rot_quaternion"; } + if (found_euler) { imu.rotation_format = "rot_euler"; } + if (found_rot_mat) { imu.rotation_format = "rot_matrix"; } + } + // If no Orientation data is available we'll abort completely + OPENSIM_THROW_IF(!(rotation_reps_found >= 1), + TableMissingHeader, + "Rotation Data not found in: " + imu.name + + " Please ensure that the " + "XsensDataReaderSettings match the file " + "format!\n Attempted to parse with rotation " + "format : \"" + + imu.rotation_format + "\" and delimiter: \"" + + delimiter + "\""); + 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 row, stitch values from different + // files then append,time and timestep are based on the first file + 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 + // An exception was thrown if there is no rotation + 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..4075f2aef3 100644 --- a/OpenSim/Common/XsensDataReaderSettings.h +++ b/OpenSim/Common/XsensDataReaderSettings.h @@ -41,6 +41,21 @@ 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. Defaults to 40Hz if no value provided." + "This is used in calculating the time interval (1/frequency) for the resultant tables." + "Newer versions of the Xsens software do not specify the recording data rate." + "See #3956 for details."); + OpenSim_DECLARE_PROPERTY(delimiter, std::string, + "The delimiter used within the file to separate columns. Defaults to tab (\t)." + "Currently parsable options include: tab,space, and comma (\"\t\",\" \",\",\")"); + OpenSim_DECLARE_PROPERTY(rotation_representation, std::string, + "How the rotation information is represented in the file. Defaults to \"rot_matrix\"." + "Valid values are: (\"rot_matrix\", \"rot_euler\", and \"rot_quaternion\"). " + "This value is only used if multiple rotation representations are present in the file." + "If only a single rotation format is present, this setting is disregarded."); OpenSim_DECLARE_LIST_PROPERTY(ExperimentalSensors, ExperimentalSensor, "List of Experimental sensors and desired associated names in resulting tables"); @@ -61,6 +76,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(XsensDataReaderSettings, Object); void constructProperties() { constructProperty_data_folder(""); constructProperty_trial_prefix(""); + constructProperty_trial_extension(".txt"); + constructProperty_sampling_rate(40.0); + constructProperty_delimiter("\t"); + constructProperty_rotation_representation("rot_matrix"); constructProperty_ExperimentalSensors(); } };