From e8d32c03a1835ce8ba22ceceb9e0e026b52d7c88 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Tue, 31 Mar 2020 18:08:28 +0200
Subject: [PATCH] More recent SolverManager virtual functions added to
QRManager (most as dummy functions)
---
include/core/ceres_wrapper/qr_manager.h | 18 ++++++++-
src/ceres_wrapper/qr_manager.cpp | 50 +++++++++++++++++++++----
2 files changed, 59 insertions(+), 9 deletions(-)
diff --git a/include/core/ceres_wrapper/qr_manager.h b/include/core/ceres_wrapper/qr_manager.h
index 04332ec28..0afbdcbd1 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 a3e6ab52a..bebe8c031 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
--
GitLab