Skip to content
Snippets Groups Projects
Commit e5d8981b authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Merge branch '439-solvermanager-profiling-extra-results' into 'devel'

Resolve "SolverManager profiling extra results"

Closes #439

See merge request !435
parents e62baaf7 e56d0595
No related branches found
No related tags found
2 merge requests!436Release to start wolf public,!435Resolve "SolverManager profiling extra results"
Pipeline #8514 failed
...@@ -32,12 +32,12 @@ message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.") ...@@ -32,12 +32,12 @@ message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT") SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT") SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
#Set compiler according C++11 support #Set compiler according C++14 support
include(CheckCXXCompilerFlag) include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
if(COMPILER_SUPPORTS_CXX14) if(COMPILER_SUPPORTS_CXX14)
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.") message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
else() else()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.") message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
endif() endif()
......
/CTestCostData.txt
/LastTest.log
...@@ -43,7 +43,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres); ...@@ -43,7 +43,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres);
struct ParamsCeres : public ParamsSolver struct ParamsCeres : public ParamsSolver
{ {
bool update_immediately = false; bool interrupt_on_problem_change = false;
int min_num_iterations = 1; int min_num_iterations = 1;
ceres::Solver::Options solver_options; ceres::Solver::Options solver_options;
ceres::Problem::Options problem_options; ceres::Problem::Options problem_options;
...@@ -60,9 +60,9 @@ struct ParamsCeres : public ParamsSolver ...@@ -60,9 +60,9 @@ struct ParamsCeres : public ParamsSolver
{ {
loadHardcodedValues(); loadHardcodedValues();
// stop solver whenever the problem is updated (via ceres::iterationCallback) // interrupt solver whenever the problem is updated (via ceres::iterationCallback)
update_immediately = _server.getParam<bool>(prefix + "update_immediately"); interrupt_on_problem_change = _server.getParam<bool>(prefix + "interrupt_on_problem_change");
if (update_immediately) if (interrupt_on_problem_change)
min_num_iterations = _server.getParam<int>(prefix + "min_num_iterations"); min_num_iterations = _server.getParam<int>(prefix + "min_num_iterations");
// CERES SOLVER OPTIONS // CERES SOLVER OPTIONS
...@@ -139,6 +139,10 @@ class SolverCeres : public SolverManager ...@@ -139,6 +139,10 @@ class SolverCeres : public SolverManager
ParamsCeresPtr params_ceres_; ParamsCeresPtr params_ceres_;
// profiling
unsigned int n_iter_acc_, n_iter_max_;
unsigned int n_convergence_, n_interrupted_, n_no_convergence_;
public: public:
SolverCeres(const ProblemPtr& _wolf_problem); SolverCeres(const ProblemPtr& _wolf_problem);
...@@ -160,13 +164,10 @@ class SolverCeres : public SolverManager ...@@ -160,13 +164,10 @@ class SolverCeres : public SolverManager
bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override; bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override;
bool hasConverged() override; bool hasConverged() override;
bool wasStopped() override;
SizeStd iterations() override; unsigned int iterations() override;
double initialCost() override; double initialCost() override;
double finalCost() override; double finalCost() override;
double totalTime() override; double totalTime() override;
ceres::Solver::Options& getSolverOptions(); ceres::Solver::Options& getSolverOptions();
...@@ -207,6 +208,8 @@ class SolverCeres : public SolverManager ...@@ -207,6 +208,8 @@ class SolverCeres : public SolverManager
const LocalParametrizationBasePtr& local_param) override; const LocalParametrizationBasePtr& local_param) override;
bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override; bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override;
void printProfilingDerived(std::ostream& _stream) const override;
}; };
inline ceres::Solver::Summary SolverCeres::getSummary() inline ceres::Solver::Summary SolverCeres::getSummary()
......
...@@ -142,19 +142,6 @@ class SolverManager ...@@ -142,19 +142,6 @@ class SolverManager
virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final; virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final;
virtual bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0;
virtual bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0;
virtual bool hasConverged() = 0;
virtual SizeStd iterations() = 0;
virtual double initialCost() = 0;
virtual double finalCost() = 0;
virtual double totalTime() = 0;
/** /**
* \brief Updates solver's problem according to the wolf_problem * \brief Updates solver's problem according to the wolf_problem
...@@ -214,22 +201,37 @@ class SolverManager ...@@ -214,22 +201,37 @@ class SolverManager
virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final; virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final;
virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final; virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final;
// Derived virtual functions
protected: protected:
// Derived virtual functions
virtual std::string solveDerived(const ReportVerbosity report_level) = 0; virtual std::string solveDerived(const ReportVerbosity report_level) = 0;
virtual bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0;
virtual bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0;
virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0; virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0;
virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0; virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0;
virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0; virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0; virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0; virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0;
virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0; virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;
virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0; virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0;
virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0; virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
virtual bool checkDerived(std::string prefix="") const = 0;
virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) = 0; virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) = 0;
virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0;
virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
const LocalParametrizationBasePtr& local_param) = 0; const LocalParametrizationBasePtr& local_param) = 0;
virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0;
virtual void printProfilingDerived(std::ostream& stream = std::cout) const = 0;
virtual bool checkDerived(std::string prefix="") const = 0;
public:
virtual bool hasConverged() = 0;
virtual bool wasStopped() = 0;
virtual unsigned int iterations() = 0;
virtual double initialCost() = 0;
virtual double finalCost() = 0;
virtual double totalTime() = 0;
}; };
// Params (here becaure it needs of declaration of SolverManager::ReportVerbosity) // Params (here becaure it needs of declaration of SolverManager::ReportVerbosity)
......
...@@ -41,11 +41,16 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, ...@@ -41,11 +41,16 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem,
const ParamsCeresPtr& _params) const ParamsCeresPtr& _params)
: SolverManager(_wolf_problem, _params) : SolverManager(_wolf_problem, _params)
, params_ceres_(_params) , params_ceres_(_params)
, n_iter_acc_(0)
, n_iter_max_(0)
, n_convergence_(0)
, n_interrupted_(0)
, n_no_convergence_(0)
{ {
covariance_ = wolf::make_unique<ceres::Covariance>(params_ceres_->covariance_options); covariance_ = wolf::make_unique<ceres::Covariance>(params_ceres_->covariance_options);
ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options); ceres_problem_ = wolf::make_unique<ceres::Problem>(params_ceres_->problem_options);
if (params_ceres_->update_immediately) if (params_ceres_->interrupt_on_problem_change)
getSolverOptions().callbacks.push_back(new IterationUpdateCallback(wolf_problem_, getSolverOptions().callbacks.push_back(new IterationUpdateCallback(wolf_problem_,
params_ceres_->min_num_iterations, params_ceres_->min_num_iterations,
params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET)); params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET));
...@@ -74,24 +79,6 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level) ...@@ -74,24 +79,6 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
// run Ceres Solver // run Ceres Solver
ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_); ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_);
/*/ if termination by user (Iteration callback, update and solve again)
// solve until max iterations reached
if (params_ceres_->update_immediately)
{
auto max_num_iterations = getSolverOptions().max_num_iterations;
while (summary_.termination_type == ceres::USER_SUCCESS)
{
// decrease wasted iterations
getSolverOptions().max_num_iterations -= summary_.iterations.size();
if (getSolverOptions().max_num_iterations <= 0)
break;
// update and solve again
update();
ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_);
}
getSolverOptions().max_num_iterations = max_num_iterations;
}*/
std::string report; std::string report;
//return report //return report
...@@ -100,6 +87,19 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level) ...@@ -100,6 +87,19 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
else if (report_level == ReportVerbosity::FULL) else if (report_level == ReportVerbosity::FULL)
report = summary_.FullReport(); report = summary_.FullReport();
// PROFILING
// iterations (profiling)
n_iter_acc_ += iterations();
n_iter_max_ = std::max(n_iter_max_, iterations());
// convergence (profiling)
if (hasConverged())
n_convergence_++;
else if (wasStopped())
n_interrupted_++;
else
n_no_convergence_++;
return report; return report;
} }
...@@ -182,17 +182,6 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _ ...@@ -182,17 +182,6 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
break; break;
} }
} }
// // loop all marginals (PO marginals)
// for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++)
// {
// state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i]);
// state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i+1]);
// state_block_pairs.emplace_back(all_state_blocks[2*i+1],all_state_blocks[2*i+1]);
//
// double_pairs.emplace_back(all_state_blocks[2*i]->get(),all_state_blocks[2*i]->get());
// double_pairs.emplace_back(all_state_blocks[2*i]->get(),all_state_blocks[2*i+1]->get());
// double_pairs.emplace_back(all_state_blocks[2*i+1]->get(),all_state_blocks[2*i+1]->get());
// }
break; break;
} }
case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS: case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
...@@ -618,9 +607,16 @@ bool SolverCeres::hasConverged() ...@@ -618,9 +607,16 @@ bool SolverCeres::hasConverged()
return summary_.termination_type == ceres::CONVERGENCE; return summary_.termination_type == ceres::CONVERGENCE;
} }
SizeStd SolverCeres::iterations() bool SolverCeres::wasStopped()
{ {
return summary_.iterations.size(); return summary_.termination_type == ceres::USER_FAILURE or summary_.termination_type == ceres::USER_SUCCESS;
}
unsigned int SolverCeres::iterations()
{
if (summary_.num_successful_steps + summary_.num_unsuccessful_steps < 1)
return 0;
return summary_.num_successful_steps + summary_.num_unsuccessful_steps;
} }
double SolverCeres::initialCost() double SolverCeres::initialCost()
...@@ -817,6 +813,20 @@ bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st, ...@@ -817,6 +813,20 @@ bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st,
== state_blocks_local_param_.at(st).get(); == state_blocks_local_param_.at(st).get();
} }
void SolverCeres::printProfilingDerived(std::ostream& _stream) const
{
_stream << "Iterations:"
<< "\n\tUser-defined limit: " << params_ceres_->solver_options.max_num_iterations
<< "\n\tAverage iterations: " << n_iter_acc_ / n_solve_
<< "\n\tMax. iterations: " << n_iter_max_
<< "\nTermination:"
<< "\n\tConvergence: " << 1e2 * n_convergence_ / n_solve_ << " %"
<< "\n\tInterrupted by problem change: " << 1e2 * n_interrupted_ / n_solve_ << " %"
<< "\n\tMax. iterations reached: " << 1e2 * n_no_convergence_ / n_solve_ << " %"
<< std::endl;
}
} // namespace wolf } // namespace wolf
#include "core/solver/factory_solver.h" #include "core/solver/factory_solver.h"
namespace wolf { namespace wolf {
......
...@@ -184,11 +184,6 @@ std::string SolverManager::solve(const ReportVerbosity report_level) ...@@ -184,11 +184,6 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
// update StateBlocks with optimized state value. // update StateBlocks with optimized state value.
auto start_state = std::chrono::high_resolution_clock::now(); auto start_state = std::chrono::high_resolution_clock::now();
/// @todo whatif someone has changed the state notification during opti ??
/// JV: I do not see a problem here, the solver provides the optimal state given the factors, if someone changed the state during optimization, it will be overwritten by the optimal one.
//std::map<StateBlockPtr, Eigen::VectorXd>::iterator it = state_blocks_.begin(),
// it_end = state_blocks_.end();
for (auto& stateblock_statevector : state_blocks_) for (auto& stateblock_statevector : state_blocks_)
{ {
// Avoid usuless copies // Avoid usuless copies
...@@ -662,6 +657,7 @@ void SolverManager::printProfiling(std::ostream& _stream) const ...@@ -662,6 +657,7 @@ void SolverManager::printProfiling(std::ostream& _stream) const
<< "\n\t\tUpdate state: " << 1e-3*max_duration_state_.count() << " ms" << "\n\t\tUpdate state: " << 1e-3*max_duration_state_.count() << " ms"
<< "\n\tSolving covariance: " << 1e-3*max_duration_cov_.count() << " ms" << std::endl; << "\n\tSolving covariance: " << 1e-3*max_duration_cov_.count() << " ms" << std::endl;
printProfilingDerived(_stream);
} }
} // namespace wolf } // namespace wolf
...@@ -77,11 +77,13 @@ class SolverManagerDummy : public SolverManager ...@@ -77,11 +77,13 @@ class SolverManagerDummy : public SolverManager
// The following are dummy implementations // The following are dummy implementations
bool hasConverged() override { return true; } bool hasConverged() override { return true; }
SizeStd iterations() override { return 1; } bool wasStopped() override { return false; }
double initialCost() override { return double(1); } unsigned int iterations() override { return 1; }
double finalCost() override { return double(0); } double initialCost() override { return double(1); }
double totalTime() override { return double(0); } double finalCost() override { return double(0); }
double totalTime() override { return double(0); }
void printProfilingDerived(std::ostream& _stream) const override {}
protected: protected:
......
period: 1 period: 1
verbose: 2 verbose: 2
update_immediately: false interrupt_on_problem_change: false
max_num_iterations: 1000 max_num_iterations: 1000
n_threads: 2 n_threads: 2
function_tolerance: 0.000001 function_tolerance: 0.000001
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment