diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index 62e2ab1a2da15ecec5ea372150a3c83c8b6b3bea..0673207bc47e1873084e3f4d711628a67b322f3c 100644
--- a/include/core/ceres_wrapper/solver_ceres.h
+++ b/include/core/ceres_wrapper/solver_ceres.h
@@ -106,10 +106,10 @@ class SolverCeres : public SolverManager
std::unique_ptr<ceres::Problem>& getCeresProblem();
- bool computeCovariances(CovarianceBlocksToBeComputed _blocks
+ bool computeCovariancesDerived(CovarianceBlocksToBeComputed _blocks
= CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
- bool computeCovariances(const std::vector<StateBlockPtr>& st_list) override;
+ bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override;
bool hasConverged() override;
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 53fc181cb072d8eaf3e4ca4977dcef25c9ac6198..fb38a8be279f9ed6572c729fdebe1551bb99f4ae 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -71,11 +71,19 @@ class SolverManager
};
// PROFILING
- unsigned int n_solve_;
- std::chrono::microseconds acc_duration_manager_;
+ unsigned int n_solve_, n_cov_;
+ std::chrono::microseconds acc_duration_total_;
std::chrono::microseconds acc_duration_solver_;
- std::chrono::microseconds max_duration_manager_;
+ std::chrono::microseconds acc_duration_update_;
+ std::chrono::microseconds acc_duration_state_;
+ std::chrono::microseconds acc_duration_cov_;
+
+ std::chrono::microseconds max_duration_total_;
std::chrono::microseconds max_duration_solver_;
+ std::chrono::microseconds max_duration_update_;
+ std::chrono::microseconds max_duration_state_;
+ std::chrono::microseconds max_duration_cov_;
+
void printProfiling(std::ostream& stream = std::cout) const;
protected:
@@ -105,9 +113,13 @@ class SolverManager
*/
std::string solve(const ReportVerbosity report_level);
- virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0;
+ virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) final;
+
+ virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final;
+
+ virtual bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0;
- virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) = 0;
+ virtual bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0;
virtual bool hasConverged() = 0;
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index e0995c7be03642c74d9e75b4ce2f266a165d81f3..7f33435f917a913eb4bf9cbf7b541634ac2a8e1c 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -82,7 +82,7 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
return report;
}
-bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
+bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _blocks)
{
// update problem
update();
@@ -291,7 +291,7 @@ bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
return false;
}
-bool SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list)
+bool SolverCeres::computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list)
{
//std::cout << "SolverCeres: computing covariances..." << std::endl;
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index ba07f02ef4dd7d1e3def7a3f819cd0fb2a512514..99dfb8729c9bc5cb05f00758494571de9d52fc23 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -13,10 +13,17 @@ SolverManager::SolverManager(const ProblemPtr& _problem) :
SolverManager::SolverManager(const ProblemPtr& _problem,
const ParamsSolverPtr& _params) :
n_solve_(0),
- acc_duration_manager_(0),
+ n_cov_(0),
+ acc_duration_total_(0),
acc_duration_solver_(0),
- max_duration_manager_(0),
+ acc_duration_update_(0),
+ acc_duration_state_(0),
+ acc_duration_cov_(0),
+ max_duration_total_(0),
max_duration_solver_(0),
+ max_duration_update_(0),
+ max_duration_state_(0),
+ max_duration_cov_(0),
wolf_problem_(_problem),
params_(_params)
{
@@ -137,11 +144,15 @@ std::string SolverManager::solve()
std::string SolverManager::solve(const ReportVerbosity report_level)
{
- auto start_manager = std::chrono::high_resolution_clock::now();
+ auto start_total = std::chrono::high_resolution_clock::now();
n_solve_++;
// update problem
+ auto start_update = std::chrono::high_resolution_clock::now();
update();
+ auto duration_update = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_update);
+ acc_duration_update_ += duration_update;
+ max_duration_update_ = std::max(max_duration_update_,duration_update);
// call derived solver
auto start_solver = std::chrono::high_resolution_clock::now();
@@ -151,6 +162,7 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
max_duration_solver_ = std::max(max_duration_solver_,duration_solver);
// update StateBlocks with optimized state value.
+ 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.
@@ -162,14 +174,46 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
if (!stateblock_statevector.first->isFixed())
stateblock_statevector.first->setState(stateblock_statevector.second, false); // false = do not raise the flag state_updated_
}
+ auto duration_state = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_state);
+ acc_duration_state_ += duration_state;
+ max_duration_state_ = std::max(max_duration_state_,duration_state);
- auto duration_manager = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_manager);
- acc_duration_manager_ += duration_manager;
- max_duration_manager_ = std::max(max_duration_manager_,duration_manager);
+ auto duration_total = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_total);
+ acc_duration_total_ += duration_total;
+ max_duration_total_ = std::max(max_duration_total_,duration_total);
return report;
}
+
+bool SolverManager::computeCovariances(const CovarianceBlocksToBeComputed blocks)
+{
+ auto start_cov = std::chrono::high_resolution_clock::now();
+ n_cov_++;
+
+ auto ret = computeCovariancesDerived(blocks);
+
+ auto duration_cov = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov);
+ acc_duration_cov_ += duration_cov;
+ max_duration_cov_ = std::max(max_duration_cov_,duration_cov);
+
+ return ret;
+}
+
+bool SolverManager::computeCovariances(const std::vector<StateBlockPtr>& st_list)
+{
+ auto start_cov = std::chrono::high_resolution_clock::now();
+ n_cov_++;
+
+ auto ret = computeCovariancesDerived(st_list);
+
+ auto duration_cov = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start_cov);
+ acc_duration_cov_ += duration_cov;
+ max_duration_cov_ = std::max(max_duration_cov_,duration_cov);
+
+ return ret;
+}
+
void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
{
// Warning if adding an already added
@@ -564,15 +608,24 @@ bool SolverManager::check(std::string prefix) const
void SolverManager::printProfiling(std::ostream& _stream) const
{
_stream <<"\nSolverManager:"
- << "\n\ttotal time: " << 1e-6*(acc_duration_manager_ + acc_duration_solver_).count() << " s"
- << "\n\tmanager time: " << 1e-6*acc_duration_manager_.count() << " s"
- << "\n\tsolver time: " << 1e-6*acc_duration_solver_.count() << " s"
- << "\n\texecutions: " << n_solve_
- << "\n\taverage time: " << 1e-3*(acc_duration_manager_ + acc_duration_solver_).count() / n_solve_ << " ms"
- << "\n\taverage manager time: " << 1e-3*acc_duration_manager_.count() / n_solve_ << " ms"
- << "\n\taverage solver time: " << 1e-3*acc_duration_solver_.count() / n_solve_ << " ms"
- << "\n\tmax manager time: " << 1e-3*max_duration_manager_.count() << " ms"
- << "\n\tmax solver time: " << 1e-3*max_duration_solver_.count() << " ms" << std::endl;
+ <<"\nTotal values:"
+ << "\n\tSolving state: " << 1e-6*acc_duration_total_.count() << " s - executions: " << n_solve_
+ << "\n\t\tUpdate problem: " << 1e-6*acc_duration_update_.count() << " s" << " (" << 100.0 * acc_duration_update_.count() / acc_duration_total_.count() << " %)"
+ << "\n\t\tSolver: " << 1e-6*acc_duration_solver_.count() << " s" << " (" << 100.0 * acc_duration_solver_.count() / acc_duration_total_.count() << " %)"
+ << "\n\t\tUpdate state: " << 1e-6*acc_duration_state_.count() << " s" << " (" << 100.0 * acc_duration_state_.count() / acc_duration_total_.count() << " %)"
+ << "\n\tSolving covariance: " << 1e-6*acc_duration_cov_.count() << " s - executions: " << n_cov_
+ <<"\nAverage:"
+ << "\n\tSolving state: " << 1e-3*acc_duration_total_.count() / n_solve_ << " ms"
+ << "\n\t\tUpdate problem: " << 1e-3*acc_duration_update_.count() / n_solve_ << " ms"
+ << "\n\t\tSolver: " << 1e-3*acc_duration_solver_.count() / n_solve_ << " ms"
+ << "\n\t\tUpdate state: " << 1e-3*acc_duration_state_.count() / n_solve_ << " ms"
+ << "\n\tSolving covariance: " << 1e-3*acc_duration_cov_.count() / n_cov_ << " ms"
+ <<"\nMax values:"
+ << "\n\tSolving state: " << 1e-3*max_duration_total_.count() << " ms"
+ << "\n\t\tUpdate problem: " << 1e-3*max_duration_update_.count() << " ms"
+ << "\n\t\tSolver: " << 1e-3*max_duration_solver_.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;
}
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
index 885f955d217b5a6afcd8084ab5d951af26e5b374..aab56cd1ab532a6a7d3e4b876f2de6dea0ea1b7b 100644
--- a/test/dummy/solver_manager_dummy.h
+++ b/test/dummy/solver_manager_dummy.h
@@ -51,8 +51,8 @@ class SolverManagerDummy : public SolverManager
return factors_derived_.size();
};
- bool computeCovariances(const CovarianceBlocksToBeComputed blocks) override {return false;};
- bool computeCovariances(const std::vector<StateBlockPtr>& st_list) override {return false;};
+ bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) override {return false;};
+ bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override {return false;};
// The following are dummy implementations