Skip to content

Commit

Permalink
Exposing RippleReductionProblem parameters to python when fixing mark…
Browse files Browse the repository at this point in the history
…ers up
  • Loading branch information
keenon committed Oct 1, 2023
1 parent 9bbe031 commit f9dcc93
Show file tree
Hide file tree
Showing 8 changed files with 46 additions and 21 deletions.
2 changes: 1 addition & 1 deletion VERSION.txt
Original file line number Diff line number Diff line change
@@ -1 +1 @@
0.10.2.2
0.10.3
8 changes: 6 additions & 2 deletions dart/biomechanics/MarkerFitter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1092,10 +1092,14 @@ bool MarkerFitter::checkForEnoughMarkers(
/// produce fixes where possible.
std::shared_ptr<MarkersErrorReport> MarkerFitter::generateDataErrorsReport(
std::vector<std::map<std::string, Eigen::Vector3s>> immutableMarkerObservations,
s_t dt)
s_t dt,
bool rippleReduce,
bool rippleReduceUseSparse,
bool rippleReduceUseIterativeSolver,
int rippleReduceSolverIterations)
{
std::shared_ptr<MarkersErrorReport> report
= MarkerFixer::generateDataErrorsReport(immutableMarkerObservations, dt);
= MarkerFixer::generateDataErrorsReport(immutableMarkerObservations, dt, rippleReduce, rippleReduceUseSparse, rippleReduceUseIterativeSolver, rippleReduceSolverIterations);

// 1. Generate a list of the markers we observe in this clip

Expand Down
6 changes: 5 additions & 1 deletion dart/biomechanics/MarkerFitter.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -607,7 +607,11 @@ class MarkerFitter
std::shared_ptr<MarkersErrorReport> generateDataErrorsReport(
std::vector<std::map<std::string, Eigen::Vector3s>>
markerObservations,
s_t dt);
s_t dt,
bool rippleReduce = true,
bool rippleReduceUseSparse = true,
bool rippleReduceUseIterativeSolver = true,
int rippleReduceSolverIterations = 1e5);

/// After we've finished our initialization, it may become clear that markers
/// in some of the files should be reversed. This method will do that check,
Expand Down
27 changes: 16 additions & 11 deletions dart/biomechanics/MarkerFixer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -659,7 +659,7 @@ void RippleReductionProblem::interpolateMissingPoints()

//==============================================================================
std::vector<std::map<std::string, Eigen::Vector3s>>
RippleReductionProblem::smooth(MarkersErrorReport* report)
RippleReductionProblem::smooth(MarkersErrorReport* report, bool useSparse, bool useIterativeSolver, int solverIterations)
{
dropSuspiciousPoints(report);
interpolateMissingPoints();
Expand Down Expand Up @@ -688,7 +688,8 @@ RippleReductionProblem::smooth(MarkersErrorReport* report)

// Smooth only the window during which we observed the marker (don't smooth
// to the 0's on frames where we weren't observing the marker)
AccelerationSmoother smoother(duration, 0.3, 1.0);
AccelerationSmoother smoother(duration, 0.3, 1.0, useSparse, useIterativeSolver);
smoother.setIterations(solverIterations);
mMarkers[markerName].block(0, firstObserved, 3, duration) = smoother.smooth(
mMarkers[markerName].block(0, firstObserved, 3, duration));
}
Expand Down Expand Up @@ -821,21 +822,23 @@ void RippleReductionProblem::saveToGUI(std::string markerName, std::string path)
/// produce fixes where possible.
std::shared_ptr<MarkersErrorReport> MarkerFixer::generateDataErrorsReport(
std::vector<std::map<std::string, Eigen::Vector3s>>
immutableMarkerObservations,
markerObservations,
s_t dt,
bool dropProlongedStillness)
bool dropProlongedStillness,
bool rippleReduce,
bool rippleReduceUseSparse,
bool rippleReduceUseIterativeSolver,
int rippleReduceSolverIterations)
{
std::shared_ptr<MarkersErrorReport> report
= std::make_shared<MarkersErrorReport>();
std::vector<std::map<std::string, Eigen::Vector3s>> markerObservations
= immutableMarkerObservations;

// 1. Attempt to detect marker flips that occur partway through the trajectory

// 1.1. Collect markers into continuous traces. Break marker traces that imply
// a velocity greater than 20 m/s
std::vector<LabeledMarkerTrace> traces = LabeledMarkerTrace::createRawTraces(
immutableMarkerObservations, dt * 20.0);
markerObservations, dt * 20.0);

