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

implemented ctest ok

parent b9ae2e41
No related branches found
No related tags found
1 merge request!393Resolve "computeCovariances returning bool and not clearing"
Pipeline #5904 passed
...@@ -103,10 +103,10 @@ class SolverCeres : public SolverManager ...@@ -103,10 +103,10 @@ class SolverCeres : public SolverManager
std::unique_ptr<ceres::Problem>& getCeresProblem(); std::unique_ptr<ceres::Problem>& getCeresProblem();
void computeCovariances(CovarianceBlocksToBeComputed _blocks bool computeCovariances(CovarianceBlocksToBeComputed _blocks
= CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override; = 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; bool hasConverged() override;
......
...@@ -96,9 +96,9 @@ class SolverManager ...@@ -96,9 +96,9 @@ class SolverManager
*/ */
std::string solve(const ReportVerbosity report_level); 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; virtual bool hasConverged() = 0;
......
...@@ -82,14 +82,11 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level) ...@@ -82,14 +82,11 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
return report; return report;
} }
void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) bool SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
{ {
// update problem // update problem
update(); update();
// CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
wolf_problem_->clearCovariance();
// CREATE DESIRED COVARIANCES LIST // CREATE DESIRED COVARIANCES LIST
std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs; std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
std::vector<std::pair<const double*, const double*>> double_pairs; std::vector<std::pair<const double*, const double*>> double_pairs;
...@@ -214,6 +211,9 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) ...@@ -214,6 +211,9 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
// COMPUTE DESIRED COVARIANCES // COMPUTE DESIRED COVARIANCES
if (covariance_->Compute(double_pairs, ceres_problem_.get())) if (covariance_->Compute(double_pairs, ceres_problem_.get()))
{ {
// CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
wolf_problem_->clearCovariance();
// STORE DESIRED COVARIANCES // STORE DESIRED COVARIANCES
for (unsigned int i = 0; i < double_pairs.size(); i++) for (unsigned int i = 0; i < double_pairs.size(); i++)
{ {
...@@ -222,20 +222,20 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks) ...@@ -222,20 +222,20 @@ void SolverCeres::computeCovariances(const CovarianceBlocksToBeComputed _blocks)
wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov); wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
// std::cout << "covariance got switch: " << std::endl << cov << std::endl; // 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; //std::cout << "SolverCeres: computing covariances..." << std::endl;
// update problem // update problem
update(); update();
// CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
wolf_problem_->clearCovariance();
// CREATE DESIRED COVARIANCES LIST // CREATE DESIRED COVARIANCES LIST
std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs; std::vector<std::pair<StateBlockPtr, StateBlockPtr>> state_block_pairs;
std::vector<std::pair<const double*, const double*>> double_pairs; std::vector<std::pair<const double*, const double*>> double_pairs;
...@@ -260,6 +260,10 @@ void SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list) ...@@ -260,6 +260,10 @@ void SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list)
// COMPUTE DESIRED COVARIANCES // COMPUTE DESIRED COVARIANCES
if (covariance_->Compute(double_pairs, ceres_problem_.get())) if (covariance_->Compute(double_pairs, ceres_problem_.get()))
{
// CLEAR STORED COVARIANCE BLOCKS IN WOLF PROBLEM
wolf_problem_->clearCovariance();
// STORE DESIRED COVARIANCES // STORE DESIRED COVARIANCES
for (unsigned int i = 0; i < double_pairs.size(); i++) for (unsigned int i = 0; i < double_pairs.size(); i++)
{ {
...@@ -268,8 +272,11 @@ void SolverCeres::computeCovariances(const std::vector<StateBlockPtr>& st_list) ...@@ -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); 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; // std::cout << "covariance got from st_list: " << std::endl << cov << std::endl;
} }
else return true;
std::cout << "WARNING: Couldn't compute covariances!" << std::endl; }
WOLF_WARN("SolverCeres::computeCovariances: Couldn't compute covariances!");
return false;
} }
void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr) void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr)
......
...@@ -51,8 +51,8 @@ class SolverManagerDummy : public SolverManager ...@@ -51,8 +51,8 @@ class SolverManagerDummy : public SolverManager
return factors_derived_.size(); return factors_derived_.size();
}; };
void computeCovariances(const CovarianceBlocksToBeComputed blocks) override {}; bool computeCovariances(const CovarianceBlocksToBeComputed blocks) override {return false;};
void computeCovariances(const std::vector<StateBlockPtr>& st_list) override {}; bool computeCovariances(const std::vector<StateBlockPtr>& st_list) override {return false;};
// The following are dummy implementations // The following are dummy implementations
......
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