Computing marker RMS uses the number of non-NaN values

OpenSim/Simulation/InverseKinematicsSolver.cpp

@@ -118,6 +118,31 @@ void InverseKinematicsSolver::updateMarkerWeights(const SimTK::Array_<double> &w
         throw Exception("InverseKinematicsSolver::updateMarkerWeights: invalid size of weights.");
 }
 
+/* Get the current reference values for markers*/
+const SimTK::Vec3& InverseKinematicsSolver::getMarkerReferenceValue(int markerIndex)
+{
+    SimTK::Markers::ObservationIx oix(
+        _markerAssemblyCondition->getObservationIxForMarker(
+            SimTK::Markers::MarkerIx(markerIndex)) );
+    return _markerAssemblyCondition->getObservation(oix);
+}
+const SimTK::Vec3& InverseKinematicsSolver::getMarkerReferenceValue(const std::string &markerName)
+{
+    SimTK::Markers::ObservationIx oix(
+        _markerAssemblyCondition->getObservationIxForMarker(
+            _markerAssemblyCondition->getMarkerIx(markerName)) );
+    return _markerAssemblyCondition->getObservation(oix);
+}
+void InverseKinematicsSolver::getMarkerReferenceValues(SimTK::Array_<SimTK::Vec3> &markerRefs)
+{
+    int nm = getNumMarkersInUse();
+    markerRefs.resize(nm);
+
+    for (int i = 0; i < nm; ++i) {
+        markerRefs[i] = getMarkerReferenceValue(i);
+    }
+}
+
 /* Compute and return the spatial location of a marker in ground. */
 SimTK::Vec3 InverseKinematicsSolver::computeCurrentMarkerLocation(const std::string &markerName)
 {

OpenSim/Simulation/InverseKinematicsSolver.h

@@ -112,6 +112,14 @@ class OSIMSIMULATION_API InverseKinematicsSolver: public AssemblySolver
         solver was constructed. */
     void updateMarkerWeights(const SimTK::Array_<double> &weights);
 
+    /** Get the current reference value for the marker specified by index */
+    const SimTK::Vec3& getMarkerReferenceValue(int markerIndex);
+    /** Get the current reference value for the marker specified by name */
+    const SimTK::Vec3& getMarkerReferenceValue(const std::string &markerName);
+    /** Get the current reference values for all markers in use. Missing
+        reference values at the current instant are denoted by NaN. */
+    void getMarkerReferenceValues(SimTK::Array_<SimTK::Vec3> &markerRefs);
+
     /** Compute and return a marker's spatial location in the ground frame,
         given the marker's name. */
     SimTK::Vec3 computeCurrentMarkerLocation(const std::string &markerName);

OpenSim/Simulation/Test/testInverseKinematicsSolver.cpp

@@ -550,14 +550,21 @@ void testNumberOfMarkersMismatch()
 
     int nm = ikSolver.getNumMarkersInUse();
 
+    SimTK::Array_<SimTK::Vec3> markerRefs(nm);
     SimTK::Array_<double> markerErrors(nm);
+
     for (unsigned i = 0; i < markersRef.getNumFrames(); ++i) {
         state.updTime() = i*dt;
         ikSolver.track(state);
 
+        ikSolver.getMarkerReferenceValues(markerRefs);
+        int nmr = markerRefs.size();
+        SimTK_ASSERT_ALWAYS(nmr == nm,
+            "InverseKinematicsSolver number of reference values "
+            "failed to match the number of markers in use.");
+
         //get the marker errors
         ikSolver.computeCurrentMarkerErrors(markerErrors);
-
         int nme = markerErrors.size();
 
         SimTK_ASSERT_ALWAYS(nme == nm,
@@ -582,6 +589,15 @@ void testNumberOfMarkersMismatch()
                 SimTK_ASSERT_ALWAYS(markerErrors[j] <= tol,
                     "InverseKinematicsSolver mangled marker order.");
             }
+
+            if (markerRefs[j].isNaN()) {
+                SimTK_ASSERT_ALWAYS(markerName == "mR",
+                    "InverseKinematicsSolver order of marker reference "
+                    "values does not match order of markers.");
+                SimTK_ASSERT_ALWAYS(markerErrors[j] == 0,
+                    "InverseKinematicsSolver failed to assign 0 for the "
+                    "error associated with marker reference with NaN value.");
+            }
         }
         cout << endl;
     }

OpenSim/Tools/InverseKinematicsTool.cpp

@@ -332,10 +332,17 @@ bool InverseKinematicsTool::run()
             ikSolver.track(s);
 
             if(_reportErrors){
-                Array<double> markerErrors(0.0, 3);
+                Array_<Vec3> markerRefValues(nm);
+                Array<double> markerErrors(0.0, nm);
                 double totalSquaredMarkerError = 0.0;
                 double maxSquaredMarkerError = 0.0;
                 int worst = -1;
+                int ignore_cnt = 0;
+
+                ikSolver.getMarkerReferenceValues(markerRefValues);
+                for (const Vec3& val : markerRefValues) {
+                    val.isNaN() ? ignore_cnt++ : 0;
+                }
 
                 ikSolver.computeCurrentSquaredMarkerErrors(squaredMarkerErrors);
                 for(int j=0; j<nm; ++j){
@@ -346,7 +353,8 @@ bool InverseKinematicsTool::run()
                     }
                 }
 
-                double rms = nm > 0 ? sqrt(totalSquaredMarkerError / nm) : 0;
+                double rms = (nm-ignore_cnt) > 0 ? 
+                    sqrt(totalSquaredMarkerError/(nm-ignore_cnt)) : NaN;
                 markerErrors.set(0, totalSquaredMarkerError); 
                 markerErrors.set(1, rms);
                 markerErrors.set(2, sqrt(maxSquaredMarkerError));