12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

bug fix in LM

release/4.3a0
Luca 2014-05-02 15:01:42 -04:00
parent 15201bda0f
commit f1fb2c09a4
1 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -268,14 +268,17 @@ void LevenbergMarquardtOptimizer::iterate() {
// cost change in the original, nonlinear system (old - new) // cost change in the original, nonlinear system (old - new)
double costChange = state_.error - newError; double costChange = state_.error - newError;
if (linearizedCostChange > 1e-15) { // the error has to decrease to satisfy this condition if (linearizedCostChange > 1e-20) { // the (linear) error has to decrease to satisfy this condition
// fidelity of linearized model VS original system between // fidelity of linearized model VS original system between
modelFidelity = costChange / linearizedCostChange; modelFidelity = costChange / linearizedCostChange;
// if we decrease the error in the nonlinear system and modelFidelity is above threshold // if we decrease the error in the nonlinear system and modelFidelity is above threshold
step_is_successful = modelFidelity > params_.minModelFidelity; step_is_successful = modelFidelity > params_.minModelFidelity;
} else { if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
step_is_successful = true; // linearizedCostChange close to zero cout << "modelFidelity: " << modelFidelity << endl;
} }
//else {
// step_is_successful = true; // linearizedCostChange close to zero
//}
double minAbsoluteTolerance = params_.relativeErrorTol * state_.error; double minAbsoluteTolerance = params_.relativeErrorTol * state_.error;
// if the change is small we terminate // if the change is small we terminate