More recent SolverManager virtual functions added to QRManager (most as dummy functions)

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);
 };
 

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