Merge branch '359-solvermanager-posponing-floating-state-blocks' into 'devel'

Resolve "SolverManager posponing "floating" state blocks"

Closes #359

See merge request !391

.gitignore

@@ -33,3 +33,4 @@ src/examples/map_apriltag_save.yaml
 build_release/
 .clangd
 wolfcore.found
+/wolf.found

include/core/ceres_wrapper/solver_ceres.h

@@ -116,6 +116,10 @@ class SolverCeres : public SolverManager
 
         const Eigen::SparseMatrixd computeHessian() const;
 
+        virtual int numStateBlocksDerived() const override;
+
+        virtual int numFactorsDerived() const override;
+
     protected:
 
         bool checkDerived(std::string prefix="") const override;
@@ -138,7 +142,14 @@ class SolverCeres : public SolverManager
 
         bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
 
-        bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override;
+        bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override;
+
+        bool isStateBlockFixedDerived(const StateBlockPtr& st) override;
+
+        bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+                                                const LocalParametrizationBasePtr& local_param) override;
+
+        bool hasLocalParametrizationDerived(const StateBlockPtr& st)  const override;
 };
 
 inline ceres::Solver::Summary SolverCeres::getSummary()
@@ -158,16 +169,35 @@ inline ceres::Solver::Options& SolverCeres::getSolverOptions()
 
 inline bool SolverCeres::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const
 {
-    return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end()
-            && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
+    return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() and
+           fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
 }
 
-inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr)
+inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const
 {
-    return state_blocks_local_param_.find(state_ptr) != state_blocks_local_param_.end()
-            && ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
+    return ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
 }
 
+inline bool SolverCeres::isStateBlockFixedDerived(const StateBlockPtr& st)
+{
+    return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st));
+};
+
+inline bool SolverCeres::hasLocalParametrizationDerived(const StateBlockPtr& st) const
+{
+    return state_blocks_local_param_.count(st) == 1;
+};
+
+inline int SolverCeres::numStateBlocksDerived() const
+{
+    return ceres_problem_->NumParameterBlocks();
+}
+
+inline int SolverCeres::numFactorsDerived() const
+{
+    return ceres_problem_->NumResidualBlocks();
+};
+
 } // namespace wolf
 
 #endif

include/core/solver/solver_manager.h

@@ -119,31 +119,47 @@ class SolverManager
 
         ReportVerbosity getVerbosity() const;
 
-        virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr);
+        virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final;
 
-        virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const;
+        virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final;
 
-        bool check(std::string prefix="") const;
+        virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final;
+        virtual bool isStateBlockFixed(const StateBlockPtr& st) final;
+
+        virtual bool hasThisLocalParametrization(const StateBlockPtr& st,
+                                                 const LocalParametrizationBasePtr& local_param) final;
+
+        virtual bool hasLocalParametrization(const StateBlockPtr& st) const final;
+
+        virtual int numFactors() const final;
+        virtual int numStateBlocks() const final;
+        virtual int numStateBlocksFloating() const final;
+
+        virtual int numFactorsDerived() const = 0;
+        virtual int numStateBlocksDerived() const = 0;
+
+        virtual bool check(std::string prefix="") const final;
 
     protected:
 
         std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
         std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_;
         std::set<FactorBasePtr> factors_;
+        std::set<StateBlockPtr> floating_state_blocks_;
 
         virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
         const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;
-        virtual double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
+        double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
 
     private:
         // SolverManager functions
-        void addFactor(const FactorBasePtr& fac_ptr);
-        void removeFactor(const FactorBasePtr& fac_ptr);
-        void addStateBlock(const StateBlockPtr& state_ptr);
-        void removeStateBlock(const StateBlockPtr& state_ptr);
-        void updateStateBlockState(const StateBlockPtr& state_ptr);
-        void updateStateBlockStatus(const StateBlockPtr& state_ptr);
-        void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr);
+        virtual void addFactor(const FactorBasePtr& fac_ptr) final;
+        virtual void removeFactor(const FactorBasePtr& fac_ptr) final;
+        virtual void addStateBlock(const StateBlockPtr& state_ptr) final;
+        virtual void removeStateBlock(const StateBlockPtr& state_ptr) final;
+        virtual void updateStateBlockState(const StateBlockPtr& state_ptr) final;
+        virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final;
+        virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final;
 
     protected:
         // Derived virtual functions
