diff --git a/include/core/ceres_wrapper/solver_ceres.h b/include/core/ceres_wrapper/solver_ceres.h
index b81d9f8cfaf90396ffdf904ebf3c12d9bb6249d1..c899b7a054d6e1a9ebab5f45cca35fa0bd86c3d0 100644
--- a/include/core/ceres_wrapper/solver_ceres.h
+++ b/include/core/ceres_wrapper/solver_ceres.h
@@ -169,14 +169,13 @@ 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) 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)
@@ -184,14 +183,6 @@ inline bool SolverCeres::isStateBlockFixedDerived(const StateBlockPtr& st)
return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st));
};
-inline bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st,
- const LocalParametrizationBasePtr& local_param)
-{
- return state_blocks_local_param_.count(st) == 1 and
- state_blocks_local_param_.at(st)->getLocalParametrization() == local_param and
- ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
-};
-
inline bool SolverCeres::hasLocalParametrizationDerived(const StateBlockPtr& st) const
{
return state_blocks_local_param_.count(st) == 1;
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 9649f9e063fab5b8ead117332ca8f070836b8b86..60b06fd1f7b0744f8c8a293ff4f35e0e265c9f60 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -368,26 +368,35 @@ bool SolverManager::isFactorRegistered(const FactorBasePtr& fac_ptr) const
bool SolverManager::isStateBlockFixed(const StateBlockPtr& st)
{
+ if (!isStateBlockRegistered(st))
+ return false;
+
if (isStateBlockFloating(st))
return st->isFixed();
- else
- return isStateBlockFixedDerived(st);
+
+ 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;
- else
- return hasThisLocalParametrizationDerived(st, 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();
- else
- return hasLocalParametrizationDerived(st);
+
+ return hasLocalParametrizationDerived(st);
};
int SolverManager::numFactors() const
@@ -483,7 +492,7 @@ bool SolverManager::check(std::string prefix) const
for (auto sb : state_blocks_)
if (!isStateBlockRegisteredDerived(sb.first))
{
- WOLF_ERROR("SolverManager::checkAgainstDerived: state block ", sb.first, " is in state_blocks_ but not registered in derived solver - in ", prefix);
+ WOLF_ERROR("SolverManager::check: state block ", sb.first, " is in state_blocks_ but not registered in derived solver - in ", prefix);
ok = false;
}
@@ -491,19 +500,19 @@ bool SolverManager::check(std::string prefix) const
for (auto fac : factors_)
if (!isFactorRegisteredDerived(fac))
{
- WOLF_ERROR("SolverManager::checkAgainstDerived: factor ", fac->id(), " is in factors_ but not registered in derived solver - in ", prefix);
+ 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::checkAgainstDerived: numStateBlocksDerived() = ", numStateBlocksDerived(), " DIFFERENT THAN state_blocks_.size() = ", state_blocks_.size(), " - in ", prefix);
+ WOLF_ERROR("SolverManager::check: numStateBlocksDerived() = ", numStateBlocksDerived(), " DIFFERENT THAN state_blocks_.size() = ", state_blocks_.size(), " - in ", prefix);
ok = false;
}
if (numFactorsDerived() != numFactors())
{
- WOLF_ERROR("SolverManager::checkAgainstDerived: numFactorsDerived() = ", numFactorsDerived(), " DIFFERENT THAN numFactors() = ", numFactors(), " - in ", prefix);
+ WOLF_ERROR("SolverManager::check: numFactorsDerived() = ", numFactorsDerived(), " DIFFERENT THAN numFactors() = ", numFactors(), " - in ", prefix);
ok = false;
}
diff --git a/test/dummy/solver_manager_dummy.h b/test/dummy/solver_manager_dummy.h
index 89febb761317bc9bf81d10642e5ee6a981ab356b..5faef11f35874747a5f0b98f8d99b42911d116e1 100644
--- a/test/dummy/solver_manager_dummy.h
+++ b/test/dummy/solver_manager_dummy.h
@@ -33,12 +33,12 @@ class SolverManagerDummy : public SolverManager
bool hasThisLocalParametrizationDerived(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) override
{
- return state_block_local_param_.count(st) == 1 and state_block_local_param_.at(st) == local_param;
+ return state_block_local_param_.at(st) == local_param;
};
bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override
{
- return state_block_local_param_.count(st) == 1;
+ return state_block_local_param_.at(st) != nullptr;
};
virtual int numStateBlocksDerived() const override
@@ -61,8 +61,6 @@ 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;}
@@ -91,10 +89,7 @@ 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
{
diff --git a/test/gtest_solver_ceres.cpp b/test/gtest_solver_ceres.cpp
index 3dd5a30b546edc5755b130ea204c60c5592a4c23..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,568 +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)
- {
- };
-
- int numStateBlocks()
- {
- return ceres_problem_->NumParameterBlocks();
- };
+/*
+ * Following tests are the same as in gtest_solver_manager.cpp
+ * (modifications should be applied to both tests)
+ */
- int numFactors()
- {
- return ceres_problem_->NumResidualBlocks();
- };
+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->isStateBlockRegistered(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();
+
+ // 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_ceres->update();
+ solver_ptr->update();
- // check stateblock
- ASSERT_TRUE(solver_ceres->isStateBlockRegistered(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();
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // 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->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->isStateBlockRegistered(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);
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // update solver manager
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(), 0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
+
+ // 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->isStateBlockRegistered(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->isStateBlockRegistered(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->isStateBlockRegistered(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();
+
+ // 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->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 ==
- // 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);
+ // 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);
+
+ // 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 State block
+ auto sb_ptr = createStateBlock();
+
+ // 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));
- // 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 factor (state block now not floating)
+ P->notifyFactor(fac_ptr,ADD);
- // add factor again
- P->notifyFactor(c,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);
- // 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 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();
+ 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);
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // 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);
+
+ // notify stateblock
+ P->notifyStateBlock(sb_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);
+ // notify factor
+ P->notifyFactor(fac_ptr,ADD);
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // 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);
- ASSERT_TRUE(P->getFactorNotificationMapSize() == 0); // add+remove = empty
+ // 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);
+
+ // 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);
- // 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 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();
+
+ // 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_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());
+}
+
+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);
- // remove factor
- P->notifyFactor(c,REMOVE);
+ // 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->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);
+
+ // 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);
- // 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, 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();
- // add stateblock
+ // 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_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();
+
+ // Change State
+ sb_ptr->setState(2*sb_ptr->getState());
- // run ceres manager check
- ASSERT_TRUE(solver_ceres->check());
+ // 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 de2586dfa38d431719c8441863d099058a1d4e65..5226e125d79e059b29fbbfc3a432ce72452ef43c 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -14,8 +14,7 @@
#include "core/capture/capture_void.h"
#include "core/factor/factor_pose_2d.h"
#include "core/factor/factor_block_absolute.h"
-#include "core/state_block/local_parametrization_base.h"
-#include "core/state_block/local_parametrization_angle.h"
+#include "core/state_block/local_parametrization_quaternion.h"
#include "dummy/solver_manager_dummy.h"
#include <iostream>
@@ -24,9 +23,14 @@
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()
{
- Vector2d state; state << 1, 2;
+ Vector4d state; state << 1, 0, 0, 0;
return std::make_shared<StateBlock>(state);
}
@@ -42,20 +46,22 @@ FactorBasePtr createFactor(StateBlockPtr sb_ptr)
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 pointers
+ EXPECT_EQ(P, solver_ptr->getProblem());
- // check double pointers to branches
- EXPECT_EQ(P, solver_manager_ptr->getProblem());
- EXPECT_TRUE(solver_manager_ptr->check());
+ // run solver check
+ EXPECT_TRUE(solver_ptr->check());
}
////////////////////////////////////////////////////////
// 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
auto sb_ptr = createStateBlock();
@@ -63,19 +69,29 @@ TEST(SolverManager, FloatingStateBlock_Add)
// 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
- EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
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
auto sb_ptr = createStateBlock();
@@ -83,25 +99,44 @@ TEST(SolverManager, FloatingStateBlock_DoubleAdd)
// 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);
// 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
- EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ EXPECT_TRUE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
-TEST(SolverManager, FloatingStateBlock_AddUpdate)
+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
auto sb_ptr = createStateBlock();
@@ -109,13 +144,23 @@ TEST(SolverManager, FloatingStateBlock_AddUpdate)
// 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_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());
@@ -123,20 +168,28 @@ TEST(SolverManager, FloatingStateBlock_AddUpdate)
EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock unfixed
- EXPECT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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_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());
@@ -144,15 +197,15 @@ TEST(SolverManager, FloatingStateBlock_AddUpdate)
EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->isStateBlockFloating(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
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
auto sb_ptr = createStateBlock();
@@ -160,6 +213,12 @@ TEST(SolverManager, FloatingStateBlock_AddFixed)
// 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());
@@ -174,7 +233,11 @@ TEST(SolverManager, FloatingStateBlock_AddFixed)
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
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -182,16 +245,16 @@ TEST(SolverManager, FloatingStateBlock_AddFixed)
EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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, 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
auto sb_ptr = createStateBlock();
@@ -199,13 +262,19 @@ TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
// 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<LocalParametrizationAngle>();
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
sb_ptr->setLocalParametrization(local_ptr);
// Fix state block
@@ -215,9 +284,19 @@ TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
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
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -225,23 +304,23 @@ TEST(SolverManager, FloatingStateBlock_AddUpdateLocalParam)
EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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, 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
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
@@ -252,11 +331,21 @@ TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
// 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
- EXPECT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ EXPECT_TRUE(solver_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
// check flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -272,19 +361,23 @@ TEST(SolverManager, FloatingStateBlock_AddLocalParamRemoveLocalParam)
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
- EXPECT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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, 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
auto sb_ptr = createStateBlock();
@@ -292,24 +385,43 @@ TEST(SolverManager, FloatingStateBlock_Remove)
// 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
- EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
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
auto sb_ptr = createStateBlock();
@@ -317,21 +429,79 @@ TEST(SolverManager, FloatingStateBlock_AddRemove)
// 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
- EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
+}
+
+TEST(SolverManager, FloatingStateBlock_AddRemoveAdd)
+{
+ ProblemPtr P = Problem::create("PO", 3);
+ auto solver_ptr = std::make_shared<SolverManagerDummy>(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);
+
+ // remove state_block
+ 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_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, 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
auto sb_ptr = createStateBlock();
@@ -339,26 +509,39 @@ TEST(SolverManager, FloatingStateBlock_DoubleRemove)
// 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_manager_ptr->check());
+ EXPECT_TRUE(solver_ptr->check());
}
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
auto sb_ptr = createStateBlock();
@@ -378,6 +561,8 @@ TEST(SolverManager, FloatingStateBlock_AddUpdated)
// 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));
@@ -396,7 +581,7 @@ TEST(SolverManager, FloatingStateBlock_AddUpdated)
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();
+ solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
// Check flags have been reset
EXPECT_FALSE(sb_ptr->fixUpdated());
@@ -407,6 +592,7 @@ TEST(SolverManager, FloatingStateBlock_AddUpdated)
// 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);
@@ -420,9 +606,11 @@ TEST(SolverManager, FloatingStateBlock_AddUpdated)
P->notifyStateBlock(sb_ptr,ADD);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
+ // 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);
@@ -434,8 +622,8 @@ TEST(SolverManager, FloatingStateBlock_AddUpdated)
// 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
auto sb_ptr = createStateBlock();
@@ -446,30 +634,45 @@ TEST(SolverManager, StateBlockAndFactor_Add)
// 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_manager_ptr->update();
+ solver_ptr->update();
// checks
- EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
+ 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);
// checks
- EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_FALSE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isFactorRegistered(fac_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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_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
auto sb_ptr = createStateBlock();
@@ -483,8 +686,20 @@ TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
// 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);
@@ -492,20 +707,30 @@ TEST(SolverManager, StateBlockAndFactor_DoubleAdd)
// 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_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// check stateblock
- EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_FALSE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isFactorRegistered(fac_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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", 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();
@@ -519,13 +744,24 @@ TEST(SolverManager, StateBlockAndFactor_Fix)
// 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());
// 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());
@@ -533,8 +769,8 @@ TEST(SolverManager, StateBlockAndFactor_Fix)
EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock unfixed
- EXPECT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ EXPECT_FALSE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
// Fix frame
sb_ptr->fix();
@@ -544,8 +780,16 @@ TEST(SolverManager, StateBlockAndFactor_Fix)
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_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());
@@ -553,14 +797,14 @@ TEST(SolverManager, StateBlockAndFactor_Fix)
EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ EXPECT_TRUE(solver_ptr->isStateBlockFixed(sb_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
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();
@@ -574,6 +818,13 @@ TEST(SolverManager, StateBlockAndFactor_AddFixed)
// 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());
@@ -588,7 +839,11 @@ TEST(SolverManager, StateBlockAndFactor_AddFixed)
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
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -596,15 +851,15 @@ TEST(SolverManager, StateBlockAndFactor_AddFixed)
EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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", 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();
@@ -618,13 +873,20 @@ TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
// 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<LocalParametrizationAngle>();
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
sb_ptr->setLocalParametrization(local_ptr);
// Fix state block
@@ -636,7 +898,11 @@ TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
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 flags
EXPECT_FALSE(sb_ptr->stateUpdated());
@@ -644,16 +910,16 @@ TEST(SolverManager, StateBlockAndFactor_UpdateLocalParam)
EXPECT_FALSE(sb_ptr->localParamUpdated());
// check stateblock fixed
- EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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", 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();
@@ -662,7 +928,7 @@ TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
auto fac_ptr = createFactor(sb_ptr);
// Local param
- LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
+ LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationQuaternionLocal>();
sb_ptr->setLocalParametrization(local_ptr);
// check flags
@@ -676,11 +942,25 @@ TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
// 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_manager_ptr->update();
+ solver_ptr->update();
- // check stateblock localparam
- EXPECT_TRUE(solver_manager_ptr->hasThisLocalParametrization(sb_ptr,local_ptr));
+ // 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());
@@ -696,17 +976,18 @@ TEST(SolverManager, StateBlockAndFactor_AddLocalParamRemoveLocalParam)
EXPECT_TRUE(sb_ptr->localParamUpdated());
// update solver manager
- solver_manager_ptr->update();
+ solver_ptr->update();
- // check stateblock localparam
- EXPECT_FALSE(solver_manager_ptr->hasLocalParametrization(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ // 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", 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();
@@ -720,28 +1001,46 @@ TEST(SolverManager, StateBlockAndFactor_RemoveStateBlock)
// 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();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// remove state_block
- P->notifyStateBlock(sb_ptr,REMOVE); // there is afactor involved, removal posponed (REMOVE in notification list)
+ 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(); // there is a factor involved, removal posponed (REMOVE in notification list)
- // checks
- Notification notif;
- EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isFactorRegistered(fac_ptr));
+ // check notifications
EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
- EXPECT_EQ(notif, REMOVE);
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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());
}
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();
@@ -755,26 +1054,46 @@ TEST(SolverManager, StateBlockAndFactor_RemoveFactor)
// 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();
+
+ // 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_manager_ptr->update();
+ solver_ptr->update();
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(), 0);
// checks
- EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
- EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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", 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();
@@ -788,25 +1107,39 @@ TEST(SolverManager, StateBlockAndFactor_AddRemoveStateBlock)
// 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_manager_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added)
+ solver_ptr->update(); // factor ADD notification is not executed since state block involved is missing (ADD notification added)
- // check state block
- Notification notif;
- EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
- EXPECT_EQ(notif, ADD);
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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", 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();
@@ -820,23 +1153,39 @@ TEST(SolverManager, StateBlockAndFactor_AddRemoveFactor)
// 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); // state block should be automatically stored as floating
+ 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_manager_ptr->update();
+ 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_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
- EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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", 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();
@@ -850,31 +1199,56 @@ TEST(SolverManager, StateBlockAndFactor_DoubleRemoveStateBlock)
// 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_manager_ptr->update(); // factor ADD is posponed
+ 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); // should be ignored
+ 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_manager_ptr->update();
+ solver_ptr->update(); // repeated REMOVE should be ignored
- // check state block
- Notification notif;
- EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
- EXPECT_EQ(notif, ADD);
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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", 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();
@@ -888,29 +1262,50 @@ TEST(SolverManager, StateBlockAndFactor_DoubleRemoveFactor)
// 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_manager_ptr->update(); // state block should be automatically stored as floating
+ 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); // should be ignored
+ 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_manager_ptr->update();
+ solver_ptr->update(); // repeated REMOVE should be ignored
// checks
- EXPECT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_TRUE(solver_manager_ptr->isStateBlockFloating(sb_ptr));
- EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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", 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();
@@ -936,10 +1331,12 @@ TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
// notify factor
P->notifyFactor(fac_ptr,ADD);
+ // check notifications
Notification notif;
- EXPECT_EQ(P->getStateBlockNotificationMapSize(),1);
EXPECT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
- EXPECT_EQ(notif, ADD);
+ EXPECT_EQ(ADD, notif);
+ EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
+ EXPECT_EQ(ADD, notif);
// == Insert OTHER notifications ==
@@ -954,7 +1351,7 @@ TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
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();
+ solver_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
// Check flags have been reset
EXPECT_FALSE(sb_ptr->fixUpdated());
@@ -978,7 +1375,7 @@ TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
P->notifyStateBlock(sb_ptr,ADD);
// update solver
- solver_manager_ptr->update();
+ solver_ptr->update();
EXPECT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
@@ -992,8 +1389,8 @@ TEST(SolverManager, StateBlockAndFactor_AddUpdatedStateBlock)
// ONLY FACTOR
TEST(SolverManager, OnlyFactor_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
auto sb_ptr = createStateBlock();
@@ -1004,22 +1401,30 @@ TEST(SolverManager, OnlyFactor_Add)
// 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(); // factor should be put in the notification list again
+ solver_ptr->update(); // factor ADD should be posponed (in the notification list again)
- // checks
- Notification notif;
- EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
EXPECT_TRUE(P->getFactorNotification(fac_ptr, notif));
- EXPECT_EQ(notif, ADD);
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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", 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();
@@ -1030,27 +1435,44 @@ TEST(SolverManager, OnlyFactor_Remove)
// 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(); // factor should be put in the notification list again
+ 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);
// 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(); // factor ADD notification should be cancelled
+ solver_ptr->update(); // nothing to update
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
// checks
- Notification notif;
- EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
- EXPECT_FALSE(P->getFactorNotification(fac_ptr, notif));
- EXPECT_TRUE(solver_manager_ptr->check());
+ 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", 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();
@@ -1061,35 +1483,48 @@ TEST(SolverManager, OnlyFactor_AddRemove)
// 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(); // factor ADD notification should be cancelled
+ solver_ptr->update(); // nothing to update
+
+ // check notifications
+ EXPECT_EQ(P->getStateBlockNotificationMapSize(),0);
+ EXPECT_EQ(P->getFactorNotificationMapSize(),0);
// checks
- Notification notif;
- EXPECT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr));
- EXPECT_FALSE(solver_manager_ptr->isFactorRegistered(fac_ptr));
- EXPECT_FALSE(P->getFactorNotification(fac_ptr, notif));
- EXPECT_TRUE(solver_manager_ptr->check());
+ EXPECT_FALSE(solver_ptr->isStateBlockRegistered(sb_ptr));
+ EXPECT_FALSE(solver_ptr->isFactorRegistered(fac_ptr));
+ EXPECT_TRUE(solver_ptr->check());
}
////////////////////////////////////////////////////////
// MULTITHREADING
-/*TEST(SolverManager, MultiThreadingTruncatedNotifications)
+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;
}});
@@ -1115,7 +1550,7 @@ TEST(SolverManager, OnlyFactor_AddRemove)
}
t.join();
-}*/
+}
int main(int argc, char **argv)
{