/** * 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) const { 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.getParentFrame().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; }
/** * This method creates a SimmMotionTrial instance with the markerFile and * timeRange parameters. It also creates a Storage instance with the * coordinateFile parameter. Then it updates the coordinates and markers in * the model, if specified. Then it does IK to fit the model to the static * pose. Then it uses the current model pose to relocate all non-fixed markers * according to their locations in the SimmMotionTrial. Then it writes the * output files selected by the user. * * @param aModel the model to use for the marker placing process. * @return Whether the marker placing process was successful or not. */ bool MarkerPlacer::processModel(Model* aModel, const string& aPathToSubject) const { if(!getApply()) return false; cout << endl << "Step 3: Placing markers on model" << endl; if (_timeRange.getSize()<2) throw Exception("MarkerPlacer::processModel, time_range is unspecified."); /* Load the static pose marker file, and average all the * frames in the user-specified time range. */ MarkerData* staticPose = new MarkerData(aPathToSubject + _markerFileName); staticPose->averageFrames(_maxMarkerMovement, _timeRange[0], _timeRange[1]); staticPose->convertToUnits(aModel->getLengthUnits()); /* Delete any markers from the model that are not in the static * pose marker file. */ aModel->deleteUnusedMarkers(staticPose->getMarkerNames()); // Construct the system and get the working state when done changing the model SimTK::State& s = aModel->initSystem(); // Create references and WeightSets needed to initialize InverseKinemaicsSolver Set<MarkerWeight> markerWeightSet; _ikTaskSet.createMarkerWeightSet(markerWeightSet); // order in tasks file // MarkersReference takes ownership of marker data (staticPose) MarkersReference markersReference(staticPose, &markerWeightSet); SimTK::Array_<CoordinateReference> coordinateReferences; // Load the coordinate data // create CoordinateReferences for Coordinate Tasks FunctionSet *coordFunctions = NULL; // bool haveCoordinateFile = false; if(_coordinateFileName != "" && _coordinateFileName != "Unassigned"){ Storage coordinateValues(aPathToSubject + _coordinateFileName); aModel->getSimbodyEngine().convertDegreesToRadians(coordinateValues); // haveCoordinateFile = true; coordFunctions = new GCVSplineSet(5,&coordinateValues); } int index = 0; for(int i=0; i< _ikTaskSet.getSize(); i++){ IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&_ikTaskSet[i]); if (coordTask && coordTask->getApply()){ std::unique_ptr<CoordinateReference> coordRef{}; if(coordTask->getValueType() == IKCoordinateTask::FromFile){ index = coordFunctions->getIndex(coordTask->getName(), index); if(index >= 0){ coordRef.reset(new CoordinateReference(coordTask->getName(),coordFunctions->get(index))); } } else if((coordTask->getValueType() == IKCoordinateTask::ManualValue)){ Constant reference(Constant(coordTask->getValue())); coordRef.reset(new CoordinateReference(coordTask->getName(), reference)); } else{ // assume it should be held at its current/default value double value = aModel->getCoordinateSet().get(coordTask->getName()).getValue(s); Constant reference = Constant(value); coordRef.reset(new CoordinateReference(coordTask->getName(), reference)); } if(coordRef == NULL) throw Exception("MarkerPlacer: value for coordinate "+coordTask->getName()+" not found."); // We have a valid coordinate reference so now set its weight according to the task coordRef->setWeight(coordTask->getWeight()); coordinateReferences.push_back(*coordRef); } } double constraintWeight = std::numeric_limits<SimTK::Real>::infinity(); InverseKinematicsSolver ikSol(*aModel, markersReference, coordinateReferences, constraintWeight); ikSol.assemble(s); // Call realize Position so that the transforms are updated and markers can be moved correctly aModel->getMultibodySystem().realize(s, SimTK::Stage::Position); // Report marker errors to assess the quality int nm = markerWeightSet.getSize(); SimTK::Array_<double> squaredMarkerErrors(nm, 0.0); SimTK::Array_<Vec3> markerLocations(nm, Vec3(0)); double totalSquaredMarkerError = 0.0; double maxSquaredMarkerError = 0.0; int worst = -1; // Report in the same order as the marker tasks/weights ikSol.computeCurrentSquaredMarkerErrors(squaredMarkerErrors); for(int j=0; j<nm; ++j){ totalSquaredMarkerError += squaredMarkerErrors[j]; if(squaredMarkerErrors[j] > maxSquaredMarkerError){ maxSquaredMarkerError = squaredMarkerErrors[j]; worst = j; } } cout << "Frame at (t=" << s.getTime() << "):\t"; cout << "total squared error = " << totalSquaredMarkerError; cout << ", marker error: RMS=" << sqrt(totalSquaredMarkerError/nm); cout << ", max=" << sqrt(maxSquaredMarkerError) << " (" << ikSol.getMarkerNameForIndex(worst) << ")" << endl; /* Now move the non-fixed markers on the model so that they are coincident * with the measured markers in the static pose. The model is already in * the proper configuration so the coordinates do not need to be changed. */ if(_moveModelMarkers) moveModelMarkersToPose(s, *aModel, *staticPose); _outputStorage.reset(); // Make a storage file containing the solved states and markers for display in GUI. Storage motionData; StatesReporter statesReporter(aModel); statesReporter.begin(s); _outputStorage.reset(new Storage(statesReporter.updStatesStorage())); _outputStorage->setName("static pose"); //_outputStorage->print("statesReporterOutput.sto"); Storage markerStorage; staticPose->makeRdStorage(*_outputStorage); _outputStorage->getStateVector(0)->setTime(s.getTime()); statesReporter.updStatesStorage().addToRdStorage(*_outputStorage, s.getTime(), s.getTime()); //_outputStorage->print("statesReporterOutputWithMarkers.sto"); if(_printResultFiles) { if (!_outputModelFileNameProp.getValueIsDefault()) { aModel->print(aPathToSubject + _outputModelFileName); cout << "Wrote model file " << _outputModelFileName << " from model " << aModel->getName() << endl; } if (!_outputMarkerFileNameProp.getValueIsDefault()) { aModel->writeMarkerFile(aPathToSubject + _outputMarkerFileName); cout << "Wrote marker file " << _outputMarkerFileName << " from model " << aModel->getName() << endl; } if (!_outputMotionFileNameProp.getValueIsDefault()) { _outputStorage->print(aPathToSubject + _outputMotionFileName, "w", "File generated from solving marker data for model "+aModel->getName()); } } return true; }
/** * This method creates a SimmMotionTrial instance with the markerFile and * timeRange parameters. It also creates a Storage instance with the * coordinateFile parameter. Then it updates the coordinates and markers in * the model, if specified. Then it does IK to fit the model to the static * pose. Then it uses the current model pose to relocate all non-fixed markers * according to their locations in the SimmMotionTrial. Then it writes the * output files selected by the user. * * @param aModel the model to use for the marker placing process. * @return Whether the marker placing process was successful or not. */ bool MarkerPlacer::processModel(Model* aModel, const string& aPathToSubject) const { if(!getApply()) return false; cout << endl << "Step 3: Placing markers on model" << endl; if (_timeRange.getSize()<2) throw Exception("MarkerPlacer::processModel, time_range is unspecified."); /* Load the static pose marker file, and average all the * frames in the user-specified time range. */ TimeSeriesTableVec3 staticPoseTable{aPathToSubject + _markerFileName}; const auto& timeCol = staticPoseTable.getIndependentColumn(); // Users often set a time range that purposely exceeds the range of // their data with the mindset that all their data will be used. // To allow for that, we have to narrow the provided range to data // range, since the TimeSeriesTable will correctly throw that the // desired time exceeds the data range. if (_timeRange[0] < timeCol.front()) _timeRange[0] = timeCol.front(); if (_timeRange[1] > timeCol.back()) _timeRange[1] = timeCol.back(); const auto avgRow = staticPoseTable.averageRow(_timeRange[0], _timeRange[1]); for(size_t r = staticPoseTable.getNumRows(); r-- > 0; ) staticPoseTable.removeRowAtIndex(r); staticPoseTable.appendRow(_timeRange[0], avgRow); OPENSIM_THROW_IF(!staticPoseTable.hasTableMetaDataKey("Units"), Exception, "MarkerPlacer::processModel -- Marker file does not have " "'Units'."); Units staticPoseUnits{staticPoseTable.getTableMetaData<std::string>("Units")}; double scaleFactor = staticPoseUnits.convertTo(aModel->getLengthUnits()); OPENSIM_THROW_IF(SimTK::isNaN(scaleFactor), Exception, "Model has unspecified units."); if(std::fabs(scaleFactor - 1) >= SimTK::Eps) { for(unsigned r = 0; r < staticPoseTable.getNumRows(); ++r) staticPoseTable.updRowAtIndex(r) *= scaleFactor; staticPoseUnits = aModel->getLengthUnits(); staticPoseTable.removeTableMetaDataKey("Units"); staticPoseTable.addTableMetaData("Units", staticPoseUnits.getAbbreviation()); } MarkerData* staticPose = new MarkerData(aPathToSubject + _markerFileName); staticPose->averageFrames(_maxMarkerMovement, _timeRange[0], _timeRange[1]); staticPose->convertToUnits(aModel->getLengthUnits()); /* Delete any markers from the model that are not in the static * pose marker file. */ aModel->deleteUnusedMarkers(staticPose->getMarkerNames()); // Construct the system and get the working state when done changing the model SimTK::State& s = aModel->initSystem(); s.updTime() = _timeRange[0]; // Create references and WeightSets needed to initialize InverseKinemaicsSolver Set<MarkerWeight> markerWeightSet; _ikTaskSet.createMarkerWeightSet(markerWeightSet); // order in tasks file // MarkersReference takes ownership of marker data (staticPose) MarkersReference markersReference(staticPoseTable, &markerWeightSet); SimTK::Array_<CoordinateReference> coordinateReferences; // Load the coordinate data // create CoordinateReferences for Coordinate Tasks FunctionSet *coordFunctions = NULL; // bool haveCoordinateFile = false; if(_coordinateFileName != "" && _coordinateFileName != "Unassigned"){ Storage coordinateValues(aPathToSubject + _coordinateFileName); aModel->getSimbodyEngine().convertDegreesToRadians(coordinateValues); // haveCoordinateFile = true; coordFunctions = new GCVSplineSet(5,&coordinateValues); } int index = 0; for(int i=0; i< _ikTaskSet.getSize(); i++){ IKCoordinateTask *coordTask = dynamic_cast<IKCoordinateTask *>(&_ikTaskSet[i]); if (coordTask && coordTask->getApply()){ std::unique_ptr<CoordinateReference> coordRef{}; if(coordTask->getValueType() == IKCoordinateTask::FromFile){ index = coordFunctions->getIndex(coordTask->getName(), index); if(index >= 0){ coordRef.reset(new CoordinateReference(coordTask->getName(),coordFunctions->get(index))); } } else if((coordTask->getValueType() == IKCoordinateTask::ManualValue)){ Constant reference(Constant(coordTask->getValue())); coordRef.reset(new CoordinateReference(coordTask->getName(), reference)); } else{ // assume it should be held at its current/default value double value = aModel->getCoordinateSet().get(coordTask->getName()).getValue(s); Constant reference = Constant(value); coordRef.reset(new CoordinateReference(coordTask->getName(), reference)); } if(coordRef == NULL) throw Exception("MarkerPlacer: value for coordinate "+coordTask->getName()+" not found."); // We have a valid coordinate reference so now set its weight according to the task coordRef->setWeight(coordTask->getWeight()); coordinateReferences.push_back(*coordRef); } } double constraintWeight = std::numeric_limits<SimTK::Real>::infinity(); InverseKinematicsSolver ikSol(*aModel, markersReference, coordinateReferences, constraintWeight); ikSol.assemble(s); // Call realize Position so that the transforms are updated and markers can be moved correctly aModel->getMultibodySystem().realize(s, SimTK::Stage::Position); // Report marker errors to assess the quality int nm = markerWeightSet.getSize(); SimTK::Array_<double> squaredMarkerErrors(nm, 0.0); SimTK::Array_<Vec3> markerLocations(nm, Vec3(0)); double totalSquaredMarkerError = 0.0; double maxSquaredMarkerError = 0.0; int worst = -1; // Report in the same order as the marker tasks/weights ikSol.computeCurrentSquaredMarkerErrors(squaredMarkerErrors); for(int j=0; j<nm; ++j){ totalSquaredMarkerError += squaredMarkerErrors[j]; if(squaredMarkerErrors[j] > maxSquaredMarkerError){ maxSquaredMarkerError = squaredMarkerErrors[j]; worst = j; } } cout << "Frame at (t=" << s.getTime() << "):\t"; cout << "total squared error = " << totalSquaredMarkerError; cout << ", marker error: RMS=" << sqrt(totalSquaredMarkerError/nm); cout << ", max=" << sqrt(maxSquaredMarkerError) << " (" << ikSol.getMarkerNameForIndex(worst) << ")" << endl; /* Now move the non-fixed markers on the model so that they are coincident * with the measured markers in the static pose. The model is already in * the proper configuration so the coordinates do not need to be changed. */ if(_moveModelMarkers) moveModelMarkersToPose(s, *aModel, *staticPose); _outputStorage.reset(); // Make a storage file containing the solved states and markers for display in GUI. Storage motionData; StatesReporter statesReporter(aModel); statesReporter.begin(s); _outputStorage.reset(new Storage(statesReporter.updStatesStorage())); _outputStorage->setName("static pose"); _outputStorage->getStateVector(0)->setTime(s.getTime()); if(_printResultFiles) { std::string savedCwd = IO::getCwd(); IO::chDir(aPathToSubject); try { // writing can throw an exception if (_outputModelFileNameProp.isValidFileName()) { aModel->print(aPathToSubject + _outputModelFileName); cout << "Wrote model file " << _outputModelFileName << " from model " << aModel->getName() << endl; } if (_outputMarkerFileNameProp.isValidFileName()) { aModel->writeMarkerFile(aPathToSubject + _outputMarkerFileName); cout << "Wrote marker file " << _outputMarkerFileName << " from model " << aModel->getName() << endl; } if (_outputMotionFileNameProp.isValidFileName()) { _outputStorage->print(aPathToSubject + _outputMotionFileName, "w", "File generated from solving marker data for model " + aModel->getName()); } } // catch the exception so we can reset the working directory catch (std::exception& ex) { IO::chDir(savedCwd); OPENSIM_THROW_FRMOBJ(Exception, ex.what()); } IO::chDir(savedCwd); } return true; }