diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h index a32f91a7f55247aaf102ac4f861bcf4b80456d41..210a1aad9b11724188c0d0951551d8bc680d572e 100644 --- a/include/core/ceres_wrapper/solver_ceres.h +++ b/include/core/ceres_wrapper/solver_ceres.h @@ -103,10 +103,10 @@ class SolverCeres : public SolverManager std::unique_ptr<ceres::Problem>& getCeresProblem(); - void computeCovariances(CovarianceBlocksToBeComputed _blocks + bool computeCovariances(CovarianceBlocksToBeComputed _blocks = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override; - void computeCovariances(const std::vector<StateBlockPtr>& st_list) override; + bool computeCovariances(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 bdd8a37aaa817b36609cc672402161f60a5fdba2..3f1ae77d736b8af1709516d5fb9c8a306b7a6b49 100644 --- a/include/core/solver/solver_manager.h +++ b/include/core/solver/solver_manager.h @@ -96,9 +96,9 @@ class SolverManager */ std::string solve(const ReportVerbosity report_level); - virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0; + virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0; - virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list) = 0; + virtual bool computeCovariances(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 0f058b107cb9355c93bb2852d73aac862faddcb5..28ef3943c7e55a2b2a8779a4b42d24b60df0a4ef 100644 --- a/src/ceres_wrapper/solver_ceres.cpp +++ b/src/ceres_wrapper/solver_ceres.cpp @@ -82,14 +82,11 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level) return report; } -void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) +bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) { // update problem update(); - // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM - wolf_problem_->clearCovariance(); - // CREATE DESIRED COVARIANCES LIST std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs; std::vector<std::pair<const double*, const double*>> double_pairs; @@ -214,6 +211,9 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) // COMPUTE DESIRED COVARIANCES if (covariance_->Compute(double_pairs, ceres_problem_.get())) { + // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM + wolf_problem_->clearCovariance(); + // STORE DESIRED COVARIANCES for (unsigned int i = 0; i < double_pairs.size(); i++) { @@ -222,20 +222,20 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov); // std::cout << "covariance got switch: " << std::endl << cov << std::endl; } + return true; } - else - std::cout << "WARNING: Couldn't compute covariances!" << std::endl; + + WOLF_WARN("SolverCeres::computeCovariances: Couldn't compute covariances!"); + return false; } -void SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list) + +bool SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list) { //std::cout << "SolverCeres: computing covariances..." << std::endl; // update problem update(); - // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM - wolf_problem_->clearCovariance(); - // CREATE DESIRED COVARIANCES LIST std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs; std::vector<std::pair<const double*, const double*>> double_pairs; @@ -260,6 +260,10 @@ void SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list) // COMPUTE DESIRED COVARIANCES if (covariance_->Compute(double_pairs, ceres_problem_.get())) + { + // CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM + wolf_problem_->clearCovariance(); + // STORE DESIRED COVARIANCES for (unsigned int i = 0; i < double_pairs.size(); i++) { @@ -268,8 +272,11 @@ void SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list) wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov); // std::cout << "covariance got from st_list: " << std::endl << cov << std::endl; } - else - std::cout << "WARNING: Couldn't compute covariances!" << std::endl; + return true; + } + + WOLF_WARN("SolverCeres::computeCovariances: Couldn't compute covariances!"); + return false; } void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr) diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h index 5faef11f35874747a5f0b98f8d99b42911d116e1..885f955d217b5a6afcd8084ab5d951af26e5b374 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(); }; - void computeCovariances(const CovarianceBlocksToBeComputed blocks) override {}; - void computeCovariances(const std::vector<StateBlockPtr>& st_list) override {}; + bool computeCovariances(const CovarianceBlocksToBeComputed blocks) override {return false;}; + bool computeCovariances(const std::vector<StateBlockPtr>& st_list) override {return false;}; // The following are dummy implementations