/**
 * 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;
}
Example #2
0
/**
 * 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;
    }
}