// 1.2. Label the traces based on their majority label during the trace
std::vector<std::string> traceLabels;
Expand All @@ -861,7 +864,7 @@ std::shared_ptr<MarkersErrorReport> MarkerFixer::generateDataErrorsReport(
// 2. Emit the corrected marker observations

std::vector<std::map<std::string, Eigen::Vector3s>> correctedObservations;
for (int t = 0; t < immutableMarkerObservations.size(); t++)
for (int t = 0; t < markerObservations.size(); t++)
{
std::vector<std::tuple<std::string, Eigen::Vector3s, std::string>>
droppedMarkerWarningsFrame;
Expand Down Expand Up @@ -933,7 +936,7 @@ std::shared_ptr<MarkersErrorReport> MarkerFixer::generateDataErrorsReport(
std::map<std::string, s_t> markerDropPercentage;
for (auto& pair : observationCount)
{
s_t percentage = (s_t)pair.second / (s_t)immutableMarkerObservations.size();
s_t percentage = (s_t)pair.second / (s_t)markerObservations.size();
if (percentage < 0.1)
{
report->warnings.push_back(
Expand Down Expand Up @@ -961,8 +964,10 @@ std::shared_ptr<MarkersErrorReport> MarkerFixer::generateDataErrorsReport(
}
}

RippleReductionProblem rippleReduction(correctedObservations, dt);
correctedObservations = rippleReduction.smooth(report.get());
if (rippleReduce) {
RippleReductionProblem rippleReduction(correctedObservations, dt);
correctedObservations = rippleReduction.smooth(report.get(), rippleReduceUseSparse, rippleReduceUseIterativeSolver, rippleReduceSolverIterations);
}

// 3. Emit warnings based on any traces that include markers that are not
// labelled correctly during part of the trace
Expand Down
8 changes: 6 additions & 2 deletions dart/biomechanics/MarkerFixer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -143,7 +143,7 @@ class RippleReductionProblem
void interpolateMissingPoints();

std::vector<std::map<std::string, Eigen::Vector3s>> smooth(
MarkersErrorReport* report = nullptr);
MarkersErrorReport* report = nullptr, bool useSparse = true, bool useIterativeSolver = true, int solverIterations = 1e5);

void saveToGUI(std::string markerName, std::string path);

Expand All @@ -163,7 +163,11 @@ class MarkerFixer
std::vector<std::map<std::string, Eigen::Vector3s>>
markerObservations,
s_t dt,
bool dropProlongedStillness = false);
bool dropProlongedStillness = false,
bool rippleReduce = true,
bool rippleReduceUseSparse = true,
bool rippleReduceUseIterativeSolver = true,
int rippleReduceSolverIterations = 1e5);
};

} // namespace biomechanics
Expand Down
6 changes: 5 additions & 1 deletion python/_nimblephysics/biomechanics/MarkerFitter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -398,7 +398,11 @@ void MarkerFitter(py::module& m)
"generateDataErrorsReport",
&dart::biomechanics::MarkerFitter::generateDataErrorsReport,
::py::arg("markerObservations"),
::py::arg("dt"))
::py::arg("dt"),
::py::arg("rippleReduce") = true,
::py::arg("rippleReduceUseSparse") = true,
::py::arg("rippleReduceUseIterativeSolver") = true,
::py::arg("rippleReduceSolverIterations") = 1e5)
.def(
"checkForFlippedMarkers",
&dart::biomechanics::MarkerFitter::checkForFlippedMarkers,
Expand Down
6 changes: 5 additions & 1 deletion python/_nimblephysics/biomechanics/MarkerFixer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,11 @@ void MarkerFixer(py::module& m)
&dart::biomechanics::MarkerFixer::generateDataErrorsReport,
::py::arg("immutableMarkerObservations"),
::py::arg("dt"),
::py::arg("dropProlongedStillness") = false);
::py::arg("dropProlongedStillness") = false,
::py::arg("rippleReduce") = true,
::py::arg("rippleReduceUseSparse") = true,
::py::arg("rippleReduceUseIterativeSolver") = true,
::py::arg("rippleReduceSolverIterations") = 1e5);
}

} // namespace python
Expand Down
4 changes: 2 additions & 2 deletions stubs/_nimblephysics-stubs/biomechanics/__init__.pyi
Original file line number Diff line number Diff line change
Expand Up @@ -1419,7 +1419,7 @@ class MarkerFitter():
def debugTrajectoryAndMarkersToGUI(self, server: nimblephysics_libs._nimblephysics.server.GUIWebsocketServer, init: MarkerInitialization, markerObservations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], forcePlates: typing.List[ForcePlate] = None, goldOsim: OpenSimFile = None, goldPoses: numpy.ndarray[numpy.float64, _Shape[m, n]] = array([], shape=(0, 0), dtype=float64)) -> None: ...
def findJointCenters(self, initializations: MarkerInitialization, newClip: typing.List[bool], markerObservations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]]) -> None: ...
def fineTuneWithIMU(self, accObservations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], gyroObservations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], markerObservations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], newClip: typing.List[bool], init: MarkerInitialization, dt: float, weightAccs: float = 1.0, weightGyros: float = 1.0, weightMarkers: float = 100.0, regularizePoses: float = 1.0, useIPOPT: bool = True, iterations: int = 300, lbfgsMemory: int = 100) -> MarkerInitialization: ...
def generateDataErrorsReport(self, markerObservations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], dt: float) -> MarkersErrorReport: ...
def generateDataErrorsReport(self, markerObservations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], dt: float, rippleReduce: bool = True, rippleReduceUseSparse: bool = True, rippleReduceUseIterativeSolver: bool = True, rippleReduceSolverIterations: int = 100000.0) -> MarkersErrorReport: ...
def getIMUFineTuneProblem(self, accObservations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], gyroObservations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], markerObservations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], init: MarkerInitialization, dt: float, start: int, end: int) -> IMUFineTuneProblem: ...
def getImuList(self) -> typing.List[typing.Tuple[nimblephysics_libs._nimblephysics.dynamics.BodyNode, nimblephysics_libs._nimblephysics.math.Isometry3]]: ...
def getImuMap(self) -> typing.Dict[str, typing.Tuple[nimblephysics_libs._nimblephysics.dynamics.BodyNode, nimblephysics_libs._nimblephysics.math.Isometry3]]: ...
Expand Down Expand Up @@ -1618,7 +1618,7 @@ class MarkerFitterState():
pass
class MarkerFixer():
@staticmethod
def generateDataErrorsReport(immutableMarkerObservations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], dt: float, dropProlongedStillness: bool = False) -> MarkersErrorReport: ...
def generateDataErrorsReport(immutableMarkerObservations: typing.List[typing.Dict[str, numpy.ndarray[numpy.float64, _Shape[3, 1]]]], dt: float, dropProlongedStillness: bool = False, rippleReduce: bool = True, rippleReduceUseSparse: bool = True, rippleReduceUseIterativeSolver: bool = True, rippleReduceSolverIterations: int = 100000.0) -> MarkersErrorReport: ...
pass
class MarkerInitialization():
@property
Expand Down

0 comments on commit f9dcc93

Please sign in to comment.