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

Fix LM linearizedCostChange for robust noise models 

When robust noise models are used, the linearized error is not
the same as the nonlinear error even at zero delta.

In the L2 loss case, the rho(error) = 0.5 * error^2
which is the same as 0.5 *(sqrt(weight)*error)^2
since weight is always 1.0;

However this is not the case with other loss functions since in general,
rho(error) != 0.5 * weight * error^2

This PR fixes the LevenbergMarquardtOptimizer to explicityly compute
the linear cost change.
release/4.3a0
Michael Bosse 2019-12-10 13:05:18 -08:00 committed by Mike Bosse
parent 7c255010e4
commit 63ef77156e
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

@ -152,6 +152,9 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
systemSolvedSuccessfully = false;
}
// 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));
if (systemSolvedSuccessfully) {
if (verbose)
cout << "linear delta norm = " << delta.norm() << endl;
@ -161,7 +164,7 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
// cost change in the linearized system (old - new)
double newlinearizedError = linear.error(delta);
double linearizedCostChange = currentState->error - newlinearizedError;
double linearizedCostChange = oldLinearizedError - newlinearizedError;
if (verbose)
cout << "newlinearizedError = " << newlinearizedError
<< " linearizedCostChange = " << linearizedCostChange << endl;
@ -188,8 +191,8 @@ bool LevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear,
// cost change in the original, nonlinear system (old - new)
costChange = currentState->error - newError;
if (linearizedCostChange >
1e-20) { // the (linear) error has to decrease to satisfy this condition
if (linearizedCostChange > std::numeric_limits<double>::epsilon() * oldLinearizedError) {
// the (linear) error has to decrease to satisfy this condition
// fidelity of linearized model VS original system between
modelFidelity = costChange / linearizedCostChange;
// if we decrease the error in the nonlinear system and modelFidelity is above threshold