@@ -154,9 +170,13 @@ class SolverManager
         virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
         virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0;
         virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;
-        virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) = 0;
+        virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0;
         virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
         virtual bool checkDerived(std::string prefix="") const = 0;
+        virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) = 0;
+        virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+                                                        const LocalParametrizationBasePtr& local_param) = 0;
+        virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0;
 };
 
 // Params (here becaure it needs of declaration of SolverManager::ReportVerbosity)

src/ceres_wrapper/solver_ceres.cpp

@@ -579,6 +579,14 @@ const Eigen::SparseMatrixd SolverCeres::computeHessian() const
     return H;
 }
 
+bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st,
+                                                     const LocalParametrizationBasePtr& local_param)
+{
+    return state_blocks_local_param_.count(st) == 1
+            && state_blocks_local_param_.at(st)->getLocalParametrization() == local_param
+            && ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st))
+                    == state_blocks_local_param_.at(st).get();
+}
 
 } // namespace wolf
 #include "core/solver/factory_solver.h"

src/solver/solver_manager.cpp

@@ -56,12 +56,24 @@ void SolverManager::update()
 
         // remove
         else
-            removeStateBlock(sb_notification_map.begin()->first);
+        {
+            if (floating_state_blocks_.count(sb_notification_map.begin()->first) == 1)
+                floating_state_blocks_.erase(sb_notification_map.begin()->first);
+            else
+                removeStateBlock(sb_notification_map.begin()->first);
+        }
 
         // remove notification
         sb_notification_map.erase(sb_notification_map.begin());
     }
 
+    // ADD "floating" STATE BLOCKS (last update they weren't involved in any factor)
+    while (!floating_state_blocks_.empty())
+    {
+        addStateBlock(*floating_state_blocks_.begin());
+        floating_state_blocks_.erase(floating_state_blocks_.begin());
+    }
+
     // ADD FACTORS
     while (!fac_notification_map.empty())
     {
@@ -79,6 +91,14 @@ void SolverManager::update()
     {
         auto state_ptr = state_pair.first;
 
+        // Check for "floating" state blocks (estimated but not involved in any factor -> not observable problem)
+        if (state_blocks_2_factors_.at(state_ptr).empty())
+        {
+            WOLF_INFO("SolverManager::update(): 'Floating' StateBlock ", state_ptr, " (not involved in any factor) Storing it apart.");
+            floating_state_blocks_.insert(state_ptr);
+            continue;
+        }
+
         // state update
         if (state_ptr->stateUpdated())
             updateStateBlockState(state_ptr);
@@ -92,6 +112,16 @@ void SolverManager::update()
             updateStateBlockLocalParametrization(state_ptr);
     }
 
+    // REMOVE "floating" STATE BLOCKS (will be added next update() call)
+    for (auto state_ptr : floating_state_blocks_)
+    {
+        removeStateBlock(state_ptr);
+        // reset flags meaning "solver will handle this change" (state, fix and local param will be set in addStateBlock)
+        state_ptr->resetStateUpdated();
+        state_ptr->resetFixUpdated();
+        state_ptr->resetLocalParamUpdated();
+    }
+
     #ifdef _WOLF_DEBUG
         assert(check("after update()"));
     #endif
@@ -321,14 +351,67 @@ SolverManager::ReportVerbosity SolverManager::getVerbosity() const
     return params_->verbose;
 }
 
-bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr)
+bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr) const
+{
+    return isStateBlockFloating(state_ptr) or (state_blocks_.count(state_ptr) == 1 and isStateBlockRegisteredDerived(state_ptr));
+}
+
+bool SolverManager::isStateBlockFloating(const StateBlockPtr& state_ptr) const
 {
-    return state_blocks_.count(state_ptr) ==1 && isStateBlockRegisteredDerived(state_ptr);
+    return floating_state_blocks_.count(state_ptr) == 1;
 }
 
 bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
 {
-    return isFactorRegisteredDerived(fac_ptr);
+    return factors_.count(fac_ptr) and isFactorRegisteredDerived(fac_ptr);
+}
+
+bool SolverManager::isStateBlockFixed(const StateBlockPtr& st)
+{
+    if (!isStateBlockRegistered(st))
+        return false;
+
+    if (isStateBlockFloating(st))
+        return st->isFixed();
+
+    return isStateBlockFixedDerived(st);
+}
+
+bool SolverManager::hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
+{
+    if (!isStateBlockRegistered(st))
+        return false;
+
+    if (isStateBlockFloating(st))
+        return st->getLocalParametrization() == local_param;
+
+    return hasThisLocalParametrizationDerived(st, local_param);
+};
+
+bool SolverManager::hasLocalParametrization(const StateBlockPtr& st) const
+{
+    if (!isStateBlockRegistered(st))
+        return false;
+
+    if (isStateBlockFloating(st))
+        return st->hasLocalParametrization();
+
+    return hasLocalParametrizationDerived(st);
+};
+
+int SolverManager::numFactors() const
+{
+    return factors_.size();
+}
+
+int SolverManager::numStateBlocks() const
+{
+    return state_blocks_.size() + floating_state_blocks_.size();
+}
+
+int SolverManager::numStateBlocksFloating() const
+{
+    return floating_state_blocks_.size();
 }
 
 double SolverManager::getPeriod() const
@@ -357,15 +440,32 @@ bool SolverManager::check(std::string prefix) const
             ok = false;
         }
 
-        // factor involving state block in factors_
-        for (auto fac : sb_fac_it->second)
+        // no factors involving state block
+        if (sb_fac_it->second.empty())
+        {
+            WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is in state_blocks_ but not involved in any factor - in ", prefix);
+            ok = false;
+        }
+        else
         {
-            if (factors_.count(fac) == 0)
+            // factor involving state block in factors_
+            for (auto fac : sb_fac_it->second)
             {
-                WOLF_ERROR("SolverManager::check: factor ", fac->id(), " (involved in sb ", sb_fac_it->first, ") missing in factors_ map - in ", prefix);
-                ok = false;
+                if (factors_.count(fac) == 0)
+                {
+                    WOLF_ERROR("SolverManager::check: factor ", fac->id(), " (involved in sb ", sb_fac_it->first, ") missing in factors_ map - in ", prefix);
+                    ok = false;
+                }
             }
         }
+
+        // can't be in both state_blocks_ and floating_state_blocks_
+        if (floating_state_blocks_.count(sb_fac_it->first) == 1)
+        {
+            WOLF_ERROR("SolverManager::check: state block ", sb_fac_it->first, " is both in state_blocks_ and floating_state_blocks_ - in ", prefix);
+            ok = false;
+        }
+
         sb_vec_it++;
         sb_fac_it++;
     }
@@ -384,9 +484,38 @@ bool SolverManager::check(std::string prefix) const
         }
     }
 
-    // checkDerived
+    // CHECK DERIVED ----------------------
     ok &= checkDerived(prefix);
 
+    // CHECK IF DERIVED IS UP TO DATE ----------------------
+    // state blocks registered in derived solver
+    for (auto sb : state_blocks_)
+        if (!isStateBlockRegisteredDerived(sb.first))
+        {
+            WOLF_ERROR("SolverManager::check: state block ", sb.first, " is in state_blocks_ but not registered in derived solver - in ", prefix);
+            ok = false;
+        }
+
+    // factors registered in derived solver
+    for (auto fac : factors_)
+        if (!isFactorRegisteredDerived(fac))
+        {
+            WOLF_ERROR("SolverManager::check: factor ", fac->id(), " is in factors_ but not registered in derived solver - in ", prefix);
+            ok = false;
+        }
+
+    // numbers
+    if (numStateBlocksDerived() != state_blocks_.size())
+    {
+        WOLF_ERROR("SolverManager::check: numStateBlocksDerived() = ", numStateBlocksDerived(), " DIFFERENT THAN state_blocks_.size() = ", state_blocks_.size(), " - in ", prefix);
+        ok = false;
+    }
+    if (numFactorsDerived() != numFactors())
+    {
+        WOLF_ERROR("SolverManager::check: numFactorsDerived() = ", numFactorsDerived(), " DIFFERENT THAN numFactors() = ", numFactors(), " - in ", prefix);
+        ok = false;
+    }
+
     return ok;
 }
 

