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

WIP

parent e7f4acd0
No related branches found
No related tags found
1 merge request!391Resolve "SolverManager posponing "floating" state blocks"
Pipeline #5894 failed
Stage: build
    Hide whitespace changes
    Inline Side-by-side
    include/core/ceres_wrapper/solver_ceres.h
    + 39
    − 0
    View file @ b2e6175f
    ...@@ -116,6 +116,10 @@ class SolverCeres : public SolverManager ...@@ -116,6 +116,10 @@ class SolverCeres : public SolverManager
    const Eigen::SparseMatrixd computeHessian() const; const Eigen::SparseMatrixd computeHessian() const;
    virtual int numStateBlocksDerived() const override;
    virtual int numFactorsDerived() const override;
    protected: protected:
    bool checkDerived(std::string prefix="") const override; bool checkDerived(std::string prefix="") const override;
    ...@@ -139,6 +143,13 @@ class SolverCeres : public SolverManager ...@@ -139,6 +143,13 @@ class SolverCeres : public SolverManager
    bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override; bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
    bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const 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() inline ceres::Solver::Summary SolverCeres::getSummary()
    ...@@ -168,6 +179,34 @@ inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& stat ...@@ -168,6 +179,34 @@ inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& stat
    && ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr)); && ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
    } }
    inline bool SolverCeres::isStateBlockFixedDerived(const StateBlockPtr& st)
    {
    return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st));
    };
    inline bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st,
    const LocalParametrizationBasePtr& local_param)
    {
    return state_blocks_local_param_.count(st) == 1 and
    state_blocks_local_param_.at(st)->getLocalParametrization() == local_param and
    ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
    };
    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 } // namespace wolf
    #endif #endif
    include/core/solver/solver_manager.h
    + 20
    − 1
    View file @ b2e6175f
    ...@@ -121,7 +121,22 @@ class SolverManager ...@@ -121,7 +121,22 @@ class SolverManager
    virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final; virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final;
    virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final;
    virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final; 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; virtual bool check(std::string prefix="") const final;
    ...@@ -134,7 +149,7 @@ class SolverManager ...@@ -134,7 +149,7 @@ class SolverManager
    virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr); virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
    const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const; const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;
    virtual double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr); double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
    private: private:
    // SolverManager functions // SolverManager functions
    ...@@ -158,6 +173,10 @@ class SolverManager ...@@ -158,6 +173,10 @@ class SolverManager
    virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0; virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0;
    virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0; virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
    virtual bool checkDerived(std::string prefix="") 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) // Params (here becaure it needs of declaration of SolverManager::ReportVerbosity)
    ......
    src/solver/solver_manager.cpp
    + 75
    − 2
    View file @ b2e6175f
    ...@@ -353,7 +353,12 @@ SolverManager::ReportVerbosity SolverManager::getVerbosity() const ...@@ -353,7 +353,12 @@ SolverManager::ReportVerbosity SolverManager::getVerbosity() const
    bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr) const bool SolverManager::isStateBlockRegistered(const StateBlockPtr& state_ptr) const
    { {
    return floating_state_blocks_.count(state_ptr) == 1 or (state_blocks_.count(state_ptr) == 1 and isStateBlockRegisteredDerived(state_ptr)); return isStateBlockFloating(state_ptr) or (state_blocks_.count(state_ptr) == 1 and isStateBlockRegisteredDerived(state_ptr));
    }
    bool SolverManager::isStateBlockFloating(const StateBlockPtr& state_ptr) const
    {
    return floating_state_blocks_.count(state_ptr) == 1;
    } }
    bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
    ...@@ -361,6 +366,45 @@ bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const ...@@ -361,6 +366,45 @@ bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
    return factors_.count(fac_ptr) and isFactorRegisteredDerived(fac_ptr); return factors_.count(fac_ptr) and isFactorRegisteredDerived(fac_ptr);
    } }
    bool SolverManager::isStateBlockFixed(const StateBlockPtr& st)
    {
    if (isStateBlockFloating(st))
    return st->isFixed();
    else
    return isStateBlockFixedDerived(st);
    }
    bool SolverManager::hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
    {
    if (isStateBlockFloating(st))
    return st->getLocalParametrization() == local_param;
    else
    return hasThisLocalParametrizationDerived(st, local_param);
    };
    bool SolverManager::hasLocalParametrization(const StateBlockPtr& st) const
    {
    if (isStateBlockFloating(st))
    return st->hasLocalParametrization();
    else
    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 double SolverManager::getPeriod() const
    { {
    return params_->period; return params_->period;
    ...@@ -431,9 +475,38 @@ bool SolverManager::check(std::string prefix) const ...@@ -431,9 +475,38 @@ bool SolverManager::check(std::string prefix) const
    } }
    } }
    // checkDerived // CHECK DERIVED ----------------------
    ok &= checkDerived(prefix); 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::checkAgainstDerived: 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::checkAgainstDerived: 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::checkAgainstDerived: numStateBlocksDerived() = ", numStateBlocksDerived(), " DIFFERENT THAN state_blocks_.size() = ", state_blocks_.size(), " - in ", prefix);
    ok = false;
    }
    if (numFactorsDerived() != numFactors())
    {
    WOLF_ERROR("SolverManager::checkAgainstDerived: numFactorsDerived() = ", numFactorsDerived(), " DIFFERENT THAN numFactors() = ", numFactors(), " - in ", prefix);
    ok = false;
    }
    return ok; return ok;
    } }
    ......
    test/dummy/solver_manager_dummy.h
    + 20
    − 19
    View file @ b2e6175f
    ...@@ -17,7 +17,7 @@ WOLF_PTR_TYPEDEFS(SolverManagerDummy); ...@@ -17,7 +17,7 @@ WOLF_PTR_TYPEDEFS(SolverManagerDummy);
    class SolverManagerDummy : public SolverManager class SolverManagerDummy : public SolverManager
    { {
    public: public:
    std::set<FactorBasePtr> factors_; std::set<FactorBasePtr> factors_derived_;
    std::map<StateBlockPtr,bool> state_block_fixed_; std::map<StateBlockPtr,bool> state_block_fixed_;
    std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_; std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
    ...@@ -26,28 +26,29 @@ class SolverManagerDummy : public SolverManager ...@@ -26,28 +26,29 @@ class SolverManagerDummy : public SolverManager
    { {
    }; };
    bool isStateBlockFixed(const StateBlockPtr& st) const bool isStateBlockFixedDerived(const StateBlockPtr& st) override
    { {
    if (floating_state_blocks_.count(st)) return state_block_fixed_.at(st);
    return st->isFixed();
    else
    return state_block_fixed_.at(st);
    }; };
    bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) override
    { {
    if (floating_state_blocks_.count(st)) return state_block_local_param_.count(st) == 1 and state_block_local_param_.at(st) == local_param;
    return st->getLocalParametrization() == local_param;
    else
    return state_block_local_param_.count(st) == 1 and state_block_local_param_.at(st) == local_param;
    }; };
    bool hasLocalParametrization(const StateBlockPtr& st) const bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override
    { {
    if (floating_state_blocks_.count(st)) return state_block_local_param_.count(st) == 1;
    return st->hasLocalParametrization(); };
    else
    return state_block_local_param_.count(st) == 1; virtual int numStateBlocksDerived() const override
    {
    return state_block_fixed_.size();
    }
    virtual int numFactorsDerived() const override
    {
    return factors_derived_.size();
    }; };
    void computeCovariances(const CovarianceBlocksToBeComputed blocks) override {}; void computeCovariances(const CovarianceBlocksToBeComputed blocks) override {};
    ...@@ -68,11 +69,11 @@ class SolverManagerDummy : public SolverManager ...@@ -68,11 +69,11 @@ class SolverManagerDummy : public SolverManager
    std::string solveDerived(const ReportVerbosity report_level) override { return std::string("");}; std::string solveDerived(const ReportVerbosity report_level) override { return std::string("");};
    void addFactorDerived(const FactorBasePtr& fac_ptr) override void addFactorDerived(const FactorBasePtr& fac_ptr) override
    { {
    factors_.insert(fac_ptr); factors_derived_.insert(fac_ptr);
    }; };
    void removeFactorDerived(const FactorBasePtr& fac_ptr) override void removeFactorDerived(const FactorBasePtr& fac_ptr) override
    { {
    factors_.erase(fac_ptr); factors_derived_.erase(fac_ptr);
    }; };
    void addStateBlockDerived(const StateBlockPtr& state_ptr) override void addStateBlockDerived(const StateBlockPtr& state_ptr) override
    { {
    ...@@ -102,7 +103,7 @@ class SolverManagerDummy : public SolverManager ...@@ -102,7 +103,7 @@ class SolverManagerDummy : public SolverManager
    bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override
    { {
    return factors_.count(fac_ptr) == 1; return factors_derived_.count(fac_ptr) == 1;
    }; };
    }; };
    ......
    test/gtest_solver_ceres.cpp
    + 0
    − 26
    View file @ b2e6175f
    ...@@ -36,14 +36,6 @@ class SolverCeresWrapper : public SolverCeres ...@@ -36,14 +36,6 @@ class SolverCeresWrapper : public SolverCeres
    { {
    }; };
    bool isStateBlockFixed(const StateBlockPtr& st)
    {
    if (floating_state_blocks_.count(st))
    return st->isFixed();
    else
    return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st));
    };
    int numStateBlocks() int numStateBlocks()
    { {
    return ceres_problem_->NumParameterBlocks(); return ceres_problem_->NumParameterBlocks();
    ...@@ -54,24 +46,6 @@ class SolverCeresWrapper : public SolverCeres ...@@ -54,24 +46,6 @@ class SolverCeresWrapper : public SolverCeres
    return ceres_problem_->NumResidualBlocks(); return ceres_problem_->NumResidualBlocks();
    }; };
    bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
    {
    if (floating_state_blocks_.count(st))
    return st->getLocalParametrization() == local_param;
    else
    return state_blocks_local_param_.count(st) == 1 and
    state_blocks_local_param_.at(st)->getLocalParametrization() == local_param and
    ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
    };
    bool hasLocalParametrization(const StateBlockPtr& st) const
    {
    if (floating_state_blocks_.count(st))
    return st->hasLocalParametrization();
    else
    return state_blocks_local_param_.count(st) == 1;
    };
    }; };
    TEST(SolverCeres, Create) TEST(SolverCeres, Create)
    ......
    test/gtest_solver_manager.cpp
    + 720
    − 193
    View file @ b2e6175f
    ...@@ -13,6 +13,7 @@ ...@@ -13,6 +13,7 @@
    #include "core/state_block/state_block.h" #include "core/state_block/state_block.h"
    #include "core/capture/capture_void.h" #include "core/capture/capture_void.h"
    #include "core/factor/factor_pose_2d.h" #include "core/factor/factor_pose_2d.h"
    #include "core/factor/factor_block_absolute.h"
    #include "core/state_block/local_parametrization_base.h" #include "core/state_block/local_parametrization_base.h"
    #include "core/state_block/local_parametrization_angle.h" #include "core/state_block/local_parametrization_angle.h"
    #include "dummy/solver_manager_dummy.h" #include "dummy/solver_manager_dummy.h"
    ...@@ -23,25 +24,43 @@ ...@@ -23,25 +24,43 @@
    using namespace wolf; using namespace wolf;
    using namespace Eigen; using namespace Eigen;
    StateBlockPtr createStateBlock()
    {
    Vector2d state; state << 1, 2;
    return std::make_shared<StateBlock>(state);
    }
    FactorBasePtr createFactor(StateBlockPtr sb_ptr)
    {
    return std::make_shared<FactorBlockAbsolute>(std::make_shared<FeatureBase>("Dummy",
    VectorXd::Zero(sb_ptr->getSize()),
    MatrixXd::Identity(sb_ptr->getSize(),sb_ptr->getSize())),
    sb_ptr,
    nullptr,
    false);
    }
    TEST(SolverManager, Create) TEST(SolverManager, Create)
    { {
    ProblemPtr P = Problem::create("PO", 2); ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // check double pointers to branches // check double pointers to branches
    ASSERT_EQ(P, solver_manager_ptr->getProblem()); EXPECT_EQ(P, solver_manager_ptr->getProblem());
    EXPECT_TRUE(solver_manager_ptr->check());
    } }
    TEST(SolverManager, AddStateBlock) ////////////////////////////////////////////////////////
    // FLOATING STATE BLOCKS
    TEST(SolverManager, FloatingStateBlock_Add)
    { {
    ProblemPtr P = Problem::create("PO", 2); ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block // Create State block
    Vector2d state; state << 1, 2; auto sb_ptr = createStateBlock();
    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
    // add stateblock // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD); P->notifyStateBlock(sb_ptr,ADD);
    // update solver // update solver
    ...@@ -49,24 +68,25 @@ TEST(SolverManager, AddStateBlock) ...@@ -49,24 +68,25 @@ TEST(SolverManager, AddStateBlock)
    // check stateblock // check stateblock
    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    } }
    TEST(SolverManager, DoubleAddStateBlock) TEST(SolverManager, FloatingStateBlock_DoubleAdd)
    { {
    ProblemPtr P = Problem::create("PO", 2); ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block // Create State block
    Vector2d state; state << 1, 2; auto sb_ptr = createStateBlock();
    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
    // add stateblock // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD); P->notifyStateBlock(sb_ptr,ADD);
    // update solver // update solver
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // add stateblock again // notify stateblock again
    P->notifyStateBlock(sb_ptr,ADD); P->notifyStateBlock(sb_ptr,ADD);
    // update solver // update solver
    ...@@ -74,110 +94,115 @@ TEST(SolverManager, DoubleAddStateBlock) ...@@ -74,110 +94,115 @@ TEST(SolverManager, DoubleAddStateBlock)
    // check stateblock // check stateblock
    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    } }
    TEST(SolverManager, UpdateStateBlock) TEST(SolverManager, FloatingStateBlock_AddUpdate)
    { {
    ProblemPtr P = Problem::create("PO", 2); ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block // Create State block
    Vector2d state; state << 1, 2; auto sb_ptr = createStateBlock();
    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
    // add stateblock // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD); P->notifyStateBlock(sb_ptr,ADD);
    // check flags // check flags
    ASSERT_FALSE(sb_ptr->stateUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated());
    ASSERT_FALSE(sb_ptr->fixUpdated()); EXPECT_FALSE(sb_ptr->fixUpdated());
    ASSERT_FALSE(sb_ptr->localParamUpdated()); EXPECT_FALSE(sb_ptr->localParamUpdated());
    // update solver // update solver
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // check flags // check flags
    ASSERT_FALSE(sb_ptr->stateUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated());
    ASSERT_FALSE(sb_ptr->fixUpdated()); EXPECT_FALSE(sb_ptr->fixUpdated());
    ASSERT_FALSE(sb_ptr->localParamUpdated()); EXPECT_FALSE(sb_ptr->localParamUpdated());
    // check stateblock unfixed // check stateblock unfixed
    ASSERT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr)); EXPECT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    // Fix frame // Fix frame
    sb_ptr->fix(); sb_ptr->fix();
    // check flags // check flags
    ASSERT_FALSE(sb_ptr->stateUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated());
    ASSERT_TRUE(sb_ptr->fixUpdated()); EXPECT_TRUE(sb_ptr->fixUpdated());
    ASSERT_FALSE(sb_ptr->localParamUpdated()); EXPECT_FALSE(sb_ptr->localParamUpdated());
    // update solver manager // update solver manager
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // check flags // check flags
    ASSERT_FALSE(sb_ptr->stateUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated());
    ASSERT_FALSE(sb_ptr->fixUpdated()); EXPECT_FALSE(sb_ptr->fixUpdated());
    ASSERT_FALSE(sb_ptr->localParamUpdated()); EXPECT_FALSE(sb_ptr->localParamUpdated());
    // check stateblock fixed // check stateblock fixed
    ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr)); EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    } }
    TEST(SolverManager, AddUpdateStateBlock) TEST(SolverManager, FloatingStateBlock_AddFixed)
    { {
    ProblemPtr P = Problem::create("PO", 2); ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block // Create State block
    Vector2d state; state << 1, 2; auto sb_ptr = createStateBlock();
    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
    // add stateblock // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD); P->notifyStateBlock(sb_ptr,ADD);
    // check flags // check flags
    ASSERT_FALSE(sb_ptr->stateUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated());
    ASSERT_FALSE(sb_ptr->fixUpdated()); EXPECT_FALSE(sb_ptr->fixUpdated());
    ASSERT_FALSE(sb_ptr->localParamUpdated()); EXPECT_FALSE(sb_ptr->localParamUpdated());
    // Fix state block // Fix state block
    sb_ptr->fix(); sb_ptr->fix();
    // check flags // check flags
    ASSERT_FALSE(sb_ptr->stateUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated());
    ASSERT_TRUE(sb_ptr->fixUpdated()); EXPECT_TRUE(sb_ptr->fixUpdated());
    ASSERT_FALSE(sb_ptr->localParamUpdated()); EXPECT_FALSE(sb_ptr->localParamUpdated());
    // update solver manager // update solver manager
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // check flags // check flags
    ASSERT_FALSE(sb_ptr->stateUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated());
    ASSERT_FALSE(sb_ptr->fixUpdated()); EXPECT_FALSE(sb_ptr->fixUpdated());
    ASSERT_FALSE(sb_ptr->localParamUpdated()); EXPECT_FALSE(sb_ptr->localParamUpdated());
    // check stateblock fixed // check stateblock fixed
    ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr)); EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    } }
    TEST(SolverManager, AddUpdateLocalParamStateBlock) TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
    { {
    ProblemPtr P = Problem::create("PO", 2); ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block // Create State block
    Vector2d state; state << 1, 2; auto sb_ptr = createStateBlock();
    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
    // add stateblock // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD); P->notifyStateBlock(sb_ptr,ADD);
    // check flags // check flags
    ASSERT_FALSE(sb_ptr->stateUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated());
    ASSERT_FALSE(sb_ptr->fixUpdated()); EXPECT_FALSE(sb_ptr->fixUpdated());
    ASSERT_FALSE(sb_ptr->localParamUpdated()); EXPECT_FALSE(sb_ptr->localParamUpdated());
    // Local param // Local param
    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>(); LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
    ...@@ -187,81 +212,84 @@ TEST(SolverManager, AddUpdateLocalParamStateBlock) ...@@ -187,81 +212,84 @@ TEST(SolverManager, AddUpdateLocalParamStateBlock)
    sb_ptr->fix(); sb_ptr->fix();
    // check flags // check flags
    ASSERT_FALSE(sb_ptr->stateUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated());
    ASSERT_TRUE(sb_ptr->fixUpdated()); EXPECT_TRUE(sb_ptr->fixUpdated());
    ASSERT_TRUE(sb_ptr->localParamUpdated()); EXPECT_TRUE(sb_ptr->localParamUpdated());
    // update solver manager // update solver manager
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // check flags // check flags
    ASSERT_FALSE(sb_ptr->stateUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated());
    ASSERT_FALSE(sb_ptr->fixUpdated()); EXPECT_FALSE(sb_ptr->fixUpdated());
    ASSERT_FALSE(sb_ptr->localParamUpdated()); EXPECT_FALSE(sb_ptr->localParamUpdated());
    // check stateblock fixed // check stateblock fixed
    ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
    ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr)); EXPECT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
    ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    } }
    TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock) TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
    { {
    ProblemPtr P = Problem::create("PO", 2); ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block // Create State block
    Vector2d state; state << 1, 2; auto sb_ptr = createStateBlock();
    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
    // Local param // Local param
    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>(); LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
    sb_ptr->setLocalParametrization(local_ptr); sb_ptr->setLocalParametrization(local_ptr);
    // check flags // check flags
    ASSERT_FALSE(sb_ptr->stateUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated());
    ASSERT_FALSE(sb_ptr->fixUpdated()); EXPECT_FALSE(sb_ptr->fixUpdated());
    ASSERT_TRUE(sb_ptr->localParamUpdated()); EXPECT_TRUE(sb_ptr->localParamUpdated());
    // add stateblock // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD); P->notifyStateBlock(sb_ptr,ADD);
    // update solver manager // update solver manager
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // check stateblock localparam // check stateblock localparam
    ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr)); EXPECT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
    // check flags // check flags
    ASSERT_FALSE(sb_ptr->stateUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated());
    ASSERT_FALSE(sb_ptr->fixUpdated()); EXPECT_FALSE(sb_ptr->fixUpdated());
    ASSERT_FALSE(sb_ptr->localParamUpdated()); EXPECT_FALSE(sb_ptr->localParamUpdated());
    // Remove local param // Remove local param
    sb_ptr->removeLocalParametrization(); sb_ptr->removeLocalParametrization();
    // check flags // check flags
    ASSERT_FALSE(sb_ptr->stateUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated());
    ASSERT_FALSE(sb_ptr->fixUpdated()); EXPECT_FALSE(sb_ptr->fixUpdated());
    ASSERT_TRUE(sb_ptr->localParamUpdated()); EXPECT_TRUE(sb_ptr->localParamUpdated());
    // update solver manager // update solver manager
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // check stateblock localparam // check stateblock localparam
    ASSERT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr)); EXPECT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    } }
    TEST(SolverManager, RemoveStateBlock) TEST(SolverManager, FloatingStateBlock_Remove)
    { {
    ProblemPtr P = Problem::create("PO", 2); ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block // Create State block
    Vector2d state; state << 1, 2; auto sb_ptr = createStateBlock();
    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
    // add stateblock // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD); P->notifyStateBlock(sb_ptr,ADD);
    // update solver // update solver
    ...@@ -274,19 +302,19 @@ TEST(SolverManager, RemoveStateBlock) ...@@ -274,19 +302,19 @@ TEST(SolverManager, RemoveStateBlock)
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // check stateblock // check stateblock
    ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    } }
    TEST(SolverManager, AddRemoveStateBlock) TEST(SolverManager, FloatingStateBlock_AddRemove)
    { {
    ProblemPtr P = Problem::create("PO", 2); ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block // Create State block
    Vector2d state; state << 1, 2; auto sb_ptr = createStateBlock();
    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
    // add stateblock // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD); P->notifyStateBlock(sb_ptr,ADD);
    // remove state_block // remove state_block
    ...@@ -296,38 +324,19 @@ TEST(SolverManager, AddRemoveStateBlock) ...@@ -296,38 +324,19 @@ TEST(SolverManager, AddRemoveStateBlock)
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // check state block // check state block
    ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    } EXPECT_TRUE(solver_manager_ptr->check());
    TEST(SolverManager, RemoveUpdateStateBlock)
    {
    ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block
    Vector2d state; state << 1, 2;
    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
    // add state_block
    P->notifyStateBlock(sb_ptr,ADD);
    // remove state_block
    P->notifyStateBlock(sb_ptr,REMOVE);
    // update solver
    solver_manager_ptr->update();
    } }
    TEST(SolverManager, DoubleRemoveStateBlock) TEST(SolverManager, FloatingStateBlock_DoubleRemove)
    { {
    ProblemPtr P = Problem::create("PO", 2); ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block // Create State block
    Vector2d state; state << 1, 2; auto sb_ptr = createStateBlock();
    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
    // add stateblock // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD); P->notifyStateBlock(sb_ptr,ADD);
    // remove state_block // remove state_block
    ...@@ -341,216 +350,734 @@ TEST(SolverManager, DoubleRemoveStateBlock) ...@@ -341,216 +350,734 @@ TEST(SolverManager, DoubleRemoveStateBlock)
    // update solver manager // update solver manager
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // checks
    EXPECT_TRUE(solver_manager_ptr->check());
    } }
    TEST(SolverManager, AddUpdatedStateBlock) TEST(SolverManager, FloatingStateBlock_AddUpdated)
    { {
    ProblemPtr P = Problem::create("PO", 2); ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block // Create State block
    Vector2d state; state << 1, 2; auto sb_ptr = createStateBlock();
    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
    // Fix // Fix
    sb_ptr->fix(); sb_ptr->fix();
    // Set State // Set State
    Vector2d state_2 = 2*state; sb_ptr->setState(2*sb_ptr->getState());
    sb_ptr->setState(state_2);
    // Check flags have been set true // Check flags have been set true
    ASSERT_TRUE(sb_ptr->fixUpdated()); EXPECT_TRUE(sb_ptr->fixUpdated());
    ASSERT_TRUE(sb_ptr->stateUpdated()); EXPECT_TRUE(sb_ptr->stateUpdated());
    // == When an ADD is notified: a ADD notification should be stored == // == When an ADD is notified: a ADD notification should be stored ==
    // add state_block // notify state block
    P->notifyStateBlock(sb_ptr,ADD); P->notifyStateBlock(sb_ptr,ADD);
    Notification notif; Notification notif;
    ASSERT_EQ(P->getStateBlockNotificationMapSize(),1); EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
    ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
    ASSERT_EQ(notif, ADD); EXPECT_EQ(notif, ADD);
    // == Insert OTHER notifications == // == Insert OTHER notifications ==
    // Set State --> FLAG // Set State --> FLAG
    state_2 = 2*state; sb_ptr->setState(2*sb_ptr->getState());
    sb_ptr->setState(state_2);
    // Fix --> FLAG // Fix --> FLAG
    sb_ptr->unfix(); sb_ptr->unfix();
    // Check flag has been set true // Check flag has been set true
    ASSERT_TRUE(sb_ptr->fixUpdated()); EXPECT_TRUE(sb_ptr->fixUpdated());
    ASSERT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb) EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
    // == consume empties the notification map == // == consume empties the notification map ==
    solver_manager_ptr->update(); // it calls P->consumeStateBlockNotificationMap(); solver_manager_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
    ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
    // Check flags have been reset // Check flags have been reset
    ASSERT_FALSE(sb_ptr->fixUpdated()); EXPECT_FALSE(sb_ptr->fixUpdated());
    ASSERT_FALSE(sb_ptr->stateUpdated()); EXPECT_FALSE(sb_ptr->stateUpdated());
    // == When an REMOVE is notified: a REMOVE notification should be stored == // == When an REMOVE is notified: a REMOVE notification should be stored ==
    // remove state_block // remove state_block
    P->notifyStateBlock(sb_ptr,REMOVE); P->notifyStateBlock(sb_ptr,REMOVE);
    ASSERT_EQ(P->getStateBlockNotificationMapSize(),1); EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
    ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif)); EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
    ASSERT_EQ(notif, REMOVE); EXPECT_EQ(notif, REMOVE);
    // == REMOVE + ADD: notification map should be empty == // == REMOVE + ADD: notification map should be empty ==
    P->notifyStateBlock(sb_ptr,ADD); P->notifyStateBlock(sb_ptr,ADD);
    ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
    // == UPDATES should clear the list of notifications == // == UPDATES should clear the list of notifications ==
    // add state_block // notify state block
    P->notifyStateBlock(sb_ptr,ADD); P->notifyStateBlock(sb_ptr,ADD);
    // update solver // update solver
    solver_manager_ptr->update(); solver_manager_ptr->update();
    ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
    // == ADD + REMOVE: notification map should be empty == // == ADD + REMOVE: notification map should be empty ==
    P->notifyStateBlock(sb_ptr,ADD); P->notifyStateBlock(sb_ptr,ADD);
    P->notifyStateBlock(sb_ptr,REMOVE); P->notifyStateBlock(sb_ptr,REMOVE);
    ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
    } }
    TEST(SolverManager, AddFactor) ////////////////////////////////////////////////////////
    // STATE BLOCKS AND FACTOR
    TEST(SolverManager, StateBlockAndFactor_Add)
    { {
    ProblemPtr P = Problem::create("PO", 2); ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block // Create State block
    Vector2d state; state << 1, 2; auto sb_ptr = createStateBlock();
    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
    // Create (and add) factor point 2d // Create Factor
    FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() ); auto fac_ptr = createFactor(sb_ptr);
    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
    auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
    // notification // notify stateblock (floating for the moment)
    Notification notif; P->notifyStateBlock(sb_ptr,ADD);
    ASSERT_TRUE(P->getFactorNotification(c,notif));
    ASSERT_EQ(notif, ADD); // update solver
    solver_manager_ptr->update();
    // checks
    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
    // notify factor (state block now not floating)
    P->notifyFactor(fac_ptr,ADD);
    // update solver // update solver
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // check factor // checks
    ASSERT_TRUE(solver_manager_ptr->isFactorRegistered(c)); EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_FALSE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isFactorRegistered(fac_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    } }
    TEST(SolverManager, RemoveFactor) TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
    { {
    ProblemPtr P = Problem::create("PO", 2); ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block // Create State block
    Vector2d state; state << 1, 2; auto sb_ptr = createStateBlock();
    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
    // Create Factor
    auto fac_ptr = createFactor(sb_ptr);
    // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD);
    // notify factor
    P->notifyFactor(fac_ptr,ADD);
    // Create (and add) factor point 2d // update solver
    FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() ); solver_manager_ptr->update();
    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); // notify stateblock again
    auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false); P->notifyStateBlock(sb_ptr,ADD);
    // notify factor
    P->notifyFactor(fac_ptr,ADD);
    // update solver // update solver
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // add factor // check stateblock
    P->notifyFactor(c,REMOVE); EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_FALSE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isFactorRegistered(fac_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    }
    TEST(SolverManager, StateBlockAndFactor_Fix)
    {
    ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block
    auto sb_ptr = createStateBlock();
    // Create Factor
    auto fac_ptr = createFactor(sb_ptr);
    // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD);
    // notification // notify factor
    P->notifyFactor(fac_ptr,ADD);
    // check flags
    EXPECT_FALSE(sb_ptr->stateUpdated());
    EXPECT_FALSE(sb_ptr->fixUpdated());
    EXPECT_FALSE(sb_ptr->localParamUpdated());
    // update solver
    solver_manager_ptr->update();
    // check flags
    EXPECT_FALSE(sb_ptr->stateUpdated());
    EXPECT_FALSE(sb_ptr->fixUpdated());
    EXPECT_FALSE(sb_ptr->localParamUpdated());
    // check stateblock unfixed
    EXPECT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    // Fix frame
    sb_ptr->fix();
    // check flags
    EXPECT_FALSE(sb_ptr->stateUpdated());
    EXPECT_TRUE(sb_ptr->fixUpdated());
    EXPECT_FALSE(sb_ptr->localParamUpdated());
    // update solver manager
    solver_manager_ptr->update();
    // check flags
    EXPECT_FALSE(sb_ptr->stateUpdated());
    EXPECT_FALSE(sb_ptr->fixUpdated());
    EXPECT_FALSE(sb_ptr->localParamUpdated());
    // check stateblock fixed
    EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    }
    TEST(SolverManager, StateBlockAndFactor_AddFixed)
    {
    ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block
    auto sb_ptr = createStateBlock();
    // Create Factor
    auto fac_ptr = createFactor(sb_ptr);
    // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD);
    // notify factor
    P->notifyFactor(fac_ptr,ADD);
    // check flags
    EXPECT_FALSE(sb_ptr->stateUpdated());
    EXPECT_FALSE(sb_ptr->fixUpdated());
    EXPECT_FALSE(sb_ptr->localParamUpdated());
    // Fix state block
    sb_ptr->fix();
    // check flags
    EXPECT_FALSE(sb_ptr->stateUpdated());
    EXPECT_TRUE(sb_ptr->fixUpdated());
    EXPECT_FALSE(sb_ptr->localParamUpdated());
    // update solver manager
    solver_manager_ptr->update();
    // check flags
    EXPECT_FALSE(sb_ptr->stateUpdated());
    EXPECT_FALSE(sb_ptr->fixUpdated());
    EXPECT_FALSE(sb_ptr->localParamUpdated());
    // check stateblock fixed
    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    }
    TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
    {
    ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block
    auto sb_ptr = createStateBlock();
    // Create Factor
    auto fac_ptr = createFactor(sb_ptr);
    // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD);
    // notify factor
    P->notifyFactor(fac_ptr,ADD);
    // check flags
    EXPECT_FALSE(sb_ptr->stateUpdated());
    EXPECT_FALSE(sb_ptr->fixUpdated());
    EXPECT_FALSE(sb_ptr->localParamUpdated());
    // Local param
    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
    sb_ptr->setLocalParametrization(local_ptr);
    // Fix state block
    sb_ptr->fix();
    // check flags
    EXPECT_FALSE(sb_ptr->stateUpdated());
    EXPECT_TRUE(sb_ptr->fixUpdated());
    EXPECT_TRUE(sb_ptr->localParamUpdated());
    // update solver manager
    solver_manager_ptr->update();
    // check flags
    EXPECT_FALSE(sb_ptr->stateUpdated());
    EXPECT_FALSE(sb_ptr->fixUpdated());
    EXPECT_FALSE(sb_ptr->localParamUpdated());
    // check stateblock fixed
    EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    }
    TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
    {
    ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block
    auto sb_ptr = createStateBlock();
    // Create Factor
    auto fac_ptr = createFactor(sb_ptr);
    // Local param
    LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
    sb_ptr->setLocalParametrization(local_ptr);
    // check flags
    EXPECT_FALSE(sb_ptr->stateUpdated());
    EXPECT_FALSE(sb_ptr->fixUpdated());
    EXPECT_TRUE(sb_ptr->localParamUpdated());
    // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD);
    // notify factor
    P->notifyFactor(fac_ptr,ADD);
    // update solver manager
    solver_manager_ptr->update();
    // check stateblock localparam
    EXPECT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
    // check flags
    EXPECT_FALSE(sb_ptr->stateUpdated());
    EXPECT_FALSE(sb_ptr->fixUpdated());
    EXPECT_FALSE(sb_ptr->localParamUpdated());
    // Remove local param
    sb_ptr->removeLocalParametrization();
    // check flags
    EXPECT_FALSE(sb_ptr->stateUpdated());
    EXPECT_FALSE(sb_ptr->fixUpdated());
    EXPECT_TRUE(sb_ptr->localParamUpdated());
    // update solver manager
    solver_manager_ptr->update();
    // check stateblock localparam
    EXPECT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    }
    TEST(SolverManager, StateBlockAndFactor_RemoveStateBlock)
    {
    ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block
    auto sb_ptr = createStateBlock();
    // Create Factor
    auto fac_ptr = createFactor(sb_ptr);
    // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD);
    // notify factor
    P->notifyFactor(fac_ptr,ADD);
    // update solver
    solver_manager_ptr->update();
    // remove state_block
    P->notifyStateBlock(sb_ptr,REMOVE); // there is afactor involved, removal posponed (REMOVE in notification list)
    // update solver
    solver_manager_ptr->update();
    // checks
    Notification notif; Notification notif;
    ASSERT_TRUE(P->getFactorNotification(c,notif)); EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    ASSERT_EQ(notif, REMOVE); EXPECT_TRUE(solver_manager_ptr->isFactorRegistered(fac_ptr));
    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
    EXPECT_EQ(notif, REMOVE);
    EXPECT_TRUE(solver_manager_ptr->check());
    }
    TEST(SolverManager, StateBlockAndFactor_RemoveFactor)
    {
    ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block
    auto sb_ptr = createStateBlock();
    // Create Factor
    auto fac_ptr = createFactor(sb_ptr);
    // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD);
    // notify factor
    P->notifyFactor(fac_ptr,ADD);
    // update solver
    solver_manager_ptr->update();
    // remove state_block
    P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
    // update solver // update solver
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // check factor // checks
    ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c)); EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
    EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    } }
    TEST(SolverManager, AddRemoveFactor) TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
    { {
    ProblemPtr P = Problem::create("PO", 2); ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block // Create State block
    Vector2d state; state << 1, 2; auto sb_ptr = createStateBlock();
    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
    // Create Factor
    auto fac_ptr = createFactor(sb_ptr);
    // Create (and add) factor point 2d // notify stateblock
    FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() ); P->notifyStateBlock(sb_ptr,ADD);
    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); // notify factor
    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); P->notifyFactor(fac_ptr,ADD);
    auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
    // notification // remove state_block
    P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification
    // update solver
    solver_manager_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added)
    // check state block
    Notification notif; Notification notif;
    ASSERT_TRUE(P->getFactorNotification(c,notif)); EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    ASSERT_EQ(notif, ADD); EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
    EXPECT_EQ(notif, ADD);
    EXPECT_TRUE(solver_manager_ptr->check());
    }
    TEST(SolverManager, StateBlockAndFactor_AddRemoveFactor)
    {
    ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block
    auto sb_ptr = createStateBlock();
    // Create Factor
    auto fac_ptr = createFactor(sb_ptr);
    // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD);
    // add factor // notify factor
    P->notifyFactor(c,REMOVE); P->notifyFactor(fac_ptr,ADD);
    // notification // remove state_block
    ASSERT_EQ(P->getFactorNotificationMapSize(), 0); // ADD+REMOVE cancels out P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
    ASSERT_FALSE(P->getFactorNotification(c, notif)); // ADD+REMOVE cancels out
    // update solver // update solver
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // check factor // checks
    ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c)); EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
    EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    } }
    TEST(SolverManager, DoubleRemoveFactor) TEST(SolverManager, StateBlockAndFactor_DoubleRemoveStateBlock)
    { {
    ProblemPtr P = Problem::create("PO", 2); ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P); SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block // Create State block
    Vector2d state; state << 1, 2; auto sb_ptr = createStateBlock();
    StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
    // Create Factor
    auto fac_ptr = createFactor(sb_ptr);
    // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD);
    // Create (and add) factor point 2d // notify factor
    FrameBasePtr F = P->emplaceKeyFrame(TimeStamp(0), P->stateZero() ); P->notifyFactor(fac_ptr,ADD);
    auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr); // remove state_block
    auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity()); P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD
    auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
    // update solver // update solver
    solver_manager_ptr->update(); // factor ADD is posponed
    // remove state_block
    P->notifyStateBlock(sb_ptr,REMOVE); // should be ignored
    // update solver manager
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // remove factor // check state block
    P->notifyFactor(c,REMOVE); Notification notif;
    EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
    EXPECT_EQ(notif, ADD);
    EXPECT_TRUE(solver_manager_ptr->check());
    }
    TEST(SolverManager, StateBlockAndFactor_DoubleRemoveFactor)
    {
    ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block
    auto sb_ptr = createStateBlock();
    // Create Factor
    auto fac_ptr = createFactor(sb_ptr);
    // notify stateblock
    P->notifyStateBlock(sb_ptr,ADD);
    // notify factor
    P->notifyFactor(fac_ptr,ADD);
    // remove state_block
    P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD
    // update solver
    solver_manager_ptr->update(); // state block should be automatically stored as floating
    // remove state_block
    P->notifyFactor(fac_ptr,REMOVE); // should be ignored
    // update solver // update solver
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // remove factor // checks
    P->notifyFactor(c,REMOVE); EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
    EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
    EXPECT_TRUE(solver_manager_ptr->check());
    }
    TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
    {
    ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block
    auto sb_ptr = createStateBlock();
    // Create Factor
    auto fac_ptr = createFactor(sb_ptr);
    // Fix
    sb_ptr->fix();
    // Change State
    sb_ptr->setState(2*sb_ptr->getState());
    // Check flags have been set true
    EXPECT_TRUE(sb_ptr->fixUpdated());
    EXPECT_TRUE(sb_ptr->stateUpdated());
    // == When an ADD is notified: a ADD notification should be stored ==
    // notify state block
    P->notifyStateBlock(sb_ptr,ADD);
    // notify factor
    P->notifyFactor(fac_ptr,ADD);
    Notification notif;
    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
    EXPECT_EQ(notif, ADD);
    // == Insert OTHER notifications ==
    // Set State --> FLAG
    sb_ptr->setState(2*sb_ptr->getState());
    // Fix --> FLAG
    sb_ptr->unfix();
    // Check flag has been set true
    EXPECT_TRUE(sb_ptr->fixUpdated());
    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
    // == consume empties the notification map ==
    solver_manager_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
    // Check flags have been reset
    EXPECT_FALSE(sb_ptr->fixUpdated());
    EXPECT_FALSE(sb_ptr->stateUpdated());
    // == When an REMOVE is notified: a REMOVE notification should be stored ==
    // remove state_block
    P->notifyStateBlock(sb_ptr,REMOVE);
    EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
    EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
    EXPECT_EQ(notif, REMOVE);
    // == REMOVE + ADD: notification map should be empty ==
    P->notifyStateBlock(sb_ptr,ADD);
    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
    // == UPDATES should clear the list of notifications ==
    // notify state block
    P->notifyStateBlock(sb_ptr,ADD);
    // update solver // update solver
    solver_manager_ptr->update(); solver_manager_ptr->update();
    // check factor EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
    ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
    // == ADD + REMOVE: notification map should be empty ==
    P->notifyStateBlock(sb_ptr,ADD);
    P->notifyStateBlock(sb_ptr,REMOVE);
    EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
    }
    ////////////////////////////////////////////////////////
    // ONLY FACTOR
    TEST(SolverManager, OnlyFactor_Add)
    {
    ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block
    auto sb_ptr = createStateBlock();
    // Create Factor
    auto fac_ptr = createFactor(sb_ptr);
    // notify factor (state block has not been notified)
    P->notifyFactor(fac_ptr,ADD);
    // update solver
    solver_manager_ptr->update(); // factor should be put in the notification list again
    // checks
    Notification notif;
    EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
    EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
    EXPECT_EQ(notif, ADD);
    EXPECT_TRUE(solver_manager_ptr->check());
    } }
    TEST(SolverManager, OnlyFactor_Remove)
    {
    ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block
    auto sb_ptr = createStateBlock();
    // Create Factor
    auto fac_ptr = createFactor(sb_ptr);
    // notify factor (state block has not been notified)
    P->notifyFactor(fac_ptr,ADD);
    // update solver
    solver_manager_ptr->update(); // factor should be put in the notification list again
    // notify factor
    P->notifyFactor(fac_ptr,REMOVE);
    // update solver
    solver_manager_ptr->update(); // factor ADD notification should be cancelled
    // checks
    Notification notif;
    EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
    EXPECT_FALSE(P->getFactorNotification(fac_ptr, notif));
    EXPECT_TRUE(solver_manager_ptr->check());
    }
    TEST(SolverManager, OnlyFactor_AddRemove)
    {
    ProblemPtr P = Problem::create("PO", 2);
    SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
    // Create State block
    auto sb_ptr = createStateBlock();
    // Create Factor
    auto fac_ptr = createFactor(sb_ptr);
    // notify factor (state block has not been notified)
    P->notifyFactor(fac_ptr,ADD);
    // notify factor
    P->notifyFactor(fac_ptr,REMOVE);
    // update solver
    solver_manager_ptr->update(); // factor ADD notification should be cancelled
    // checks
    Notification notif;
    EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
    EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
    EXPECT_FALSE(P->getFactorNotification(fac_ptr, notif));
    EXPECT_TRUE(solver_manager_ptr->check());
    }
    ////////////////////////////////////////////////////////
    // MULTITHREADING
    /*TEST(SolverManager, MultiThreadingTruncatedNotifications) /*TEST(SolverManager, MultiThreadingTruncatedNotifications)
    { {
    double Dt = 5.0; double Dt = 5.0;
    ......
    0% or .
    You are about to add 0 people to the discussion. Proceed with caution.
    Finish editing this message first!
    Please register or to comment