diff --git a/OpenSim/Common/Test/testXsensDataReader.cpp b/OpenSim/Common/Test/testXsensDataReader.cpp index 52bd6a7837..632af830f1 100644 --- a/OpenSim/Common/Test/testXsensDataReader.cpp +++ b/OpenSim/Common/Test/testXsensDataReader.cpp @@ -26,6 +26,9 @@ #include "OpenSim/Common/STOFileAdapter.h" #include +#define CATCH_CONFIG_MAIN +#include + using namespace OpenSim; /* Raw data from 00B421AF @@ -35,123 +38,113 @@ 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 */ -int main() { - - try { - 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"); +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); } - catch (const std::exception& ex) { - std::cout << "testXsensDataReader FAILED: " << ex.what() << std::endl; - return 1; + 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); - std::cout << "\n All testXsensDataReader cases passed." << std::endl; - - return 0; + // 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"); }