diff --git a/include/core/ceres_wrapper/qr_manager.h b/include/core/ceres_wrapper/qr_manager.h index 04332ec28e2e3168db8c0475d7de03fc6932b12f..0afbdcbd157a7430134a80bc3efaf825181ab1a8 100644 --- a/include/core/ceres_wrapper/qr_manager.h +++ b/include/core/ceres_wrapper/qr_manager.h @@ -39,11 +39,17 @@ class QRManager : public SolverManager virtual ~QRManager(); - virtual std::string solve(const unsigned int& _report_level); - virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override; virtual void computeCovariances(const std::vector<StateBlockPtr>& _sb_list) override; + + virtual bool hasConverged() override; + + virtual SizeStd iterations() override; + + virtual double initialCost() override; + + virtual double finalCost() override; Eigen::SparseMatrixd getHessian(); @@ -51,6 +57,8 @@ class QRManager : public SolverManager bool computeDecomposition(); + virtual std::string solveImpl(const ReportVerbosity report_level) override; + virtual void addFactor(const FactorBasePtr& _fac_ptr) override; virtual void removeFactor(const FactorBasePtr& _fac_ptr) override; @@ -61,6 +69,12 @@ class QRManager : public SolverManager virtual void updateStateBlockStatus(const StateBlockPtr& _st_ptr) override; + virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) override; ///////////// + + virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override; ///////////// + + virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override; ///////////// + void relinearizeFactor(FactorBasePtr _fac_ptr); }; diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp index a3e6ab52a5c07f80097d3eff09fc6afea5a2071c..bebe8c03113b38799be3cb5c9c8a102285664f92 100644 --- a/src/ceres_wrapper/qr_manager.cpp +++ b/src/ceres_wrapper/qr_manager.cpp @@ -27,11 +27,8 @@ QRManager::~QRManager() fac_2_row_.clear(); } -std::string QRManager::solve(const unsigned int& _report_level) +std::string QRManager::solveImpl(const SolverManager::ReportVerbosity _report_level) { - // check for update notifications - update(); - // Decomposition if (!computeDecomposition()) return std::string("decomposition failed\n"); @@ -43,7 +40,7 @@ std::string QRManager::solve(const unsigned int& _report_level) // update state blocks for (auto sb_pair : sb_2_col_) { - StateBlockPtr sb = sb; + StateBlockPtr sb = sb_pair.first; Eigen::VectorXd sb_x_incr = x_incr.segment(sb_pair.second, sb->getLocalSize()); if (sb->hasLocalParametrization()){ Eigen::Map<const Eigen::VectorXd> sb_state_map(sb->getStateData(), sb->getSize()); @@ -59,9 +56,9 @@ std::string QRManager::solve(const unsigned int& _report_level) } } - if (_report_level == 1) + if (_report_level == SolverManager::ReportVerbosity::BRIEF) return std::string("Success!\n"); - else if (_report_level == 2) + else if (_report_level == SolverManager::ReportVerbosity::FULL) return std::string("Success!\n"); return std::string(); @@ -109,6 +106,27 @@ void QRManager::computeCovariances(const std::vector<StateBlockPtr>& _sb_list) } } +bool QRManager::hasConverged() +{ + return true; +} + +SizeStd QRManager::iterations() +{ + return -1; +} + +double QRManager::initialCost() +{ + return 1; +} + +double QRManager::finalCost() +{ + return 0; +} + + bool QRManager::computeDecomposition() { if (pending_changes_) @@ -223,6 +241,24 @@ void QRManager::updateStateBlockStatus(const StateBlockPtr& _st_ptr) addStateBlock(_st_ptr); } + +void QRManager::updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) +{ + // NOT IMPLEMENTED +} + +bool QRManager::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) +{ + auto it = sb_2_col_.find(state_ptr); + return it != sb_2_col_.end(); +} + +bool QRManager::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const +{ + auto it = fac_2_row_.find(fac_ptr); + return it != fac_2_row_.end(); +} + void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr) // FIXME consider the Jacobian 'M' of the local param of each SB in the factor { // evaluate factor