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

Better debugging output for LM

release/4.3a0
Frank Dellaert 2013-11-06 06:29:42 +00:00
parent 411e934385
commit 9e0893d807
2 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -79,22 +79,22 @@ void LevenbergMarquardtOptimizer::iterate() {
gttic (LM_iterate); gttic (LM_iterate);
// Linearize graph
GaussianFactorGraph::shared_ptr linear = linearize();
// Pull out parameters we'll use // Pull out parameters we'll use
const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity; const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity;
const LevenbergMarquardtParams::VerbosityLM lmVerbosity = params_.verbosityLM; const LevenbergMarquardtParams::VerbosityLM lmVerbosity = params_.verbosityLM;
// Linearize graph
if(nloVerbosity >= NonlinearOptimizerParams::ERROR)
cout << "linearizing = " << endl;
GaussianFactorGraph::shared_ptr linear = linearize();
// Keep increasing lambda until we make make progress // Keep increasing lambda until we make make progress
while (true) { while (true) {
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "trying lambda = " << state_.lambda << endl;
++state_.totalNumberInnerIterations; ++state_.totalNumberInnerIterations;
// cout << "state_.totalNumberInnerIterations = " << state_.totalNumberInnerIterations << endl;
// Add prior-factors // Add prior-factors
// TODO: replace this dampening with a backsubstitution approach // TODO: replace this dampening with a backsubstitution approach
gttic(damp); gttic(damp);
if (lmVerbosity >= LevenbergMarquardtParams::DAMPED) cout << "building damped system" << endl;
GaussianFactorGraph dampedSystem = *linear; GaussianFactorGraph dampedSystem = *linear;
{ {
double sigma = 1.0 / std::sqrt(state_.lambda); double sigma = 1.0 / std::sqrt(state_.lambda);
@ -109,10 +109,11 @@ void LevenbergMarquardtOptimizer::iterate() {
} }
} }
gttoc(damp); gttoc(damp);
if (lmVerbosity >= LevenbergMarquardtParams::DAMPED) dampedSystem.print("damped");
// Try solving // Try solving
try { try {
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "trying lambda = " << state_.lambda << endl;
// Log current error/lambda to file // Log current error/lambda to file
if (!params_.logFile.empty()) { if (!params_.logFile.empty()) {
ofstream os(params_.logFile.c_str(), ios::app); ofstream os(params_.logFile.c_str(), ios::app);
@ -136,6 +137,7 @@ void LevenbergMarquardtOptimizer::iterate() {
// create new optimization state with more adventurous lambda // create new optimization state with more adventurous lambda
gttic (compute_error); gttic (compute_error);
if(nloVerbosity >= NonlinearOptimizerParams::ERROR) cout << "calculating error" << endl;
double error = graph_.error(newValues); double error = graph_.error(newValues);
gttoc(compute_error); gttoc(compute_error);

2
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -35,7 +35,7 @@ class GTSAM_EXPORT LevenbergMarquardtParams: public NonlinearOptimizerParams {
public: public:
/** See LevenbergMarquardtParams::lmVerbosity */ /** See LevenbergMarquardtParams::lmVerbosity */
enum VerbosityLM { enum VerbosityLM {
SILENT = 0, LAMBDA, TRYLAMBDA, TRYCONFIG, TRYDELTA, DAMPED SILENT = 0, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA
}; };
private: private: