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

Merge pull request #187 from michaelbosse/fix_LM_convergence_test 

Fix LM linearizedCostChange for robust noise models
release/4.3a0
Frank Dellaert 2020-02-29 12:25:07 -08:00 committed by GitHub
parent dd75dba801 1324cfd283
commit f02aa1bdd8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 11 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/linear/JacobianFactor.cpp
View File

@ -513,8 +513,10 @@ Vector JacobianFactor::error_vector(const VectorValues& c) const {
/* ************************************************************************* */ /* ************************************************************************* */
double JacobianFactor::error(const VectorValues& c) const { double JacobianFactor::error(const VectorValues& c) const {
Vector weighted = error_vector(c); Vector e = unweighted_error(c);
return 0.5 * weighted.dot(weighted); // Use the noise model distance function to get the correct error if available.
if (model_) return 0.5 * model_->distance(e);
return 0.5 * e.dot(e);
} }
/* ************************************************************************* */ /* ************************************************************************* */

11
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -158,10 +158,13 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
if (params_.verbosityLM >= LevenbergMarquardtParams::TRYDELTA) if (params_.verbosityLM >= LevenbergMarquardtParams::TRYDELTA)
delta.print("delta"); delta.print("delta");
// cost change in the linearized system (old - new) // Compute the old linearized error as it is not the same
// as the nonlinear error when robust noise models are used.
double oldLinearizedError = linear.error(VectorValues::Zero(delta));
double newlinearizedError = linear.error(delta); double newlinearizedError = linear.error(delta);
double linearizedCostChange = currentState->error - newlinearizedError; // cost change in the linearized system (old - new)
double linearizedCostChange = oldLinearizedError - newlinearizedError;
if (verbose) if (verbose)
cout << "newlinearizedError = " << newlinearizedError cout << "newlinearizedError = " << newlinearizedError
<< " linearizedCostChange = " << linearizedCostChange << endl; << " linearizedCostChange = " << linearizedCostChange << endl;
@ -188,8 +191,8 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
// cost change in the original, nonlinear system (old - new) // cost change in the original, nonlinear system (old - new)
costChange = currentState->error - newError; costChange = currentState->error - newError;
if (linearizedCostChange > if (linearizedCostChange > std::numeric_limits<double>::epsilon() * oldLinearizedError) {
1e-20) { // the (linear) error has to decrease to satisfy this condition // 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