test/dummy/solver_manager_dummy.h

@@ -17,7 +17,7 @@ WOLF_PTR_TYPEDEFS(SolverManagerDummy);
 class SolverManagerDummy : public SolverManager
 {
     public:
-        std::list<FactorBasePtr> factors_;
+        std::set<FactorBasePtr> factors_derived_;
         std::map<StateBlockPtr,bool> state_block_fixed_;
         std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
 
@@ -26,35 +26,34 @@ class SolverManagerDummy : public SolverManager
         {
         };
 
-        bool isStateBlockRegistered(const StateBlockPtr& st) override
+        bool isStateBlockFixedDerived(const StateBlockPtr& st) override
         {
-            return state_blocks_.find(st)!=state_blocks_.end();
+            return state_block_fixed_.at(st);
         };
 
-        bool isStateBlockFixed(const StateBlockPtr& st) const
+        bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) override
         {
-            return state_block_fixed_.at(st);
+            return state_block_local_param_.at(st) == local_param;
         };
 
-        bool isFactorRegistered(const FactorBasePtr& fac_ptr) const override
+        bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override
         {
-            return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
+            return state_block_local_param_.at(st) != nullptr;
         };
 
-        bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const
+        virtual int numStateBlocksDerived() const override
         {
-            return state_block_local_param_.find(st) != state_block_local_param_.end() && state_block_local_param_.at(st) == local_param;
-        };
+            return state_block_fixed_.size();
+        }
 
-        bool hasLocalParametrization(const StateBlockPtr& st) const
+        virtual int numFactorsDerived() const override
         {
-            return state_block_local_param_.find(st) != state_block_local_param_.end();
+            return factors_derived_.size();
         };
 
         void computeCovariances(const CovarianceBlocksToBeComputed blocks) override {};
         void computeCovariances(const std::vector<StateBlockPtr>& st_list) override {};
-        bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) override {return true;};
-        bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override {return true;};
+
 
         // The following are dummy implementations
         bool    hasConverged() override  { return true;      }
@@ -62,19 +61,17 @@ class SolverManagerDummy : public SolverManager
         double  initialCost() override   { return double(1); }
         double  finalCost() override     { return double(0); }
 
-
-
     protected:
 
         bool checkDerived(std::string prefix="") const override {return true;}
         std::string solveDerived(const ReportVerbosity report_level) override { return std::string("");};
         void addFactorDerived(const FactorBasePtr& fac_ptr) override
         {
-            factors_.push_back(fac_ptr);
+            factors_derived_.insert(fac_ptr);
         };
         void removeFactorDerived(const FactorBasePtr& fac_ptr) override
         {
-            factors_.remove(fac_ptr);
+            factors_derived_.erase(fac_ptr);
         };
         void addStateBlockDerived(const StateBlockPtr& state_ptr) override
         {
@@ -92,10 +89,16 @@ class SolverManagerDummy : public SolverManager
         };
         void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override
         {
-            if (state_ptr->getLocalParametrization() == nullptr)
-                state_block_local_param_.erase(state_ptr);
-            else
-                state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+            state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
+        };
+        bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override
+        {
+            return state_block_fixed_.count(state_ptr) == 1 and state_block_local_param_.count(state_ptr) == 1;
+        };
+
+        bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override
+        {
+            return factors_derived_.count(fac_ptr) == 1;
         };
 };