diff --git a/.gitignore b/.gitignore
index ea3b11ae3f1e21c872d7f069c6d6b56ca83c2a54..b690efc9b285d63727f435a9db3ee8b2039aee82 100644
--- a/.gitignore
+++ b/.gitignore
@@ -33,3 +33,4 @@ src/examples/map_apriltag_save.yaml
build_release/
.clangd
wolfcore.found
+/wolf.found
diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index e4306aa8ca126185a6343435405c1bcb5940e650..c899b7a054d6e1a9ebab5f45cca35fa0bd86c3d0 100644
--- a/include/core/ceres_wrapper/solver_ceres.h
+++ b/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
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 17341101c0d2ee4ee7074d220fda006e42fd0b31..bdd8a37aaa817b36609cc672402161f60a5fdba2 100644
--- a/include/core/solver/solver_manager.h
+++ b/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)
diff --git a/src/ceres_wrapper/solver_ceres.cpp b/src/ceres_wrapper/solver_ceres.cpp
index 033b0f9100b2befc728157cb0fb8d282d9808dd0..1d9cc7dd2e8f43c76820974caa473ad876b0a97c 100644
--- a/src/ceres_wrapper/solver_ceres.cpp
+++ b/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"
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 4a74ca8194f9b2b6111fb69d4c97bb7a4875b60e..60b06fd1f7b0744f8c8a293ff4f35e0e265c9f60 100644
--- a/src/solver/solver_manager.cpp
+++ b/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;
}
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
index 15db720a3c42f8151dd8745dc35d0232fc0fe885..5faef11f35874747a5f0b98f8d99b42911d116e1 100644
--- a/test/dummy/solver_manager_dummy.h
+++ b/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;
};
};
diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp
index a05fd74de52247abb7cc3047861d2ece21d5963c..9fe0e4306704919bcd7ab234716a3acf107e0227 100644
--- a/test/gtest_solver_ceres.cpp
+++ b/test/gtest_solver_ceres.cpp
@@ -1,5 +1,5 @@
/*
- * gtest_solver_ceres.cpp
+ * gtest_solver_ptr.cpp
*
* Created on: Jun, 2018
* Author: jvallve
@@ -13,6 +13,7 @@
#include "core/capture/capture_void.h"
#include "core/factor/factor_pose_2d.h"
#include "core/factor/factor_quaternion_absolute.h"
+#include "core/factor/factor_block_absolute.h"
#include "core/state_block/local_parametrization_angle.h"
#include "core/state_block/local_parametrization_quaternion.h"
@@ -27,604 +28,1533 @@
using namespace wolf;
using namespace Eigen;
-WOLF_PTR_TYPEDEFS(SolverCeresWrapper);
-class SolverCeresWrapper : public SolverCeres
-{
- public:
- SolverCeresWrapper(const ProblemPtr& wolf_problem) :
- SolverCeres(wolf_problem)
- {
- };
-
- bool isStateBlockRegisteredSolverCeres(const StateBlockPtr& st)
- {
- return ceres_problem_->HasParameterBlock(SolverManager::getAssociatedMemBlockPtr(st));
- };
-
- bool isStateBlockRegisteredSolverManager(const StateBlockPtr& st)
- {
- return state_blocks_.find(st)!=state_blocks_.end();
- };
-
- bool isStateBlockFixed(const StateBlockPtr& st)
- {
- return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st));
- };
-
- int numStateBlocks()
- {
- return ceres_problem_->NumParameterBlocks();
- };
-
- int numFactors()
- {
- return ceres_problem_->NumResidualBlocks();
- };
-
- bool isFactorRegistered(const FactorBasePtr& fac_ptr) const override
- {
- return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
- };
-
- bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
- {
- return state_blocks_local_param_.find(st) != state_blocks_local_param_.end() &&
- state_blocks_local_param_.at(st)->getLocalParametrization() == local_param &&
- ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
- };
+/*
+ * Following tests are the same as in gtest_solver_manager.cpp
+ * (modifications should be applied to both tests)
+ */
- bool hasLocalParametrization(const StateBlockPtr& st) const
- {
- return state_blocks_local_param_.find(st) != state_blocks_local_param_.end();
- };
+StateBlockPtr createStateBlock()
+{
+ Vector4d state; state << 1, 0, 0, 0;
+ 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(SolverCeres, Create)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
- // check double ointers to branches
- ASSERT_EQ(P, solver_ceres->getProblem());
+ // check pointers
+ EXPECT_EQ(P, solver_ptr->getProblem());
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddStateBlock)
+////////////////////////////////////////////////////////
+// FLOATING STATE BLOCKS
+TEST(SolverCeres, FloatingStateBlock_Add)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check stateblock
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, DoubleAddStateBlock)
+TEST(SolverCeres, FloatingStateBlock_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // add stateblock again
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock again
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check stateblock
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, UpdateStateBlock)
+TEST(SolverCeres, FloatingStateBlock_AddFix)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock unfixed
- ASSERT_FALSE(solver_ceres->isStateBlockFixed(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
// Fix frame
sb_ptr->fix();
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
// update solver manager
- solver_ceres->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_ceres->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddFixed)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // 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_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddUpdateStateBlock)
+TEST(SolverCeres, FloatingStateBlock_AddUpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // 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<LocalParametrizationQuaternionLocal>();
+ 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());
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver manager
- solver_ceres->update();
+ solver_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr));
- ASSERT_TRUE(solver_ceres->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, FloatingStateBlock_AddLocalParamRemoveLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ 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);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock localparam
+ EXPECT_TRUE(solver_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_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check stateblock localparam
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, RemoveStateBlock)
+TEST(SolverCeres, FloatingStateBlock_Remove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_TRUE(solver_ceres->isStateBlockRegisteredSolverCeres(sb_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check stateblock
- ASSERT_FALSE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_EQ(solver_ceres->numStateBlocks(), 0);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddRemoveStateBlock)
+TEST(SolverCeres, FloatingStateBlock_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check no stateblocks
- ASSERT_FALSE(solver_ceres->isStateBlockRegisteredSolverManager(sb_ptr));
- ASSERT_EQ(solver_ceres->numStateBlocks(), 0);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, RemoveUpdateStateBlock)
+TEST(SolverCeres, FloatingStateBlock_AddRemoveAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
- // update solver
- solver_ceres->update();
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD); // again ADD in notification list
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check state block
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, DoubleRemoveStateBlock)
+TEST(SolverCeres, FloatingStateBlock_DoubleRemove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // update solver
+ solver_ptr->update();
+
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver manager
- solver_ceres->update();
+ solver_ptr->update();
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddFactor)
+TEST(SolverCeres, FloatingStateBlock_AddUpdated)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Fix
+ sb_ptr->fix();
+
+ // Set 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);
+
+ // check notifications
+ // check notifications
+ 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_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 ==
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame( 0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ 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
- solver_ceres->update();
+ solver_ptr->update();
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c));
- ASSERT_EQ(solver_ceres->numFactors(), 1);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // == ADD + REMOVE: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr,REMOVE);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
}
-TEST(SolverCeres, DoubleAddFactor)
+////////////////////////////////////////////////////////
+// STATE BLOCKS AND FACTOR
+TEST(SolverCeres, StateBlockAndFactor_Add)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
+ // Create State block
+ auto sb_ptr = createStateBlock();
- // add factor again
- P->notifyFactor(c,ADD);
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify stateblock (floating for the moment)
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+
+ // notify factor (state block now not floating)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c));
- ASSERT_EQ(solver_ceres->numFactors(), 1);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, RemoveFactor)
+TEST(SolverCeres, StateBlockAndFactor_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(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);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
+ solver_ptr->update();
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock again
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check factor
- ASSERT_FALSE(solver_ceres->isFactorRegistered(c));
- ASSERT_EQ(solver_ceres->numFactors(), 0);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddRemoveFactor)
+TEST(SolverCeres, StateBlockAndFactor_Fix)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- ASSERT_TRUE(P->getFactorNotificationMapSize() == 0); // add+remove = empty
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver
- solver_ceres->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock unfixed
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // Fix frame
+ sb_ptr->fix();
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // check factor
- ASSERT_FALSE(solver_ceres->isFactorRegistered(c));
- ASSERT_EQ(solver_ceres->numFactors(), 0);
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, DoubleRemoveFactor)
+TEST(SolverCeres, StateBlockAndFactor_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f,f,nullptr,false);
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // 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_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_UpdateLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(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 notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // 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<LocalParametrizationQuaternionLocal>();
+ 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_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // Local param
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
+ 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);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // 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_ptr->update();
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_RemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(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 notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list)
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
- // remove factor
- P->notifyFactor(c,REMOVE);
+TEST(SolverCeres, StateBlockAndFactor_RemoveFactor)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(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 notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
// update solver
- solver_ceres->update();
+ solver_ptr->update();
- // check factor
- ASSERT_FALSE(solver_ceres->isFactorRegistered(c));
- ASSERT_EQ(solver_ceres->numFactors(), 0);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddStateBlockLocalParam)
+TEST(SolverCeres, StateBlockAndFactor_AddRemoveStateBlock)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector1d state; state << 1;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // Local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrization(local_param_ptr);
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added)
- // check stateblock
- ASSERT_TRUE(solver_ceres->hasLocalParametrization(sb_ptr));
- ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(sb_ptr,local_param_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, RemoveLocalParam)
+TEST(SolverCeres, StateBlockAndFactor_AddRemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector1d state; state << 1;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // Local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrization(local_param_ptr);
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD notification
+
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // state block should be automatically stored as floating
- // Remove local param
- sb_ptr->removeLocalParametrization();
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(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 notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // factor ADD is posponed
- // check stateblock
- ASSERT_FALSE(solver_ceres->hasLocalParametrization(sb_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver manager
+ solver_ptr->update(); // repeated REMOVE should be ignored
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, AddLocalParam)
+TEST(SolverCeres, StateBlockAndFactor_DoubleRemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
// Create State block
- Vector1d state; state << 1;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD
+
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // state block should be automatically stored as floating
- // check stateblock
- ASSERT_FALSE(solver_ceres->hasLocalParametrization(sb_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
- // Local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrization(local_param_ptr);
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // repeated REMOVE should be ignored
- // check stateblock
- ASSERT_TRUE(solver_ceres->hasLocalParametrization(sb_ptr));
- ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(sb_ptr,local_param_ptr));
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverCeres, StateBlockAndFactor_AddUpdatedStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // Fix
+ sb_ptr->fix();
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // 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);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // == 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_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
+ solver_ptr->update();
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+
+ // == ADD + REMOVE: notification map should be empty ==
+ P->notifyStateBlock(sb_ptr,ADD);
+ P->notifyStateBlock(sb_ptr,REMOVE);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
}
-TEST(SolverCeres, FactorsRemoveLocalParam)
+////////////////////////////////////////////////////////
+// ONLY FACTOR
+TEST(SolverCeres, OnlyFactor_Add)
{
ProblemPtr P = Problem::create("PO", 3);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
- // Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
- FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
+ // 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);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
- // check local param
- ASSERT_TRUE(solver_ceres->hasLocalParametrization(F->getO()));
- ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization()));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c1));
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c2));
- ASSERT_EQ(solver_ceres->numFactors(), 2);
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
- // remove local param
- F->getO()->removeLocalParametrization();
+TEST(SolverCeres, OnlyFactor_Remove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverCeres>(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);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // ADD factor should be posponed (in the notification list again)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // check local param
- ASSERT_FALSE(solver_ceres->hasLocalParametrization(F->getO()));
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c1));
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c2));
- ASSERT_EQ(solver_ceres->numFactors(), 2);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // update solver
+ solver_ptr->update(); // nothing to update
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverCeres, FactorsUpdateLocalParam)
+TEST(SolverCeres, OnlyFactor_AddRemove)
{
ProblemPtr P = Problem::create("PO", 3);
- SolverCeresWrapperPtr solver_ceres = std::make_shared<SolverCeresWrapper>(P);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
- // Create (and add) 2 factors quaternion
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
- FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, f, F->getO(),nullptr,false);
+ // Create State block
+ auto sb_ptr = createStateBlock();
- // update solver
- solver_ceres->update();
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
+
+ // notify factor (state block has not been notified)
+ P->notifyFactor(fac_ptr,ADD);
- // check local param
- ASSERT_TRUE(solver_ceres->hasLocalParametrization(F->getO()));
- ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization()));
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c1));
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c2));
- ASSERT_EQ(solver_ceres->numFactors(), 2);
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
- // remove local param
- LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationQuaternionGlobal>();
- F->getO()->setLocalParametrization(local_param_ptr);
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
// update solver
- solver_ceres->update();
+ solver_ptr->update(); // nothing to update
- // check local param
- ASSERT_TRUE(solver_ceres->hasLocalParametrization(F->getO()));
- ASSERT_TRUE(solver_ceres->hasThisLocalParametrization(F->getO(),local_param_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
- // check factor
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c1));
- ASSERT_TRUE(solver_ceres->isFactorRegistered(c2));
- ASSERT_EQ(solver_ceres->numFactors(), 2);
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+////////////////////////////////////////////////////////
+// MULTITHREADING
+
+TEST(SolverCeres, MultiThreadingTruncatedNotifications)
+{
+ double Dt = 5.0;
+ ProblemPtr P = Problem::create("PO", 2);
+ auto solver_ptr = std::make_shared<SolverCeres>(P);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // loop updating (without sleep)
+ std::thread t([&](){
+ auto start_t = std::chrono::high_resolution_clock::now();
+ while (true)
+ {
+ solver_ptr->update();
+ ASSERT_TRUE(solver_ptr->check());
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt)
+ break;
+ }});
+
+ // loop emplacing and removing frames (window of 10 KF)
+ auto start = std::chrono::high_resolution_clock::now();
+ TimeStamp ts(0);
+ while (true)
+ {
+ // Emplace Frame, Capture, feature and factor pose 2d
+ FrameBasePtr F = P->emplaceKeyFrame(ts, P->stateZero());
+ auto C = CaptureBase::emplace<CaptureVoid>(F, ts, nullptr);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+
+ ts += 1.0;
+
+ if (P->getTrajectory()->getFrameMap().size() > 10)
+ (*P->getTrajectory()->begin())->remove();
+
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
+ break;
+ }
+
+ t.join();
}
int main(int argc, char **argv)
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 30bf42ca390c4488c5227be99a2c59a5a1cb98c3..5226e125d79e059b29fbbfc3a432ce72452ef43c 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -13,8 +13,8 @@
#include "core/state_block/state_block.h"
#include "core/capture/capture_void.h"
#include "core/factor/factor_pose_2d.h"
-#include "core/state_block/local_parametrization_base.h"
-#include "core/state_block/local_parametrization_angle.h"
+#include "core/factor/factor_block_absolute.h"
+#include "core/state_block/local_parametrization_quaternion.h"
#include "dummy/solver_manager_dummy.h"
#include <iostream>
@@ -23,546 +23,1508 @@
using namespace wolf;
using namespace Eigen;
+/*
+ * Following tests are the same as in gtest_solver_ceres.cpp
+ * (modifications should be applied to both tests)
+ */
+
+StateBlockPtr createStateBlock()
+{
+ Vector4d state; state << 1, 0, 0, 0;
+ 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)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
- // check double pointers to branches
- ASSERT_EQ(P, solver_manager_ptr->getProblem());
+ // check pointers
+ EXPECT_EQ(P, solver_ptr->getProblem());
+
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddStateBlock)
+////////////////////////////////////////////////////////
+// FLOATING STATE BLOCKS
+TEST(SolverManager, FloatingStateBlock_Add)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, DoubleAddStateBlock)
+TEST(SolverManager, FloatingStateBlock_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // add stateblock again
+ // notify stateblock again
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, UpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddFix)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock unfixed
- ASSERT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
// Fix frame
sb_ptr->fix();
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddUpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// Fix state block
sb_ptr->fix();
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddUpdateLocalParamStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// Local param
- LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
sb_ptr->setLocalParametrization(local_ptr);
// Fix state block
sb_ptr->fix();
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
- ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
// Local param
- LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
sb_ptr->setLocalParametrization(local_ptr);
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock localparam
- ASSERT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// Remove local param
sb_ptr->removeLocalParametrization();
// check flags
- ASSERT_FALSE(sb_ptr->stateUpdated());
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->localParamUpdated());
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->localParamUpdated());
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock localparam
- ASSERT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, RemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_Remove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock
- ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddRemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddRemove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancel out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check state block
- ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, RemoveUpdateStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddRemoveAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add state_block
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE);
+ P->notifyStateBlock(sb_ptr,REMOVE); // should cancell out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD); // again ADD in notification list
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check state block
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, DoubleRemoveStateBlock)
+TEST(SolverManager, FloatingStateBlock_DoubleRemove)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // add stateblock
+ // notify stateblock
P->notifyStateBlock(sb_ptr,ADD);
+ // update solver
+ solver_ptr->update();
+
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
// remove state_block
P->notifyStateBlock(sb_ptr,REMOVE);
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddUpdatedStateBlock)
+TEST(SolverManager, FloatingStateBlock_AddUpdated)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
// Fix
sb_ptr->fix();
// Set State
- Vector2d state_2 = 2*state;
- sb_ptr->setState(state_2);
+ sb_ptr->setState(2*sb_ptr->getState());
// Check flags have been set true
- ASSERT_TRUE(sb_ptr->fixUpdated());
- ASSERT_TRUE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_TRUE(sb_ptr->stateUpdated());
// == When an ADD is notified: a ADD notification should be stored ==
- // add state_block
+ // notify state block
P->notifyStateBlock(sb_ptr,ADD);
+ // check notifications
+ // check notifications
Notification notif;
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
- ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
- ASSERT_EQ(notif, ADD);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(notif, ADD);
// == Insert OTHER notifications ==
// Set State --> FLAG
- state_2 = 2*state;
- sb_ptr->setState(state_2);
+ sb_ptr->setState(2*sb_ptr->getState());
// Fix --> FLAG
sb_ptr->unfix();
// 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 ==
- solver_manager_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+ solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
// Check flags have been reset
- ASSERT_FALSE(sb_ptr->fixUpdated());
- ASSERT_FALSE(sb_ptr->stateUpdated());
+ 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);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
- ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
- ASSERT_EQ(notif, REMOVE);
+ // check notifications
+ 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);
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
// == UPDATES should clear the list of notifications ==
- // add state_block
+ // notify state block
P->notifyStateBlock(sb_ptr,ADD);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
- ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// == ADD + REMOVE: notification map should be empty ==
P->notifyStateBlock(sb_ptr,ADD);
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);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- 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);
+ // notify stateblock (floating for the moment)
+ P->notifyStateBlock(sb_ptr,ADD);
- // notification
+ // check notifications
Notification notif;
- ASSERT_TRUE(P->getFactorNotification(c,notif));
- ASSERT_EQ(notif, ADD);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+
+ // notify factor (state block now not floating)
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // check factor
- ASSERT_TRUE(solver_manager_ptr->isFactorRegistered(c));
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, RemoveFactor)
+TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
- 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);
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // notify stateblock again
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check stateblock
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_Fix)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // Create State block
+ auto sb_ptr = createStateBlock();
- // add factor
- P->notifyFactor(c,REMOVE);
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // notification
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
+
+ // check notifications
Notification notif;
- ASSERT_TRUE(P->getFactorNotification(c,notif));
- ASSERT_EQ(notif, REMOVE);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock unfixed
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // Fix frame
+ sb_ptr->fix();
- // check factor
- ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_TRUE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, AddRemoveFactor)
+TEST(SolverManager, StateBlockAndFactor_AddFixed)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_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 notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // 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_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_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 notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // 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<LocalParametrizationQuaternionLocal>();
+ 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_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check flags
+ EXPECT_FALSE(sb_ptr->stateUpdated());
+ EXPECT_FALSE(sb_ptr->fixUpdated());
+ EXPECT_FALSE(sb_ptr->localParamUpdated());
+
+ // check stateblock fixed
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_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<LocalParametrizationQuaternionLocal>();
+ 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);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+
+ // 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_ptr->update();
+
+ // check solver
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->hasLocalParametrization(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_RemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
+
+ // Create Factor
+ auto fac_ptr = createFactor(sb_ptr);
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(0, P->stateZero() );
+ // notify stateblock
+ P->notifyStateBlock(sb_ptr,ADD);
- 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);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- // notification
+ // check notifications
Notification notif;
- ASSERT_TRUE(P->getFactorNotification(c,notif));
- ASSERT_EQ(notif, ADD);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // add factor
- P->notifyFactor(c,REMOVE);
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
- // notification
- ASSERT_EQ(P->getFactorNotificationMapSize(), 0); // ADD+REMOVE cancels out
- ASSERT_FALSE(P->getFactorNotification(c, notif)); // ADD+REMOVE cancels out
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // there is a factor involved, removal posponed (REMOVE in notification list)
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
- // check factor
- ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, DoubleRemoveFactor)
+TEST(SolverManager, StateBlockAndFactor_RemoveFactor)
{
- ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_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 notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+
+ // update solver
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_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 notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out ADD notification
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddRemoveFactor)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_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 notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD notification
+
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_DoubleRemoveStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_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 notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE); // cancells out the ADD
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // factor ADD is posponed
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyStateBlock(sb_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver manager
+ solver_ptr->update(); // repeated REMOVE should be ignored
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // check state block
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_DoubleRemoveFactor)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_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 notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE); // cancells out ADD
+
+ // check notifications
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // state block should be automatically stored as floating
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
+
+ // remove state_block
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(REMOVE, notif);
+
+ // update solver
+ solver_ptr->update(); // repeated REMOVE should be ignored
+
+ // checks
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// Create State block
- Vector2d state; state << 1, 2;
- StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
+ auto sb_ptr = createStateBlock();
- // Create (and add) factor point 2d
- FrameBasePtr F = P->emplaceKeyFrame(TimeStamp(0), P->stateZero() );
+ // Create Factor
+ 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);
+ // 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);
+
+ // check notifications
+ Notification notif;
+ EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // == 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_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
+ solver_ptr->update();
+
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
+
+ // == 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", 3);
+ auto solver_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);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // update solver
+ solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, OnlyFactor_Remove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_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);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // ADD factor should be posponed (in the notification list again)
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // nothing to update
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, OnlyFactor_AddRemove)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_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);
+
+ // check notifications
+ Notification notif;
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
+
+ // notify factor
+ P->notifyFactor(fac_ptr,REMOVE);
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update(); // nothing to update
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
- // check factor
- ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
+ // checks
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
+////////////////////////////////////////////////////////
+// MULTITHREADING
+
TEST(SolverManager, MultiThreadingTruncatedNotifications)
{
double Dt = 5.0;
ProblemPtr P = Problem::create("PO", 2);
- SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(P);
// loop updating (without sleep)
std::thread t([&](){
auto start_t = std::chrono::high_resolution_clock::now();
while (true)
{
- solver_manager_ptr->update();
+ solver_ptr->update();
+ ASSERT_TRUE(solver_ptr->check());
if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt)
break;
}});