Skip to content

Commit f62b68a

Browse files
alexbeattie42alexbeattie42
committed
Improve IMU processing logic
1 parent 9dcea62 commit f62b68a

File tree

2 files changed

+65
-105
lines changed

2 files changed

+65
-105
lines changed

‎OpenSim/Common/XsensDataReader.cpp

+65-86
Original file line numberDiff line numberDiff line change
@@ -9,27 +9,33 @@
99
#include <filesystem>
1010
#include <fstream>
1111
#include <iterator>
12+
#include <map>
13+
#include <set>
14+
#include <string>
1215

1316
namespace OpenSim {
1417

1518
XsensDataReader* XsensDataReader::clone() const {
1619
return new XsensDataReader{*this};
1720
}
1821

19-
const std::map<std::string, std::set<std::string>> XsensDataReader::_accepted_headers = {
20-
{"accelerometer", {"Acc_X", "Acc_Y", "Acc_Z"}},
21-
{"gyroscope", {"Gyr_X", "Gyr_Y", "Gyr_Z"}},
22-
{"magnetometer", {"Mag_X", "Mag_Y", "Mag_Z"}},
23-
{"rot_quaternion", {"Quat_q0", "Quat_q1", "Quat_q2", "Quat_q3"}},
24-
{"rot_euler", {"Roll", "Pitch", "Yaw"}},
25-
{"rot_matrix", {"Mat[1][1]", "Mat[2][1]", "Mat[3][1]", "Mat[1][2]",
26-
"Mat[2][2]", "Mat[3][2]", "Mat[1][3]",
27-
"Mat[2][3]", "Mat[3][3]"}}};
28-
2922
DataAdapter::OutputTables XsensDataReader::extendRead(
3023
const std::string& folderName) const {
3124

32-
std::vector<std::ifstream*> imuStreams;
25+
// This encapsulates all the accepted headers and their groups
26+
const static std::map<std::string, std::set<std::string>>
27+
_accepted_headers = {{"accelerometer", {"Acc_X", "Acc_Y", "Acc_Z"}},
28+
{"gyroscope", {"Gyr_X", "Gyr_Y", "Gyr_Z"}},
29+
{"magnetometer", {"Mag_X", "Mag_Y", "Mag_Z"}},
30+
{"rot_quaternion",
31+
{"Quat_q0", "Quat_q1", "Quat_q2", "Quat_q3"}},
32+
{"rot_euler", {"Roll", "Pitch", "Yaw"}},
33+
{"rot_matrix",
34+
{"Mat[1][1]", "Mat[2][1]", "Mat[3][1]", "Mat[1][2]",
35+
"Mat[2][2]", "Mat[3][2]", "Mat[1][3]",
36+
"Mat[2][3]", "Mat[3][3]"}}};
37+
38+
std::vector<std::unique_ptr<std::ifstream>> imuStreams;
3339
std::vector<std::string> labels;
3440
// files specified by prefix + file name exist
3541
double dataRate = _settings.get_sampling_rate();
@@ -44,25 +50,18 @@ DataAdapter::OutputTables XsensDataReader::extendRead(
4450

4551
std::string prefix = _settings.get_trial_prefix();
4652
std::map<std::string, std::string> headersKeyValuePairs;
47-
// Map to store the headers and their column indices by group
48-
std::map<std::string, std::set<std::pair<std::string, size_t>>>
49-
presentGroupsWithColumnIndices;
50-
5153
std::map<std::string, size_t> h_map;
54+
5255
for (int index = 0; index < n_imus; ++index) {
5356
std::string prefix = _settings.get_trial_prefix();
5457
const ExperimentalSensor& nextItem =
5558
_settings.get_ExperimentalSensors(index);
5659
const std::filesystem::path fileName =
5760
std::filesystem::path(folderName) /
5861
(prefix + nextItem.getName() + extension);
59-
auto* nextStream = new std::ifstream{fileName};
62+
auto nextStream = std::make_unique<std::ifstream>(fileName);
6063

6164
OPENSIM_THROW_IF(!nextStream->good(), FileDoesNotExist, fileName);
62-
// Add imu name to labels
63-
labels.push_back(nextItem.get_name_in_model());
64-
// Add corresponding stream to imuStreams
65-
imuStreams.push_back(nextStream);
6665

6766
// Skip lines to get to data
6867
std::string line;
@@ -90,58 +89,69 @@ DataAdapter::OutputTables XsensDataReader::extendRead(
9089
// Find indices for Acc_{X,Y,Z}, Gyr_{X,Y,Z},
9190
// Mag_{X,Y,Z}, Mat on first non-comment line
9291
tokens = FileAdapter::tokenize(line, delimiter);
93-
// Process each word in the line (separated by spaces)
92+
// Process each header in the line
9493
for (auto& pair : _accepted_headers) {
9594
const std::string& group = pair.first;
9695
const std::set<std::string>& headers = pair.second;
9796
for (const auto& header : headers) {
98-
int tokenIndex = find_index(tokens, header);
99-
if (tokenIndex != -1) {
100-
// Add the header with its column index to the map
101-
presentGroupsWithColumnIndices[group].insert(
102-
{header, tokenIndex});
103-
}
97+
int tokenIndex = std::distance(tokens.begin(),
98+
std::find(tokens.begin(), tokens.end(), header));
99+
if (tokenIndex != -1) { h_map.insert({header, tokenIndex}); }
104100
}
105101
}
106-
// Output the present header groups and their column indices
107-
// std::cout << "Present header groups with their column indices:"
108-
// << std::endl;
109102

110-
for (const auto& group : presentGroupsWithColumnIndices) {
111-
// std::cout << group.first << ":" << std::endl;
112-
for (const auto& header : group.second) {
113-
// std::cout << " " << header.first << " at column "
114-
// << header.second << std::endl;
115-
h_map.insert({header.first, header.second});
116-
}
117-
}
118103
// Count the total lines in the file
119104
// TODO: make sure all the sensor files have the same number of lines
120-
std::ifstream lineCount{fileName};
121-
n_lines = std::count(std::istreambuf_iterator<char>(lineCount),
105+
const int fp_pos = nextStream->tellg();
106+
// std::cout << "Current pos: " << fp_pos << std::endl;
107+
// Count the number of lines
108+
n_lines = std::count(std::istreambuf_iterator<char>(*nextStream),
122109
std::istreambuf_iterator<char>(), '\n');
110+
// Rewind file pointer to after header
111+
nextStream->seekg(fp_pos, std::ifstream::beg);
123112
// std::cout << "Number of Lines: " << n_lines << std::endl;
113+
114+
// Add imu name to labels
115+
labels.push_back(nextItem.get_name_in_model());
116+
// Add corresponding stream to imuStreams
117+
imuStreams.push_back(std::move(nextStream));
124118
}
125119
// Compute data rate based on key/value pair if available
126120
std::map<std::string, std::string>::iterator it =
127121
headersKeyValuePairs.find("Update Rate");
128122
if (it != headersKeyValuePairs.end())
129123
dataRate = OpenSim::IO::stod(it->second);
124+
// Make sure that the specified header group has all required headers
125+
auto is_group_complete =
126+
[&](const std::string& group,
127+
const std::map<std::string, std::set<std::string>>&
128+
accepted_headers,
129+
const std::map<std::string, size_t>& found_headers)
130+
-> bool {
131+
const auto& reqIt = accepted_headers.find(group);
132+
if (reqIt == accepted_headers.end()) return false;
133+
const auto& search_set = reqIt->second;
134+
return std::all_of(
135+
search_set.begin(), search_set.end(), [&](const auto& p) {
136+
return found_headers.find(p) != found_headers.end();
137+
});
138+
};
130139
// internally keep track of what data was found in input files
131-
bool foundLinearAccelerationData = is_group_complete(
132-
"accelerometer", _accepted_headers, presentGroupsWithColumnIndices);
133-
bool foundMagneticHeadingData = is_group_complete(
134-
"magnetometer", _accepted_headers, presentGroupsWithColumnIndices);
135-
bool foundAngularVelocityData = is_group_complete(
136-
"gyroscope", _accepted_headers, presentGroupsWithColumnIndices);
137-
bool foundRotationData = is_group_complete(rotation_representation,
138-
_accepted_headers, presentGroupsWithColumnIndices);
140+
bool foundLinearAccelerationData =
141+
is_group_complete("accelerometer", _accepted_headers, h_map);
142+
bool foundMagneticHeadingData =
143+
is_group_complete("magnetometer", _accepted_headers, h_map);
144+
bool foundAngularVelocityData =
145+
is_group_complete("gyroscope", _accepted_headers, h_map);
146+
bool foundRotationData = is_group_complete(
147+
rotation_representation, _accepted_headers, h_map);
139148
// If no Orientation data is available we'll abort completely
140-
OPENSIM_THROW_IF(!foundRotationData,
141-
TableMissingHeader,
142-
"Rotation Data not found. Please ensure that the XsensDataReaderSettings match the file format being parsed!\n"
143-
" Attempted to parse with rotation format: \"" + rotation_representation_str + "\""
144-
" and delimiter: \"" + delimiter + "\"");
149+
OPENSIM_THROW_IF(!foundRotationData, TableMissingHeader,
150+
"Rotation Data not found. Please ensure that the "
151+
"XsensDataReaderSettings match the file format being parsed!\n"
152+
" Attempted to parse with rotation format: \"" +
153+
rotation_representation_str + "\" and delimiter: \"" +
154+
delimiter + "\"");
145155

146156
// Will read data into pre-allocated Matrices in-memory rather than
147157
// appendRow on the fly to avoid the overhead of
@@ -166,9 +176,7 @@ DataAdapter::OutputTables XsensDataReader::extendRead(
166176
n_imus, SimTK::Vec3(SimTK::NaN)};
167177
// Cycle through the files collating values
168178
int imu_index = 0;
169-
for (std::vector<std::ifstream*>::iterator it = imuStreams.begin();
170-
it != imuStreams.end(); ++it, ++imu_index) {
171-
std::ifstream* nextStream = *it;
179+
for (auto&& nextStream : imuStreams) {
172180
// parse gyro info from imuStream
173181
std::vector<std::string> nextRow =
174182
FileAdapter::getNextLine(*nextStream, delimiter + "\r");
@@ -240,6 +248,7 @@ DataAdapter::OutputTables XsensDataReader::extendRead(
240248
imu_rotation.convertRotationToQuaternion();
241249
}
242250
}
251+
imu_index++;
243252
}
244253
if (end_of_file) { break; }
245254
// append to the tables
@@ -275,34 +284,4 @@ DataAdapter::OutputTables XsensDataReader::extendRead(
275284
return tables;
276285
}
277286

278-
bool XsensDataReader::is_group_complete(const std::string& group,
279-
const std::map<std::string, std::set<std::string>>&
280-
headers,
281-
const std::map<std::string, std::set<std::pair<std::string, size_t>>>&
282-
presentGroupsWithColumnIndices) {
283-
auto reqIt = headers.find(group);
284-
if (reqIt == headers.end()) return false;
285-
286-
auto presIt = presentGroupsWithColumnIndices.find(group);
287-
if (presIt == presentGroupsWithColumnIndices.end()) return false;
288-
289-
std::set<std::string> present;
290-
for (const auto& pair : presIt->second) { present.insert(pair.first); }
291-
292-
for (const auto& header : reqIt->second) {
293-
if (present.find(header) == present.end()) { return false; }
294-
}
295-
296-
return true;
297-
}
298-
299-
int XsensDataReader::find_index(
300-
std::vector<std::string>& tokens, const std::string& keyToMatch) {
301-
int returnIndex = -1;
302-
std::vector<std::string>::iterator it =
303-
std::find(tokens.begin(), tokens.end(), keyToMatch);
304-
if (it != tokens.end())
305-
returnIndex = static_cast<int>(std::distance(tokens.begin(), it));
306-
return returnIndex;
307-
}
308287
} // namespace OpenSim

‎OpenSim/Common/XsensDataReader.h

-19
Original file line numberDiff line numberDiff line change
@@ -29,9 +29,6 @@
2929
#include "Object.h"
3030
#include "TimeSeriesTable.h"
3131
#include "XsensDataReaderSettings.h"
32-
#include <map>
33-
#include <set>
34-
#include <string>
3532

3633
/** @file
3734
* This file defines class for reading data files from IMU maker Xsens and
@@ -92,27 +89,11 @@ class OSIMCOMMON_API XsensDataReader : public IMUDataReader {
9289
XsensDataReaderSettings& updSettings() { return _settings; }
9390

9491
private:
95-
/**
96-
* Find index of searchString in tokens
97-
*/
98-
static int find_index(
99-
std::vector<std::string>& tokens, const std::string& searchString);
100-
101-
static bool is_group_complete(const std::string& group,
102-
const std::map<std::string, std::set<std::string>>& headers,
103-
const std::map<std::string,
104-
std::set<std::pair<std::string, size_t>>>&
105-
presentGroupsWithColumnIndices);
10692
/**
10793
* This data member encapsulates all the serializable settings for
10894
* the Reader;
10995
*/
11096
XsensDataReaderSettings _settings;
111-
112-
/**
113-
* This data member encapsulates all the accepted headers and their groups
114-
*/
115-
static const std::map<std::string, std::set<std::string>> _accepted_headers;
11697
};
11798

11899
} // namespace OpenSim

0 commit comments

Comments
 (0)