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

Made output more directly comparable with ceres

release/4.3a0
Frank Dellaert 2015-07-04 19:14:25 -07:00
parent b752f8446c
commit a305218bd9
1 changed files with 15 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -25,6 +25,9 @@
#include <boost/algorithm/string.hpp> #include <boost/algorithm/string.hpp>
#include <boost/range/adaptor/map.hpp> #include <boost/range/adaptor/map.hpp>
#include <boost/format.hpp>
#include <boost/timer/timer.hpp>
#include <fstream> #include <fstream>
#include <limits> #include <limits>
#include <string> #include <string>
@ -236,7 +239,7 @@ void LevenbergMarquardtOptimizer::iterate() {
// Keep increasing lambda until we make make progress // Keep increasing lambda until we make make progress
while (true) { while (true) {
boost::timer::cpu_timer timer;
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "trying lambda = " << state_.lambda << endl; cout << "trying lambda = " << state_.lambda << endl;
@ -248,7 +251,7 @@ void LevenbergMarquardtOptimizer::iterate() {
double modelFidelity = 0.0; double modelFidelity = 0.0;
bool step_is_successful = false; bool step_is_successful = false;
bool stopSearchingLambda = false; bool stopSearchingLambda = false;
double newError = numeric_limits<double>::infinity(); double newError = numeric_limits<double>::infinity(), costChange;
Values newValues; Values newValues;
VectorValues delta; VectorValues delta;
@ -261,8 +264,6 @@ void LevenbergMarquardtOptimizer::iterate() {
systemSolvedSuccessfully = false; systemSolvedSuccessfully = false;
} }
double linearizedCostChange = 0,
newlinearizedError = 0;
if (systemSolvedSuccessfully) { if (systemSolvedSuccessfully) {
state_.reuseDiagonal = true; state_.reuseDiagonal = true;
@ -272,9 +273,9 @@ void LevenbergMarquardtOptimizer::iterate() {
delta.print("delta"); delta.print("delta");
// cost change in the linearized system (old - new) // cost change in the linearized system (old - new)
newlinearizedError = linear->error(delta); double newlinearizedError = linear->error(delta);
linearizedCostChange = state_.error - newlinearizedError; double linearizedCostChange = state_.error - newlinearizedError;
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "newlinearizedError = " << newlinearizedError << cout << "newlinearizedError = " << newlinearizedError <<
" linearizedCostChange = " << linearizedCostChange << endl; " linearizedCostChange = " << linearizedCostChange << endl;
@ -299,7 +300,7 @@ void LevenbergMarquardtOptimizer::iterate() {
<< ") new (tentative) error (" << newError << ")" << endl; << ") new (tentative) error (" << newError << ")" << endl;
// cost change in the original, nonlinear system (old - new) // cost change in the original, nonlinear system (old - new)
double costChange = state_.error - newError; costChange = state_.error - newError;
if (linearizedCostChange > 1e-20) { // the (linear) 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
@ -322,9 +323,13 @@ void LevenbergMarquardtOptimizer::iterate() {
} }
if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) { if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) {
cout << "[" << state_.iterations << "]: " << "new error = " << newlinearizedError // do timing
<< ", delta = " << linearizedCostChange << ", lambda = " << state_.lambda double iterationTime = 1e-9 * timer.elapsed().wall;
<< ", success = " << systemSolvedSuccessfully << std::endl; if (state_.iterations == 0)
cout << "iter cost cost_change lambda success iter_time" << endl;
cout << boost::format("% 4d % 8e % 3.2e % 3.2e % 4d % 3.2e") %
state_.iterations % newError % costChange % state_.lambda %
systemSolvedSuccessfully % iterationTime << endl;
} }
++state_.totalNumberInnerIterations; ++state_.totalNumberInnerIterations;