diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 7dad1e80efb84cc526484811d8ab246732fd7c79..1d38ec3e25e9085bcfda78251a61cddf05730f1f 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 ? @@ -596,12 +596,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/constraint_GPS_pseudorange_2D.h b/src/constraint_GPS_pseudorange_2D.h index 6bda014c58944abeff03bf04621f6723f234b1e6..7a04463b9c277bb3fb265fad1d0bd37cccfa06fc 100644 --- a/src/constraint_GPS_pseudorange_2D.h +++ b/src/constraint_GPS_pseudorange_2D.h @@ -4,8 +4,8 @@ #define LIGHT_SPEED_ 299792458 //Wolf includes -#include <sensor_GPS.h> -#include "feature_gps_pseudorange.h" +#include "sensor_GPS.h" +#include "feature_GPS_pseudorange.h" #include "constraint_autodiff.h" //std diff --git a/src/processor_GPS.cpp b/src/processor_GPS.cpp index a72f47119b5b67d48058e8da557ce8be824b461b..21940ef5ef058c3cfb06bfd0c688386e4dac7e37 100644 --- a/src/processor_GPS.cpp +++ b/src/processor_GPS.cpp @@ -6,7 +6,7 @@ #include <feature_GPS_pseudorange.h> #include <processor_GPS.h> -#include "capture_gps.h" +#include "capture_GPS.h" namespace wolf { diff --git a/src/processor_GPS.h b/src/processor_GPS.h index 0941b22e5227a797f18f7393b6ab4ba18788e131..bd14aa018fcdaa544507cde0f3bbcd7e6556b3ff 100644 --- a/src/processor_GPS.h +++ b/src/processor_GPS.h @@ -7,7 +7,7 @@ // Wolf includes #include "processor_base.h" -#include "capture_gps.h" +#include "capture_GPS.h" // Std includes diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index b26b977a519983f5d0e06b6cd177682dd78272a4..f31180caf423c6ac72013622913d8e955d1b7983 100644 --- a/src/solver/solver_manager.cpp +++ b/src/solver/solver_manager.cpp @@ -55,9 +55,7 @@ void SolverManager::update() } case StateBlock::Notification::STATE_UPDATE: { - const bool registered = state_blocks_.find(state)!=state_blocks_.end(); - - WOLF_DEBUG_COND(!registered, + WOLF_DEBUG_COND(state_blocks_.find(state)==state_blocks_.end(), "Updating the state of an unregistered StateBlock !"); // This will throw if StateBlock wasn't registered @@ -92,8 +90,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 8001f0b4a5c46aeebbc761b8f8f63ebaf499a70d..078d304265420ff86f7c43d5c079fcd2aaae2779 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -96,6 +96,10 @@ target_link_libraries(gtest_rotation ${PROJECT_NAME}) wolf_add_gtest(gtest_sensor_base gtest_sensor_base.cpp) target_link_libraries(gtest_sensor_base ${PROJECT_NAME}) +# SolverManager test +wolf_add_gtest(gtest_solver_manager gtest_solver_manager.cpp) +target_link_libraries(gtest_solver_manager ${PROJECT_NAME}) + # TimeStamp class test wolf_add_gtest(gtest_time_stamp gtest_time_stamp.cpp) target_link_libraries(gtest_time_stamp ${PROJECT_NAME}) @@ -110,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 new file mode 100644 index 0000000000000000000000000000000000000000..7543ea18155e7fc80a79e079d3ac35d343335634 --- /dev/null +++ b/src/test/gtest_solver_manager.cpp @@ -0,0 +1,408 @@ +/* + * gtest_solver_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 <iostream> + +using namespace wolf; +using namespace Eigen; + +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) : + SolverManager(wolf_problem) + { + }; + + bool isStateBlockRegistered(const StateBlockPtr& st) const + { + 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) + { + 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"); + SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P); + + // check double ointers to branches + ASSERT_EQ(P, solver_manager_ptr->getProblemPtr()); +} + +TEST(SolverManager, AddStateBlock) +{ + 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(); + + // check stateblock + ASSERT_TRUE(solver_manager_ptr->isStateBlockRegistered(sb_ptr)); +} + +TEST(SolverManager, DoubleAddStateBlock) +{ + 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(); + + // 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"); + 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(); + + // 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 + 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_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); + + // add stateblock + P->addStateBlock(sb_ptr); + + // 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 + 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) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} +