std::string FileAdapter::findExtension(const std::string& filename) { std::size_t found = filename.find_last_of('.'); OPENSIM_THROW_IF(found == std::string::npos, FileExtensionNotFound, filename); return filename.substr(found + 1); }
DataAdapter::OutputTables XsensDataReader::extendRead(const std::string& folderName) const { std::vector<std::ifstream*> imuStreams; std::vector<std::string> labels; // files specified by prefix + file name exist double dataRate = SimTK::NaN; int packetCounterIndex = -1; 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_<SimTK::Quaternion> rotationsData{ last_size, n_imus }; SimTK::Matrix_<SimTK::Vec3> linearAccelerationData{ last_size, n_imus }; SimTK::Matrix_<SimTK::Vec3> magneticHeadingData{ last_size, n_imus }; SimTK::Matrix_<SimTK::Vec3> angularVelocityData{ last_size, n_imus }; std::vector<double> times; times.resize(last_size); std::string prefix = _settings.get_trial_prefix(); 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; packetCounterIndex = -1; // Force moving file pointer to beginning of data for each stream for (int j = 0; packetCounterIndex == -1; j++) { std::getline(*nextStream, line); if (j == 1 && SimTK::isNaN(dataRate)) { // Extract Data rate from line 1 std::vector<std::string> tokens = FileAdapter::tokenize(line, ", "); // find Update Rate: and parse into dataRate if (tokens.size() < 4) continue; if (tokens[1] == "Update" && tokens[2] == "Rate:") { dataRate = std::stod(tokens[3]); } } // Find indices for PacketCounter, Acc_{X,Y,Z}, Gyr_{X,Y,Z}, Mag_{X,Y,Z} on line 5 std::vector<std::string> tokens = FileAdapter::tokenize(line, "\t"); packetCounterIndex = find_index(tokens, "PacketCounter"); if (packetCounterIndex == -1) { // Could be comment, skip over continue; } else { 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]"); } } } // 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 or dataRate can't be deduced we'll abort completely OPENSIM_THROW_IF((rotationsIndex == -1 || SimTK::isNaN(dataRate)), 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; double time = 0.0; double timeIncrement = 1 / dataRate; 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 filles collating values int imu_index = 0; for (std::vector<std::ifstream*>::iterator it = imuStreams.begin(); it != imuStreams.end(); ++it, ++imu_index) { std::ifstream* nextStream = *it; // parse gyro info from imuStream std::vector<std::string> nextRow = FileAdapter::getNextLine(*nextStream, "\t\r"); if (nextRow.empty()) { done = true; break; } if (foundLinearAccelerationData) accel_row_vector[imu_index] = SimTK::Vec3(std::stod(nextRow[accIndex]), std::stod(nextRow[accIndex + 1]), std::stod(nextRow[accIndex + 2])); if (foundMagneticHeadingData) magneto_row_vector[imu_index] = SimTK::Vec3(std::stod(nextRow[magIndex]), std::stod(nextRow[magIndex + 1]), std::stod(nextRow[magIndex + 2])); if (foundAngularVelocityData) gyro_row_vector[imu_index] = SimTK::Vec3(std::stod(nextRow[gyroIndex]), std::stod(nextRow[gyroIndex + 1]), std::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] = std::stod(nextRow[rotationsIndex + matrix_entry_index]); matrix_entry_index++; } } // 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 times[rowNumber] = time; 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; time += timeIncrement; rowNumber++; if (std::remainder(rowNumber, last_size) == 0) { // resize all Data/Matrices, double the size while keeping data int newSize = last_size*2; times.resize(newSize); // 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; } } // Trim Matrices in use to actual data and move into tables times.resize(rowNumber); // 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); // 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); return tables; }