/**
* Set the local offset of each non-fixed marker so that in the model's
* current pose the marker coincides with the marker's global position
* in the passed-in MarkerData.
*
* @param aModel the model to use
* @param aPose the static-pose marker cloud to get the marker locations from
*/
void MarkerPlacer::moveModelMarkersToPose(SimTK::State& s, Model& aModel, MarkerData& aPose)
{
aPose.averageFrames(0.01);
const MarkerFrame &frame = aPose.getFrame(0);
const SimbodyEngine& engine = aModel.getSimbodyEngine();
MarkerSet& markerSet = aModel.updMarkerSet();
int i;
for (i = 0; i < markerSet.getSize(); i++)
{
Marker& modelMarker = markerSet.get(i);
if (true/*!modelMarker.getFixed()*/)
{
int index = aPose.getMarkerIndex(modelMarker.getName());
if (index >= 0)
{
Vec3 globalMarker = frame.getMarker(index);
if (!globalMarker.isNaN())
{
Vec3 pt, pt2;
Vec3 globalPt = globalMarker;
double conversionFactor = aPose.getUnits().convertTo(aModel.getLengthUnits());
pt = conversionFactor*globalPt;
pt2 = modelMarker.getReferenceFrame().findLocationInAnotherFrame(s, pt, aModel.getGround());
modelMarker.set_location(pt2);
}
else
{
cout << "___WARNING___: marker " << modelMarker.getName() << " does not have valid coordinates in " << aPose.getFileName() << endl;
cout << " It will not be moved to match location in marker file." << endl;
}
}
}
}
cout << "Moved markers in model " << aModel.getName() << " to match locations in marker file " << aPose.getFileName() << endl;
}
/**
* Measure the average distance between a marker pair in an experimental marker data.
*/
double ModelScaler::takeExperimentalMarkerMeasurement(const MarkerData& aMarkerData, const string& aName1, const string& aName2, const string& aMeasurementName) const
{
const Array<string>& experimentalMarkerNames = aMarkerData.getMarkerNames();
int marker1 = experimentalMarkerNames.findIndex(aName1);
int marker2 = experimentalMarkerNames.findIndex(aName2);
if (marker1 >= 0 && marker2 >= 0) {
int startIndex, endIndex;
if (_timeRange.getSize()<2)
throw Exception("ModelScaler::takeExperimentalMarkerMeasurement, time_range is unspecified.");
aMarkerData.findFrameRange(_timeRange[0], _timeRange[1], startIndex, endIndex);
double length = 0;
for(int i=startIndex; i<=endIndex; i++) {
Vec3 p1 = aMarkerData.getFrame(i).getMarker(marker1);
Vec3 p2 = aMarkerData.getFrame(i).getMarker(marker2);
length += (p2 - p1).norm();
}
return length/(endIndex-startIndex+1);
} else {
if (marker1 < 0)
cout << "___WARNING___: marker " << aName1 << " in " << aMeasurementName << " measurement not found in " << aMarkerData.getFileName() << endl;
if (marker2 < 0)
cout << "___WARNING___: marker " << aName2 << " in " << aMeasurementName << " measurement not found in " << aMarkerData.getFileName() << endl;
return SimTK::NaN;
}
}
