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

added minModelFidelity (use if slightly different from CERES)

release/4.3a0
Luca 2014-02-12 16:47:27 -05:00
parent d13ef17ce8
commit 89d7e61ca9
2 changed files with 9 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -99,7 +99,7 @@ void LevenbergMarquardtOptimizer::iterate() {
cout << "linearizing = " << endl; cout << "linearizing = " << endl;
GaussianFactorGraph::shared_ptr linear = linearize(); GaussianFactorGraph::shared_ptr linear = linearize();
double modelFidelity = std::numeric_limits<size_t>::max(); double modelFidelity = 0.0;
// Keep increasing lambda until we make make progress // Keep increasing lambda until we make make progress
while (true) { while (true) {
@ -158,23 +158,20 @@ void LevenbergMarquardtOptimizer::iterate() {
// cost change in the original, possibly nonlinear system (old - new) // cost change in the original, possibly nonlinear system (old - new)
double costChange = state_.error - error; double costChange = state_.error - error;
std::cout << "costChange " << costChange << std::endl;
// cost change in the linearized system (old - new) // cost change in the linearized system (old - new)
std::cout << "graph_ " << graph_.size() << std::endl;
std::cout << "linear " << linear->size() << std::endl;
linear->print("linear");
std::cout << "linear->error(delta) " << linear->error(delta) << std::endl;
double linearizedCostChange = state_.error - linear->error(delta); double linearizedCostChange = state_.error - linear->error(delta);
std::cout << "linearizedCostChange " << linearizedCostChange << std::endl;
// checking similarity between change in original and linearized cost
modelFidelity = costChange / linearizedCostChange; modelFidelity = costChange / linearizedCostChange;
std::cout << "modelFidelity " << modelFidelity << std::endl;
if (error <= state_.error) { if (error < state_.error) {
state_.values.swap(newValues); state_.values.swap(newValues);
state_.error = error; state_.error = error;
decreaseLambda(modelFidelity); if(modelFidelity > params_.minModelFidelity)
decreaseLambda(modelFidelity);
else
increaseLambda(modelFidelity);
break; break;
} else { } else {
// Either we're not cautious, or the same lambda was worse than the current error. // Either we're not cautious, or the same lambda was worse than the current error.

3
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -50,11 +50,12 @@ public:
double lambdaLowerBound; ///< The minimum lambda used in LM (default: 0) double lambdaLowerBound; ///< The minimum lambda used in LM (default: 0)
VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity VerbosityLM verbosityLM; ///< The verbosity level for Levenberg-Marquardt (default: SILENT), see also NonlinearOptimizerParams::verbosity
bool disableInnerIterations; ///< If enabled inner iterations on the linearized system are performed bool disableInnerIterations; ///< If enabled inner iterations on the linearized system are performed
double minModelFidelity; ///< Lower bound for the modelFidelity to accept the result of an LM iteration
std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda] std::string logFile; ///< an optional CSV log file, with [iteration, time, error, labda]
LevenbergMarquardtParams() : LevenbergMarquardtParams() :
lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound(0.0), lambdaInitial(1e-5), lambdaFactor(10.0), lambdaUpperBound(1e5), lambdaLowerBound(0.0),
verbosityLM(SILENT), disableInnerIterations(false) { verbosityLM(SILENT), disableInnerIterations(false), minModelFidelity(1e-3) {
} }
virtual ~LevenbergMarquardtParams() { virtual ~LevenbergMarquardtParams() {
} }