diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c8774da932baa28806790fc8b1d8743f5caa4a0a..99e6d44b4271a7975e2570a3dc889d2c4d89989b 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -17,7 +17,7 @@ ENDIF() option(_WOLF_TRACE "Enable wolf tracing macro" ON) option(BUILD_EXAMPLES "Build examples" OFF) - +set(BUILD_TESTS true) # Does this has any other interest # but for the examples ? @@ -582,12 +582,10 @@ export(PACKAGE ${PROJECT_NAME}) ## Testing ## ############# IF (GLOG_FOUND) - if(BUILD_TESTS) - + IF(BUILD_TESTS) MESSAGE("Building tests.") add_subdirectory(test) - - endif(BUILD_TESTS) + ENDIF(BUILD_TESTS) ENDIF (GLOG_FOUND) IF(BUILD_EXAMPLES) diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index f37b2377b391564d10a6163ca95d89a5823c480c..9cd617a40867511acb34f304f10b80a0d7be8c37 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -7,7 +7,7 @@ namespace wolf { -CeresManager::CeresManager(ProblemPtr& _wolf_problem, +CeresManager::CeresManager(const ProblemPtr& _wolf_problem, const ceres::Solver::Options& _ceres_options) : SolverManager(_wolf_problem), ceres_options_(_ceres_options) diff --git a/src/ceres_wrapper/ceres_manager.h b/src/ceres_wrapper/ceres_manager.h index 2e1e0cb998df70e5ac6e42968edc62a6b326d151..6486f200efd89a492cc0260258a542c2ac50823d 100644 --- a/src/ceres_wrapper/ceres_manager.h +++ b/src/ceres_wrapper/ceres_manager.h @@ -40,7 +40,7 @@ protected: public: - CeresManager(ProblemPtr& _wolf_problem, + CeresManager(const ProblemPtr& _wolf_problem, const ceres::Solver::Options& _ceres_options = ceres::Solver::Options()); diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index b26b977a519983f5d0e06b6cd177682dd78272a4..26fb4fd7fe7a3df9f51f489b5fb5cce6b265339d 100644 --- a/src/solver/solver_manager.cpp +++ b/src/solver/solver_manager.cpp @@ -92,8 +92,11 @@ void SolverManager::update() WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(), "Tried to remove a StateBlock that was not added !"); - if (state_blocks_.erase(state) > 0) - removeStateBlock(state); + if (state_blocks_.find(state)!=state_blocks_.end()) + { + removeStateBlock(state); + state_blocks_.erase(state); + } break; } diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index cf4e6cbcdc39b083c506c0f519da489a42637726..078d304265420ff86f7c43d5c079fcd2aaae2779 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -114,6 +114,12 @@ target_link_libraries(gtest_trajectory ${PROJECT_NAME}) # ------- Now Derived classes ---------- +IF (Ceres_FOUND) +# CeresManager test +wolf_add_gtest(gtest_ceres_manager gtest_ceres_manager.cpp) +target_link_libraries(gtest_ceres_manager ${PROJECT_NAME}) +ENDIF(Ceres_FOUND) + # ConstraintAbs(P/O/V) classes test wolf_add_gtest(gtest_constraint_absolute gtest_constraint_absolute.cpp) target_link_libraries(gtest_constraint_absolute ${PROJECT_NAME}) diff --git a/src/test/gtest_ceres_manager.cpp b/src/test/gtest_ceres_manager.cpp new file mode 100644 index 0000000000000000000000000000000000000000..11a782e0dd4eb829103d49e98e2bd659ac73c6b7 --- /dev/null +++ b/src/test/gtest_ceres_manager.cpp @@ -0,0 +1,404 @@ +/* + * gtest_ceres_manager.cpp + * + * Created on: Jun, 2018 + * Author: jvallve + */ + +#include "utils_gtest.h" +#include "../src/logging.h" + +#include "../problem.h" +#include "../sensor_base.h" +#include "../state_block.h" +#include "../capture_void.h" +#include "../constraint_pose_2D.h" +#include "../solver/solver_manager.h" +#include "../ceres_wrapper/ceres_manager.h" + +#include "ceres/ceres.h" + +#include <iostream> + +using namespace wolf; +using namespace Eigen; + +WOLF_PTR_TYPEDEFS(CeresManagerWrapper); +class CeresManagerWrapper : public CeresManager +{ + public: + CeresManagerWrapper(const ProblemPtr& wolf_problem) : + CeresManager(wolf_problem) + { + }; + + bool isStateBlockRegisteredCeresManager(const StateBlockPtr& st) + { + return ceres_problem_->HasParameterBlock(SolverManager::getAssociatedMemBlockPtr(st)); + }; + + bool isStateBlockRegisteredSolverManager(const StateBlockPtr& st) + { + return state_blocks_.find(st)!=state_blocks_.end(); + }; + + bool isStateBlockFixed(const StateBlockPtr& st) + { + return ceres_problem_->IsParameterBlockConstant(SolverManager::getAssociatedMemBlockPtr(st)); + }; + + int numStateBlocks() + { + return ceres_problem_->NumParameterBlocks(); + }; + + bool isConstraintRegistered(const ConstraintBasePtr& ctr_ptr) const + { + return ctr_2_residual_idx_.find(ctr_ptr) != ctr_2_residual_idx_.end() && ctr_2_costfunction_.find(ctr_ptr) != ctr_2_costfunction_.end(); + }; + +}; + +TEST(SolverManager, Create) +{ + ProblemPtr P = Problem::create("PO 2D"); + CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + + // check double ointers to branches + ASSERT_EQ(P, ceres_manager_ptr->getProblemPtr()); +} + +TEST(SolverManager, AddStateBlock) +{ + ProblemPtr P = Problem::create("PO 2D"); + CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // add stateblock + P->addStateBlock(sb_ptr); + + // update solver + ceres_manager_ptr->update(); + + // check stateblock + ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); + ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); +} + +TEST(SolverManager, DoubleAddStateBlock) +{ + ProblemPtr P = Problem::create("PO 2D"); + CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // add stateblock + P->addStateBlock(sb_ptr); + + // update solver + ceres_manager_ptr->update(); + + // add stateblock again + P->addStateBlock(sb_ptr); + + // update solver + ceres_manager_ptr->update(); + + // check stateblock + ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); + ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); +} + +TEST(SolverManager, UpdateStateBlock) +{ + ProblemPtr P = Problem::create("PO 2D"); + CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // add stateblock + P->addStateBlock(sb_ptr); + + // update solver + ceres_manager_ptr->update(); + + // check stateblock unfixed + ASSERT_FALSE(ceres_manager_ptr->isStateBlockFixed(sb_ptr)); + + // Fix frame + sb_ptr->fix(); + + // update stateblock + P->updateStateBlockPtr(sb_ptr); + + // update solver manager + ceres_manager_ptr->update(); + + // check stateblock fixed + ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr)); +} + +TEST(SolverManager, AddUpdateStateBlock) +{ + ProblemPtr P = Problem::create("PO 2D"); + CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // add stateblock + P->addStateBlock(sb_ptr); + + // Fix state block + sb_ptr->fix(); + + // update stateblock + P->updateStateBlockPtr(sb_ptr); + + // update solver manager + ceres_manager_ptr->update(); + + // check stateblock fixed + ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); + ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); + ASSERT_TRUE(ceres_manager_ptr->isStateBlockFixed(sb_ptr)); +} + +TEST(SolverManager, RemoveStateBlock) +{ + ProblemPtr P = Problem::create("PO 2D"); + CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // add stateblock + P->addStateBlock(sb_ptr); + + // update solver + ceres_manager_ptr->update(); + + ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); + ASSERT_TRUE(ceres_manager_ptr->isStateBlockRegisteredCeresManager(sb_ptr)); + + // remove state_block + P->removeStateBlockPtr(sb_ptr); + + // update solver + ceres_manager_ptr->update(); + + // check stateblock + ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); + ASSERT_TRUE(ceres_manager_ptr->numStateBlocks() == 0); +} + +TEST(SolverManager, AddRemoveStateBlock) +{ + ProblemPtr P = Problem::create("PO 2D"); + CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // add stateblock + P->addStateBlock(sb_ptr); + + // remove state_block + P->removeStateBlockPtr(sb_ptr); + + // update solver + ceres_manager_ptr->update(); + + // check no stateblocks + ASSERT_FALSE(ceres_manager_ptr->isStateBlockRegisteredSolverManager(sb_ptr)); + ASSERT_TRUE(ceres_manager_ptr->numStateBlocks() == 0); +} + +TEST(SolverManager, RemoveUpdateStateBlock) +{ + ProblemPtr P = Problem::create("PO 2D"); + CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // add stateblock + P->addStateBlock(sb_ptr); + + // update solver + ceres_manager_ptr->update(); + + // remove state_block + P->removeStateBlockPtr(sb_ptr); + + // update solver + ceres_manager_ptr->update(); + + // Fix state block + sb_ptr->fix(); + + ASSERT_DEATH({ + // update stateblock + P->updateStateBlockPtr(sb_ptr); + + // update solver manager + ceres_manager_ptr->update(); + },""); +} + +TEST(SolverManager, DoubleRemoveStateBlock) +{ + ProblemPtr P = Problem::create("PO 2D"); + CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // add stateblock + P->addStateBlock(sb_ptr); + + // remove state_block + P->removeStateBlockPtr(sb_ptr); + + // update solver + ceres_manager_ptr->update(); + + // remove state_block + P->removeStateBlockPtr(sb_ptr); + + // update solver manager + ceres_manager_ptr->update(); +} + +TEST(SolverManager, AddConstraint) +{ + ProblemPtr P = Problem::create("PO 2D"); + CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // Create (and add) constraint point 2d + FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); + CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f))); + + // update solver + ceres_manager_ptr->update(); + + // check constraint + ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c)); +} + +TEST(SolverManager, RemoveConstraint) +{ + ProblemPtr P = Problem::create("PO 2D"); + CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // Create (and add) constraint point 2d + FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); + CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f))); + + // update solver + ceres_manager_ptr->update(); + + // add constraint + P->removeConstraintPtr(c); + + // update solver + ceres_manager_ptr->update(); + + // check constraint + ASSERT_FALSE(ceres_manager_ptr->isConstraintRegistered(c)); +} + +TEST(SolverManager, AddRemoveConstraint) +{ + ProblemPtr P = Problem::create("PO 2D"); + CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // Create (and add) constraint point 2d + FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); + CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f))); + + ASSERT_TRUE(P->getConstraintNotificationList().front().constraint_ptr_ == c); + + // add constraint + P->removeConstraintPtr(c); + + ASSERT_TRUE(P->getConstraintNotificationList().empty()); + + // update solver + ceres_manager_ptr->update(); + + // check constraint + ASSERT_FALSE(ceres_manager_ptr->isConstraintRegistered(c)); +} + +TEST(SolverManager, DoubleRemoveConstraint) +{ + ProblemPtr P = Problem::create("PO 2D"); + CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // Create (and add) constraint point 2d + FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); + CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f))); + + // update solver + ceres_manager_ptr->update(); + + // remove constraint + P->removeConstraintPtr(c); + + // update solver + ceres_manager_ptr->update(); + + // remove constraint + P->removeConstraintPtr(c); + + ASSERT_DEATH({ + // update solver + ceres_manager_ptr->update();},""); + + // check constraint + ASSERT_FALSE(ceres_manager_ptr->isConstraintRegistered(c)); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + diff --git a/src/test/gtest_solver_manager.cpp b/src/test/gtest_solver_manager.cpp index 16b3fad389ca792923e0fe1a80a24b91a45f4d88..7543ea18155e7fc80a79e079d3ac35d343335634 100644 --- a/src/test/gtest_solver_manager.cpp +++ b/src/test/gtest_solver_manager.cpp @@ -10,6 +10,9 @@ #include "../problem.h" #include "../sensor_base.h" +#include "../state_block.h" +#include "../capture_void.h" +#include "../constraint_pose_2D.h" #include "../solver/solver_manager.h" #include <iostream> @@ -21,10 +24,12 @@ WOLF_PTR_TYPEDEFS(SolverManagerWrapper); class SolverManagerWrapper : public SolverManager { public: + std::list<ConstraintBasePtr> constraints_; + std::map<StateBlockPtr,bool> state_block_fixed_; + SolverManagerWrapper(const ProblemPtr& wolf_problem) : - SolverManagerWrapper(wolf_problem) + SolverManager(wolf_problem) { - }; bool isStateBlockRegistered(const StateBlockPtr& st) const @@ -32,23 +37,47 @@ class SolverManagerWrapper : public SolverManager return state_blocks_.find(st)!=state_blocks_.end(); }; + bool isStateBlockFixed(const StateBlockPtr& st) const + { + return state_block_fixed_.at(st); + }; + + bool isConstraintRegistered(const ConstraintBasePtr& ctr_ptr) const + { + return std::find(constraints_.begin(), constraints_.end(), ctr_ptr) != constraints_.end(); + }; + virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){}; virtual void computeCovariances(const StateBlockList& st_list){}; protected: virtual std::string solveImpl(const ReportVerbosity report_level){ return std::string("");}; - virtual void addConstraint(const ConstraintBasePtr& ctr_ptr){}; - virtual void removeConstraint(const ConstraintBasePtr& ctr_ptr){}; - virtual void addStateBlock(const StateBlockPtr& state_ptr){}; - virtual void removeStateBlock(const StateBlockPtr& state_ptr){}; - virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){}; + virtual void addConstraint(const ConstraintBasePtr& ctr_ptr) + { + constraints_.push_back(ctr_ptr); + }; + virtual void removeConstraint(const ConstraintBasePtr& ctr_ptr) + { + constraints_.remove(ctr_ptr); + }; + virtual void addStateBlock(const StateBlockPtr& state_ptr) + { + state_block_fixed_[state_ptr] = state_ptr->isFixed(); + }; + virtual void removeStateBlock(const StateBlockPtr& state_ptr) + { + state_block_fixed_.erase(state_ptr); + }; + virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) + { + state_block_fixed_[state_ptr] = state_ptr->isFixed(); + }; }; TEST(SolverManager, Create) { ProblemPtr P = Problem::create("PO 2D"); - //CeresManagerPtr ceres_manager_ptr = std::make_shared<CeresManager>(P); SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); // check double ointers to branches @@ -58,69 +87,318 @@ TEST(SolverManager, Create) TEST(SolverManager, AddStateBlock) { ProblemPtr P = Problem::create("PO 2D"); - //CeresManagerPtr ceres_manager_ptr = std::make_shared<CeresManager>(P); SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - // Set prior -> frame + constraint - Vector3s F1_state; F1_state << 1, 2, 3; - FrameBasePtr F1 = P->emplaceFrame(KEY_FRAME, F1_state,TimeStamp()); + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - // update solver manager + // add stateblock + P->addStateBlock(sb_ptr); + + // update solver solver_manager_ptr->update(); // check stateblock - ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(F1->getStateBlockPtr(0))); - ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(F1->getStateBlockPtr(1))); - ASSERT_EQ(solver_manager_ptr->getAssociatedMemBlock(F1->getStateBlockPtr(0)), F1->getStateBlockPtr(0)->getState()); + ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); } -/*TEST(SolverManager, AddPrior) +TEST(SolverManager, DoubleAddStateBlock) { ProblemPtr P = Problem::create("PO 2D"); - CeresManagerPtr ceres_manager_ptr = std::make_shared<CeresManager>(P); + SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - // Set prior -> frame + constraint - Vector3s F1_state; F1_state << 1, 2, 3; - FrameBasePtr F1 = P->setPrior(Vector3s::Zero(),Matrix3s::Identity(),TimeStamp()); - ConstraintBasePtr C1 = F1->getCaptureList().front()->getFeatureList().front()->getConstraintList().front(); + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - // update solver manager - ceres_manager_ptr->update(); + // add stateblock + P->addStateBlock(sb_ptr); - // check stateblock - ASSERT_EQ(ceres_manager_ptr->getAssociatedMemBlock(F1->getStateBlockPtr(0)), F1->getStateBlockPtr(0)->getState()); + // update solver + solver_manager_ptr->update(); - // solve - ceres_manager_ptr->solve(); + // add stateblock again + P->addStateBlock(sb_ptr); + + // update solver + solver_manager_ptr->update(); + + // check stateblock + ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); } TEST(SolverManager, UpdateStateBlock) { ProblemPtr P = Problem::create("PO 2D"); - CeresManagerPtr ceres_manager_ptr = std::make_shared<CeresManager>(P); + SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); - // Set prior -> frame + constraint - Vector3s F1_state; F1_state << 1, 2, 3; - FrameBasePtr F1 = P->setPrior(F1_state,Matrix3s::Identity(),TimeStamp()); - ConstraintBasePtr C1 = F1->getCaptureList().front()->getFeatureList().front()->getConstraintList().front(); + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // add stateblock + P->addStateBlock(sb_ptr); + + // update solver + solver_manager_ptr->update(); + + // check stateblock unfixed + ASSERT_FALSE(solver_manager_ptr->isStateBlockFixed(sb_ptr)); + + // Fix frame + sb_ptr->fix(); + + // update stateblock + P->updateStateBlockPtr(sb_ptr); // update solver manager - ceres_manager_ptr->update(); + solver_manager_ptr->update(); + + // check stateblock fixed + ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr)); +} + +TEST(SolverManager, AddUpdateStateBlock) +{ + ProblemPtr P = Problem::create("PO 2D"); + SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // add stateblock + P->addStateBlock(sb_ptr); + + // Fix state block + sb_ptr->fix(); + + // update stateblock + P->updateStateBlockPtr(sb_ptr); + + // update solver manager + solver_manager_ptr->update(); + + // check stateblock fixed + ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); + ASSERT_TRUE(solver_manager_ptr->isStateBlockFixed(sb_ptr)); +} + +TEST(SolverManager, RemoveStateBlock) +{ + ProblemPtr P = Problem::create("PO 2D"); + SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // add stateblock + P->addStateBlock(sb_ptr); + + // update solver + solver_manager_ptr->update(); + + // remove state_block + P->removeStateBlockPtr(sb_ptr); + + // update solver + solver_manager_ptr->update(); // check stateblock - ASSERT_EQ(ceres_manager_ptr->getAssociatedMemBlock(F1->getStateBlockPtr(0)), F1->getStateBlockPtr(0)->getState()); + ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); +} + +TEST(SolverManager, AddRemoveStateBlock) +{ + ProblemPtr P = Problem::create("PO 2D"); + SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); - // solve - ceres_manager_ptr->solve(); + // add stateblock + P->addStateBlock(sb_ptr); - // Change state and fix - F1->setState(Vector3s::Zero()); - F1->fix(); + // remove state_block + P->removeStateBlockPtr(sb_ptr); + + // update solver + solver_manager_ptr->update(); + + // check stateblock + ASSERT_FALSE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); +} + +TEST(SolverManager, RemoveUpdateStateBlock) +{ + ProblemPtr P = Problem::create("PO 2D"); + SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // add stateblock + P->addStateBlock(sb_ptr); + + // remove state_block + P->removeStateBlockPtr(sb_ptr); + + // update solver + solver_manager_ptr->update(); + + // Fix state block + sb_ptr->fix(); + + ASSERT_DEATH({ + // update stateblock + P->updateStateBlockPtr(sb_ptr); + + // update solver manager + solver_manager_ptr->update(); + },""); +} + +TEST(SolverManager, DoubleRemoveStateBlock) +{ + ProblemPtr P = Problem::create("PO 2D"); + SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // add stateblock + P->addStateBlock(sb_ptr); + + // remove state_block + P->removeStateBlockPtr(sb_ptr); + + // update solver + solver_manager_ptr->update(); + + // remove state_block + P->removeStateBlockPtr(sb_ptr); // update solver manager - ceres_manager_ptr->update(); + solver_manager_ptr->update(); +} + +TEST(SolverManager, AddConstraint) +{ + ProblemPtr P = Problem::create("PO 2D"); + SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // Create (and add) constraint point 2d + FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); + CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f))); + + // update solver + solver_manager_ptr->update(); + + // check constraint + ASSERT_TRUE(solver_manager_ptr->isConstraintRegistered(c)); +} + +TEST(SolverManager, RemoveConstraint) +{ + ProblemPtr P = Problem::create("PO 2D"); + SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); -}*/ + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // Create (and add) constraint point 2d + FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); + CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f))); + + // update solver + solver_manager_ptr->update(); + + // add constraint + P->removeConstraintPtr(c); + + // update solver + solver_manager_ptr->update(); + + // check constraint + ASSERT_FALSE(solver_manager_ptr->isConstraintRegistered(c)); +} + +TEST(SolverManager, AddRemoveConstraint) +{ + ProblemPtr P = Problem::create("PO 2D"); + SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // Create (and add) constraint point 2d + FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); + CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f))); + + ASSERT_TRUE(P->getConstraintNotificationList().front().constraint_ptr_ == c); + + // add constraint + P->removeConstraintPtr(c); + + ASSERT_TRUE(P->getConstraintNotificationList().empty()); + + // update solver + solver_manager_ptr->update(); + + // check constraint + ASSERT_FALSE(solver_manager_ptr->isConstraintRegistered(c)); +} + +TEST(SolverManager, DoubleRemoveConstraint) +{ + ProblemPtr P = Problem::create("PO 2D"); + SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); + + // Create State block + Vector2s state; state << 1, 2; + StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state); + + // Create (and add) constraint point 2d + FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0)); + CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr)); + FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity())); + ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f))); + + // update solver + solver_manager_ptr->update(); + + // remove constraint + P->removeConstraintPtr(c); + + // update solver + solver_manager_ptr->update(); + + // remove constraint + P->removeConstraintPtr(c); + + // update solver + solver_manager_ptr->update(); + + // check constraint + ASSERT_FALSE(solver_manager_ptr->isConstraintRegistered(c)); +} int main(int argc, char **argv) {