From 951377c80f3cd2dc7cc0311ecb33e0298d88f8d0 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Fri, 16 Oct 2015 14:18:13 -0400 Subject: [PATCH 1/4] fix type errors and timer issue in LM optimizer --- gtsam/base/timing.h | 1 + gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 15 ++++++++------- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index d0bca4a9d..d1e90f63a 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -18,6 +18,7 @@ #pragma once #include +#include #include #include // for GTSAM_USE_TBB diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index ace35e530..ad976119a 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -22,11 +22,11 @@ #include #include #include +#include #include #include #include -#include #include #include @@ -239,7 +239,8 @@ void LevenbergMarquardtOptimizer::iterate() { // Keep increasing lambda until we make make progress while (true) { - boost::timer::cpu_timer timer; + gttic(iteration); + if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "trying lambda = " << state_.lambda << endl; @@ -322,14 +323,14 @@ void LevenbergMarquardtOptimizer::iterate() { } } + gttoc(iteration); + if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) { - // do timing - double iterationTime = 1e-9 * timer.elapsed().wall; 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") % + cout << "iter cost cost_change lambda success" << endl; + cout << boost::format("% 4d % 8e % 3.2e % 3.2e % 4d") % state_.iterations % newError % costChange % state_.lambda % - systemSolvedSuccessfully % iterationTime << endl; + systemSolvedSuccessfully << endl; } ++state_.totalNumberInnerIterations; From 971d2e519f55e6e97667833f0415af14ef6073ba Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Sat, 17 Oct 2015 18:21:48 -0400 Subject: [PATCH 2/4] gtsam type header only when using old boost timer --- gtsam/base/timing.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index d1e90f63a..b89e15637 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -18,7 +18,6 @@ #pragma once #include -#include #include #include // for GTSAM_USE_TBB @@ -118,6 +117,7 @@ # include #else # include +# include #endif #ifdef GTSAM_USE_TBB From 9628b9b1655b4331499512c4c23a913ca81cf4ad Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Mon, 19 Oct 2015 00:07:23 -0400 Subject: [PATCH 3/4] fix iteration timer in LM --- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index ad976119a..f0e240f21 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -239,7 +239,8 @@ void LevenbergMarquardtOptimizer::iterate() { // Keep increasing lambda until we make make progress while (true) { - gttic(iteration); + + gttic_(lm_iteration); if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "trying lambda = " << state_.lambda << endl; @@ -323,14 +324,17 @@ void LevenbergMarquardtOptimizer::iterate() { } } - gttoc(iteration); + gttoc_(lm_iteration); if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) { + // do timing + tictoc_getNode(iteration_timer, lm_iteration); + double iterationTime = iteration_timer->secs(); if (state_.iterations == 0) - cout << "iter cost cost_change lambda success" << endl; - cout << boost::format("% 4d % 8e % 3.2e % 3.2e % 4d") % + 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 << endl; + systemSolvedSuccessfully % iterationTime << endl; } ++state_.totalNumberInnerIterations; From 42d07a99ff3185485d085c02844070af42982195 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Mon, 19 Oct 2015 19:08:29 -0400 Subject: [PATCH 4/4] LM optimizer use boost raw timer --- .../nonlinear/LevenbergMarquardtOptimizer.cpp | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index f0e240f21..cacb0a1ff 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -240,7 +240,13 @@ void LevenbergMarquardtOptimizer::iterate() { // Keep increasing lambda until we make make progress while (true) { - gttic_(lm_iteration); +#ifdef GTSAM_USING_NEW_BOOST_TIMERS + boost::timer::cpu_timer lamda_iteration_timer; + lamda_iteration_timer.start(); +#else + boost::timer lamda_iteration_timer; + lamda_iteration_timer.restart(); +#endif if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "trying lambda = " << state_.lambda << endl; @@ -324,14 +330,16 @@ void LevenbergMarquardtOptimizer::iterate() { } } - gttoc_(lm_iteration); - if (lmVerbosity == LevenbergMarquardtParams::SUMMARY) { // do timing - tictoc_getNode(iteration_timer, lm_iteration); - double iterationTime = iteration_timer->secs(); +#ifdef GTSAM_USING_NEW_BOOST_TIMERS + double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall; +#else + double iterationTime = lamda_iteration_timer.elapsed(); +#endif 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;