From ebe2c029753ed3b89a7500e6af7ee9bb0f621163 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Sat, 19 Sep 2020 13:11:59 +0200
Subject: [PATCH] implemented ctest ok
---
include/core/ceres_wrapper/solver_ceres.h | 4 +--
include/core/solver/solver_manager.h | 4 +--
src/ceres_wrapper/solver_ceres.cpp | 31 ++++++++++++++---------
test/dummy/solver_manager_dummy.h | 4 +--
4 files changed, 25 insertions(+), 18 deletions(-)
diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index a32f91a7f..210a1aad9 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 bdd8a37aa..3f1ae77d7 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 0f058b107..28ef3943c 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 5faef11f3..885f955d2 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
--
GitLab