diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4a288d3fcdff6b78df77d38ba46025efcc4497e2..176bdf1a2db64c9aac6da0dfb421d10c65fb8ba3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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_RELEASE "-O3 -D_REENTRANT")
-#Set compiler according C++11 support
+#Set compiler according C++14 support
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
if(COMPILER_SUPPORTS_CXX14)
- message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.")
- set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
+ message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has C++14 support.")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
else()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
endif()
diff --git a/Testing/Temporary/.gitignore b/Testing/Temporary/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..6ccdef7c09c93207bc46508bb9fd8c873e94e6c9
--- /dev/null
+++ b/Testing/Temporary/.gitignore
@@ -0,0 +1,2 @@
+/CTestCostData.txt
+/LastTest.log
diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml
index c46580effcef4d12a9e4829d6a1c9cfdb8794da3..55eb56a880cd3d407cf631ce7e052e135e9bacce 100644
--- a/demos/hello_wolf/yaml/hello_wolf_config.yaml
+++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml
@@ -22,7 +22,7 @@ config:
max_num_iterations: 100
verbose: 0
period: 0.2
- update_immediately: false
+ interrupt_on_problem_change: false
n_threads: 2
compute_cov: false
minimizer: LEVENBERG_MARQUARDT
diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index e638c0850cfcac4bdab7472ed836a37fe271e7f8..da0ac085504dfe02d64ee7c0b2c4d30b0b029adb 100644
--- a/include/core/ceres_wrapper/solver_ceres.h
+++ b/include/core/ceres_wrapper/solver_ceres.h
@@ -43,7 +43,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres);
struct ParamsCeres : public ParamsSolver
{
- bool update_immediately = false;
+ bool interrupt_on_problem_change = false;
int min_num_iterations = 1;
ceres::Solver::Options solver_options;
ceres::Problem::Options problem_options;
@@ -60,9 +60,9 @@ struct ParamsCeres : public ParamsSolver
{
loadHardcodedValues();
- // stop solver whenever the problem is updated (via ceres::iterationCallback)
- update_immediately = _server.getParam<bool>(prefix + "update_immediately");
- if (update_immediately)
+ // interrupt solver whenever the problem is updated (via ceres::iterationCallback)
+ interrupt_on_problem_change = _server.getParam<bool>(prefix + "interrupt_on_problem_change");
+ if (interrupt_on_problem_change)
min_num_iterations = _server.getParam<int>(prefix + "min_num_iterations");
// CERES SOLVER OPTIONS
@@ -139,6 +139,10 @@ class SolverCeres : public SolverManager
ParamsCeresPtr params_ceres_;
+ // profiling
+ unsigned int n_iter_acc_, n_iter_max_;
+ unsigned int n_convergence_, n_interrupted_, n_no_convergence_;
+
public:
SolverCeres(const ProblemPtr& _wolf_problem);
@@ -160,13 +164,10 @@ class SolverCeres : public SolverManager
bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override;
bool hasConverged() override;
-
- SizeStd iterations() override;
-
+ bool wasStopped() override;
+ unsigned int iterations() override;
double initialCost() override;
-
double finalCost() override;
-
double totalTime() override;
ceres::Solver::Options& getSolverOptions();
@@ -207,6 +208,8 @@ class SolverCeres : public SolverManager
const LocalParametrizationBasePtr& local_param) override;
bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override;
+
+ void printProfilingDerived(std::ostream& _stream) const override;
};
inline ceres::Solver::Summary SolverCeres::getSummary()
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 8abc538d875763d1a8b50e82ef213420faaefc37..14f8588b7d01715a6ffb3dcd2e8be8d119aff78d 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -197,7 +197,6 @@ class Problem : public std::enable_shared_from_this<Problem>
void removeMotionProvider(MotionProviderPtr proc);
public:
-// MotionProviderPtr getMotionProvider();
std::map<int,MotionProviderPtr>& getMotionProviderMap();
const std::map<int,MotionProviderPtr>& getMotionProviderMap() const;
@@ -443,13 +442,6 @@ inline bool Problem::isPriorSet() const
return prior_options_ == nullptr;
}
-//inline MotionProviderPtr Problem::getMotionProvider()
-//{
-// if (not motion_provider_map_.empty())
-// return motion_provider_map_.begin()->second;
-// return nullptr;
-//}
-
inline std::map<int,MotionProviderPtr>& Problem::getMotionProviderMap()
{
return motion_provider_map_;
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 40575577f63bb634f444730ecb23fa7352fa1a86..c98804624b249ef71faddf8509116ee35bd937e0 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -142,19 +142,6 @@ class SolverManager
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
@@ -214,22 +201,37 @@ class SolverManager
virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final;
virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final;
+ // Derived virtual functions
protected:
- // Derived virtual functions
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 removeFactorDerived(const FactorBasePtr& fac_ptr) = 0;
virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0;
virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;
+
virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_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 hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0;
virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
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)
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index aaf0c0970c2fe395d80181b7f9f115af46fb7e5d..aed7fef36c99f71de9b664fa456620c7ef6523cd 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/src/ceres_wrapper/solver_ceres.cpp
@@ -41,11 +41,16 @@ SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem,
const ParamsCeresPtr& _params)
: SolverManager(_wolf_problem, _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);
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_,
params_ceres_->min_num_iterations,
params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET));
@@ -74,24 +79,6 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
// run Ceres Solver
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;
//return report
@@ -100,6 +87,19 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
else if (report_level == ReportVerbosity::FULL)
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;
}
@@ -182,17 +182,6 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
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;
}
case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
@@ -618,9 +607,16 @@ bool SolverCeres::hasConverged()
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()
@@ -817,6 +813,20 @@ bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st,
== 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
#include "core/solver/factory_solver.h"
namespace wolf {
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 787619db07741aa4ab7fd8228aaa37787885bbd6..8d5858a6e0045372d9dfb75a702f29c018ba9d22 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -969,42 +969,6 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
}
return success;
-
-
-
-// bool success(true);
-// int i = 0, j = 0;
-//
-// const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec();
-//
-// // computing size
-// SizeEigen sz = 0;
-// for (const auto& sb : state_bloc_vec)
-// if (sb)
-// sz += sb->getLocalSize();
-//
-// // resizing
-// _covariance = Eigen::MatrixXs(sz, sz);
-//
-// // filling covariance
-//
-// for (const auto& sb_i : state_bloc_vec)
-// {
-// if (sb_i)
-// {
-// j = 0;
-// for (const auto& sb_j : state_bloc_vec)
-// {
-// if (sb_j)
-// {
-// success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
-// j += sb_j->getLocalSize();
-// }
-// }
-// i += sb_i->getLocalSize();
-// }
-// }
-// return success;
}
MapBasePtr Problem::getMap() const
@@ -1181,7 +1145,7 @@ void Problem::print(int _depth, bool _constr_by, bool _metric, bool _state_block
{
_stream << std::endl;
- _stream << "P: wolf tree status ---------------------" << std::endl;
+ _stream << "Wolf tree status ------------------------" << std::endl;
getHardware()->print(_depth, _constr_by, _metric, _state_blocks, _stream, "");
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 7c9732d767955f1f7a1e7601905c4d09b9dd5282..0e649ec5cce5c11ca522c21bab70c67855922839 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -458,7 +458,7 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
{
auto sb = getStateBlockDynamic(key);
if (sb)
- _stream << _tabs << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( "
+ _stream << _tabs << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << "] = ( "
<< std::setprecision(3) << sb->getState().transpose() << " )" << " @ " << sb << std::endl;
}
}
@@ -474,7 +474,7 @@ void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _st
{
const auto &sb = getStateBlockDynamic(key);
if (sb)
- _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
+ _stream << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
}
_stream << std::endl;
}
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 1dc258b880491badfd14e139555aae4382c48f87..e0168dd1bc83a6f349f32f31995d08a776fa5699 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -184,11 +184,6 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
// 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.
-
- //std::map<StateBlockPtr, Eigen::VectorXd>::iterator it = state_blocks_.begin(),
- // it_end = state_blocks_.end();
for (auto& stateblock_statevector : state_blocks_)
{
// Avoid usuless copies
@@ -662,6 +657,7 @@ void SolverManager::printProfiling(std::ostream& _stream) const
<< "\n\t\tUpdate state: " << 1e-3*max_duration_state_.count() << " ms"
<< "\n\tSolving covariance: " << 1e-3*max_duration_cov_.count() << " ms" << std::endl;
+ printProfilingDerived(_stream);
}
} // namespace wolf
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
index bd800ab2e907b3313afdc1717b3c11f2cd07b858..36cada937e0412330e7242d2092948807549fea9 100644
--- a/src/state_block/has_state_blocks.cpp
+++ b/src/state_block/has_state_blocks.cpp
@@ -84,7 +84,7 @@ void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream&
auto sb = getStateBlock(key);
if (sb)
_stream << _tabs << " " << key
- << "[" << (sb->isFixed() ? "Fix" : "Est")
+ << " [" << (sb->isFixed() ? "Fix" : "Est")
<< "] = ( " << std::setprecision(3) << sb->getState().transpose() << " )"
<< " @ " << sb << std::endl;
}
@@ -103,7 +103,7 @@ void HasStateBlocks::printState (bool _metric, bool _state_blocks, std::ostream&
const auto &sb = getStateBlock(key);
if (sb)
_stream << " " << key
- << "[" << (sb->isFixed() ? "Fix" : "Est")
+ << " [" << (sb->isFixed() ? "Fix" : "Est")
<< "] @ " << sb;
}
_stream << std::endl;
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
index d718fc9554c2dd9371f4b18550dea2bf17c4af34..9c4df89d8d7ef2a6c0d9fd8ee568ba9d71247178 100644
--- a/test/dummy/solver_manager_dummy.h
+++ b/test/dummy/solver_manager_dummy.h
@@ -77,11 +77,13 @@ class SolverManagerDummy : public SolverManager
// The following are dummy implementations
- bool hasConverged() override { return true; }
- SizeStd iterations() override { return 1; }
- double initialCost() override { return double(1); }
- double finalCost() override { return double(0); }
- double totalTime() override { return double(0); }
+ bool hasConverged() override { return true; }
+ bool wasStopped() override { return false; }
+ unsigned int iterations() override { return 1; }
+ double initialCost() override { return double(1); }
+ double finalCost() override { return double(0); }
+ double totalTime() override { return double(0); }
+ void printProfilingDerived(std::ostream& _stream) const override {}
protected:
diff --git a/test/yaml/solver.yaml b/test/yaml/solver.yaml
index ec6a06a9eaefa6163ef6c6bb7512ac00d24c7725..53a698323208a61dc63a35084985c13fdcebdd98 100644
--- a/test/yaml/solver.yaml
+++ b/test/yaml/solver.yaml
@@ -1,6 +1,6 @@
period: 1
verbose: 2
-update_immediately: false
+interrupt_on_problem_change: false
max_num_iterations: 1000
n_threads: 2
function_tolerance: 0.000001