C3DFileAdapter::OutputTables
C3DFileAdapter::extendRead(const std::string& fileName) const {
auto reader = btk::AcquisitionFileReader::New();
reader->SetFilename(fileName);
reader->Update();
auto acquisition = reader->GetOutput();
EventTable event_table{};
auto events = acquisition->GetEvents();
for (auto it = events->Begin();
it != events->End();
++it) {
auto et = *it;
event_table.push_back({ et->GetLabel(),
et->GetTime(),
et->GetFrame(),
et->GetDescription() });
}
OutputTables tables{};
auto marker_pts = btk::PointCollection::New();
for(auto it = acquisition->BeginPoint();
it != acquisition->EndPoint();
++it) {
auto pt = *it;
if(pt->GetType() == btk::Point::Marker)
marker_pts->InsertItem(pt);
}
if(marker_pts->GetItemNumber() != 0) {
int marker_nrow = marker_pts->GetFrontItem()->GetFrameNumber();
int marker_ncol = marker_pts->GetItemNumber();
std::vector<double> marker_times(marker_nrow);
SimTK::Matrix_<SimTK::Vec3> marker_matrix(marker_nrow, marker_ncol);
std::vector<std::string> marker_labels{};
for (auto it = marker_pts->Begin(); it != marker_pts->End(); ++it) {
marker_labels.push_back(SimTK::Value<std::string>((*it)->GetLabel()));
}
double time_step{1.0 / acquisition->GetPointFrequency()};
for(int f = 0; f < marker_nrow; ++f) {
SimTK::RowVector_<SimTK::Vec3> row{ marker_pts->GetItemNumber(),
SimTK::Vec3(SimTK::NaN) };
int m{0};
for(auto it = marker_pts->Begin(); it != marker_pts->End(); ++it) {
auto pt = *it;
// BTK reads empty values as zero, but sets a "residual" value
// to -1 and it is how it knows to export these values as
// blank, instead of 0, when exporting to .trc
// See: BTKCore/Code/IO/btkTRCFileIO.cpp#L359-L360
// Read in value if it is not zero or residual is not -1
if (!pt->GetValues().row(f).isZero() || //not precisely zero
(pt->GetResiduals().coeff(f) != -1) ) {//residual is not -1
row[m] = SimTK::Vec3{ pt->GetValues().coeff(f, 0),
pt->GetValues().coeff(f, 1),
pt->GetValues().coeff(f, 2) };
}
++m;
}
marker_matrix.updRow(f) = row;
marker_times[f] = 0 + f * time_step; //TODO: 0 should be start_time
}
// Create the data
auto marker_table =
std::make_shared<TimeSeriesTableVec3>(marker_times,
marker_matrix,
marker_labels);
marker_table->
updTableMetaData().
setValueForKey("DataRate",
std::to_string(acquisition->GetPointFrequency()));
marker_table->
updTableMetaData().
setValueForKey("Units",
acquisition->GetPointUnit());
marker_table->updTableMetaData().setValueForKey("events", event_table);
tables.emplace(_markers, marker_table);
}
// This is probably the right way to get the raw forces data from force
// platforms. Extract the collection of force platforms.
auto force_platforms_extractor = btk::ForcePlatformsExtractor::New();
force_platforms_extractor->SetInput(acquisition);
auto force_platform_collection = force_platforms_extractor->GetOutput();
force_platforms_extractor->Update();
std::vector<SimTK::Matrix_<double>> fpCalMatrices{};
std::vector<SimTK::Matrix_<double>> fpCorners{};
std::vector<SimTK::Matrix_<double>> fpOrigins{};
std::vector<unsigned> fpTypes{};
auto fp_force_pts = btk::PointCollection::New();
auto fp_moment_pts = btk::PointCollection::New();
auto fp_position_pts = btk::PointCollection::New();
for(auto platform = force_platform_collection->Begin();
platform != force_platform_collection->End();
++platform) {
const auto& calMatrix = (*platform)->GetCalMatrix();
const auto& corners = (*platform)->GetCorners();
const auto& origins = (*platform)->GetOrigin();
fpCalMatrices.push_back(convertToSimtkMatrix(calMatrix));
fpCorners.push_back(convertToSimtkMatrix(corners));
fpOrigins.push_back(convertToSimtkMatrix(origins));
fpTypes.push_back(static_cast<unsigned>((*platform)->GetType()));
// Get ground reaction wrenches for the force platform.
auto ground_reaction_wrench_filter =
btk::GroundReactionWrenchFilter::New();
ground_reaction_wrench_filter->setLocation(
btk::GroundReactionWrenchFilter::Location(getLocationForForceExpression()));
ground_reaction_wrench_filter->SetInput(*platform);
auto wrench_collection = ground_reaction_wrench_filter->GetOutput();
ground_reaction_wrench_filter->Update();
for(auto wrench = wrench_collection->Begin();
wrench != wrench_collection->End();
++wrench) {
// Forces time series.
fp_force_pts->InsertItem((*wrench)->GetForce());
// Moment time series.
fp_moment_pts->InsertItem((*wrench)->GetMoment());
// Position time series.
fp_position_pts->InsertItem((*wrench)->GetPosition());
}
}
if(fp_force_pts->GetItemNumber() != 0) {
std::vector<std::string> labels{};
ValueArray<std::string> units{};
for(int fp = 1; fp <= fp_force_pts->GetItemNumber(); ++fp) {
auto fp_str = std::to_string(fp);
labels.push_back(SimTK::Value<std::string>("f" + fp_str));
auto force_unit = acquisition->GetPointUnits().
at(_unit_index.at("force"));
units.upd().push_back(SimTK::Value<std::string>(force_unit));
labels.push_back(SimTK::Value<std::string>("p" + fp_str));
auto position_unit = acquisition->GetPointUnits().
at(_unit_index.at("marker"));
units.upd().push_back(SimTK::Value<std::string>(position_unit));
labels.push_back(SimTK::Value<std::string>("m" + fp_str));
auto moment_unit = acquisition->GetPointUnits().
at(_unit_index.at("moment"));
units.upd().push_back(SimTK::Value<std::string>(moment_unit));
}
const int nf = fp_force_pts->GetFrontItem()->GetFrameNumber();
std::vector<double> force_times(nf);
SimTK::Matrix_<SimTK::Vec3> force_matrix(nf, (int)labels.size());
double time_step{1.0 / acquisition->GetAnalogFrequency()};
for(int f = 0; f < nf; ++f) {
SimTK::RowVector_<SimTK::Vec3>
row{fp_force_pts->GetItemNumber() * 3};
int col{0};
for(auto fit = fp_force_pts->Begin(),
mit = fp_moment_pts->Begin(),
pit = fp_position_pts->Begin();
fit != fp_force_pts->End();
++fit,
++mit,
++pit) {
row[col] = SimTK::Vec3{(*fit)->GetValues().coeff(f, 0),
(*fit)->GetValues().coeff(f, 1),
(*fit)->GetValues().coeff(f, 2)};
++col;
row[col] = SimTK::Vec3{(*pit)->GetValues().coeff(f, 0),
(*pit)->GetValues().coeff(f, 1),
(*pit)->GetValues().coeff(f, 2)};
++col;
row[col] = SimTK::Vec3{(*mit)->GetValues().coeff(f, 0),
(*mit)->GetValues().coeff(f, 1),
(*mit)->GetValues().coeff(f, 2)};
++col;
}
force_matrix.updRow(f) = row;
force_times[f] = 0 + f * time_step; //TODO: 0 should be start_time
}
auto& force_table =
*(new TimeSeriesTableVec3(force_times, force_matrix, labels));
TimeSeriesTableVec3::DependentsMetaData force_dep_metadata
= force_table.getDependentsMetaData();
// add units to the dependent meta data
force_dep_metadata.setValueArrayForKey("units", units);
force_table.setDependentsMetaData(force_dep_metadata);
force_table.
updTableMetaData().
setValueForKey("CalibrationMatrices", std::move(fpCalMatrices));
force_table.
updTableMetaData().
setValueForKey("Corners", std::move(fpCorners));
force_table.
updTableMetaData().
setValueForKey("Origins", std::move(fpOrigins));
force_table.
updTableMetaData().
setValueForKey("Types", std::move(fpTypes));
force_table.
updTableMetaData().
setValueForKey("DataRate",
std::to_string(acquisition->GetAnalogFrequency()));
tables.emplace(_forces,
std::shared_ptr<TimeSeriesTableVec3>(&force_table));
force_table.updTableMetaData().setValueForKey("events", event_table);
}
return tables;
}
C3DFileAdapter::OutputTables
C3DFileAdapter::extendRead(const std::string& fileName) const {
auto reader = btk::AcquisitionFileReader::New();
reader->SetFilename(fileName);
reader->Update();
auto acquisition = reader->GetOutput();
OutputTables tables{};
auto& marker_table = *(new TimeSeriesTableVec3{});
auto& force_table = *(new TimeSeriesTableVec3{});
tables.emplace(_markers,
std::shared_ptr<TimeSeriesTableVec3>(&marker_table));
tables.emplace(_forces,
std::shared_ptr<TimeSeriesTableVec3>(&force_table));
auto marker_pts = btk::PointCollection::New();
for(auto it = acquisition->BeginPoint();
it != acquisition->EndPoint();
++it) {
auto pt = *it;
if(pt->GetType() == btk::Point::Marker)
marker_pts->InsertItem(pt);
}
if(marker_pts->GetItemNumber() != 0) {
marker_table.
updTableMetaData().
setValueForKey("DataRate",
std::to_string(acquisition->GetPointFrequency()));
marker_table.
updTableMetaData().
setValueForKey("Units",
acquisition->GetPointUnit());
ValueArray<std::string> marker_labels{};
for(auto it = marker_pts->Begin();
it != marker_pts->End();
++it) {
marker_labels.
upd().
push_back(SimTK::Value<std::string>((*it)->GetLabel()));
}
TimeSeriesTableVec3::DependentsMetaData marker_dep_metadata{};
marker_dep_metadata.setValueArrayForKey("labels", marker_labels);
marker_table.setDependentsMetaData(marker_dep_metadata);
double time_step{1.0 / acquisition->GetPointFrequency()};
for(int f = 0;
f < marker_pts->GetFrontItem()->GetFrameNumber();
++f) {
SimTK::RowVector_<SimTK::Vec3> row{marker_pts->GetItemNumber()};
int m{0};
for(auto it = marker_pts->Begin();
it != marker_pts->End();
++it) {
auto pt = *it;
row[m++] = SimTK::Vec3{pt->GetValues().coeff(f, 0),
pt->GetValues().coeff(f, 1),
pt->GetValues().coeff(f, 2)};
}
marker_table.appendRow(0 + f * time_step, row);
}
}
// This is probably the right way to get the raw forces data from force
// platforms. Extract the collection of force platforms.
auto force_platforms_extractor = btk::ForcePlatformsExtractor::New();
force_platforms_extractor->SetInput(acquisition);
auto force_platform_collection = force_platforms_extractor->GetOutput();
force_platforms_extractor->Update();
std::vector<SimTK::Matrix_<double>> fpCalMatrices{};
std::vector<SimTK::Matrix_<double>> fpCorners{};
std::vector<SimTK::Matrix_<double>> fpOrigins{};
std::vector<unsigned> fpTypes{};
auto fp_force_pts = btk::PointCollection::New();
auto fp_moment_pts = btk::PointCollection::New();
auto fp_position_pts = btk::PointCollection::New();
for(auto platform = force_platform_collection->Begin();
platform != force_platform_collection->End();
++platform) {
const auto& calMatrix = (*platform)->GetCalMatrix();
const auto& corners = (*platform)->GetCorners();
const auto& origins = (*platform)->GetOrigin();
fpCalMatrices.push_back(convertToSimtkMatrix(calMatrix));
fpCorners.push_back(convertToSimtkMatrix(corners));
fpOrigins.push_back(convertToSimtkMatrix(origins));
fpTypes.push_back(static_cast<unsigned>((*platform)->GetType()));
// Get ground reaction wrenches for the force platform.
auto ground_reaction_wrench_filter =
btk::GroundReactionWrenchFilter::New();
ground_reaction_wrench_filter->SetInput(*platform);
auto wrench_collection = ground_reaction_wrench_filter->GetOutput();
ground_reaction_wrench_filter->Update();
for(auto wrench = wrench_collection->Begin();
wrench != wrench_collection->End();
++wrench) {
// Forces time series.
fp_force_pts->InsertItem((*wrench)->GetForce());
// Moment time series.
fp_moment_pts->InsertItem((*wrench)->GetMoment());
// Position time series.
fp_position_pts->InsertItem((*wrench)->GetPosition());
}
}
//shrik<btk::ForcePlatform::Origin> foo;
if(fp_force_pts->GetItemNumber() != 0) {
force_table.
updTableMetaData().
setValueForKey("CalibrationMatrices", std::move(fpCalMatrices));
force_table.
updTableMetaData().
setValueForKey("Corners", std::move(fpCorners));
force_table.
updTableMetaData().
setValueForKey("Origins", std::move(fpOrigins));
force_table.
updTableMetaData().
setValueForKey("Types", std::move(fpTypes));
force_table.
updTableMetaData().
setValueForKey("DataRate",
std::to_string(acquisition->GetAnalogFrequency()));
ValueArray<std::string> labels{};
ValueArray<std::string> units{};
for(int fp = 1; fp <= fp_force_pts->GetItemNumber(); ++fp) {
auto fp_str = std::to_string(fp);
labels.upd().push_back(SimTK::Value<std::string>("f" + fp_str));
auto force_unit = acquisition->GetPointUnits().
at(_unit_index.at("force"));
units.upd().push_back(SimTK::Value<std::string>(force_unit));
labels.upd().push_back(SimTK::Value<std::string>("m" + fp_str));
auto moment_unit = acquisition->GetPointUnits().
at(_unit_index.at("moment"));
units.upd().push_back(SimTK::Value<std::string>(moment_unit));
labels.upd().push_back(SimTK::Value<std::string>("p" + fp_str));
auto position_unit = acquisition->GetPointUnits().
at(_unit_index.at("marker"));
units.upd().push_back(SimTK::Value<std::string>(position_unit));
}
TimeSeriesTableVec3::DependentsMetaData force_dep_metadata{};
force_dep_metadata.setValueArrayForKey("labels", labels);
force_dep_metadata.setValueArrayForKey("units", units);
force_table.setDependentsMetaData(force_dep_metadata);
double time_step{1.0 / acquisition->GetAnalogFrequency()};
for(int f = 0;
f < fp_force_pts->GetFrontItem()->GetFrameNumber();
++f) {
SimTK::RowVector_<SimTK::Vec3>
row{fp_force_pts->GetItemNumber() * 3};
int col{0};
for(auto fit = fp_force_pts->Begin(),
mit = fp_moment_pts->Begin(),
pit = fp_position_pts->Begin();
fit != fp_force_pts->End();
++fit,
++mit,
++pit) {
row[col] = SimTK::Vec3{(*fit)->GetValues().coeff(f, 0),
(*fit)->GetValues().coeff(f, 1),
(*fit)->GetValues().coeff(f, 2)};
++col;
row[col] = SimTK::Vec3{(*mit)->GetValues().coeff(f, 0),
(*mit)->GetValues().coeff(f, 1),
(*mit)->GetValues().coeff(f, 2)};
++col;
row[col] = SimTK::Vec3{(*pit)->GetValues().coeff(f, 0),
(*pit)->GetValues().coeff(f, 1),
(*pit)->GetValues().coeff(f, 2)};
++col;
}
force_table.appendRow(0 + f * time_step, row);
}
}
EventTable event_table{};
auto events = acquisition->GetEvents();
for(auto it = events->Begin();
it != events->End();
++it) {
auto et = *it;
event_table.push_back({et->GetLabel(),
et->GetTime(),
et->GetFrame(),
et->GetDescription()});
}
marker_table.updTableMetaData().setValueForKey("events", event_table);
force_table.updTableMetaData().setValueForKey("events", event_table);
return tables;
}
