diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp index 0d47cd191f51c51126d38a591fea7de3614f53ae..6532dfc6a2b95d43451e00f8fad4797e7c53edf9 100644 --- a/src/solver/solver_manager.cpp +++ b/src/solver/solver_manager.cpp @@ -50,7 +50,7 @@ void SolverManager::update() } else { - WOLF_DEBUG("Tried to add an already added !"); + WOLF_DEBUG("Tried to add a StateBlock that was already added !"); } } else diff --git a/test/gtest_has_state_blocks.cpp b/test/gtest_has_state_blocks.cpp index 2d710f321b9452181a9044fece67d42ced120062..4118f0f4ddc7753a0d7b8c513b9733f0d1c3beff 100644 --- a/test/gtest_has_state_blocks.cpp +++ b/test/gtest_has_state_blocks.cpp @@ -12,6 +12,7 @@ #include "core/sensor/sensor_base.h" #include "core/landmark/landmark_base.h" #include "core/state_block/state_quaternion.h" +#include "core/ceres_wrapper/ceres_manager.h" using namespace wolf; @@ -108,6 +109,50 @@ TEST_F(HasStateBlocksTest, Notifications_addStateBlock) } +TEST_F(HasStateBlocksTest, Add_solve_notify_solve_add) +{ + + CeresManagerPtr solver = std::make_shared<CeresManager>(problem); + + + Notification n; + + // Add SB, make KF + + ASSERT_FALSE(problem->getStateBlockNotification(sbp0, n)); + + F0->link(problem->getTrajectory()); + F0->addStateBlock("V", sbv0); + F0->setKey(); + + ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n)); + ASSERT_EQ(n, ADD); + + // solve. This will clear all notifications + + std::string report = solver->solve(SolverManager::ReportVerbosity::FULL); + + ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n)); + + // Notify again the same SB + + problem->notifyStateBlock(sbv0, ADD); + + ASSERT_TRUE(problem->getStateBlockNotification(sbv0, n)); + ASSERT_EQ(n, ADD); + + // solve again + + report = solver->solve(SolverManager::ReportVerbosity::FULL); + + ASSERT_FALSE(problem->getStateBlockNotification(sbv0, n)); + + // Add again the same SB. This should crash + + ASSERT_DEATH( F0->addStateBlock("V", sbv0) , "" ); + +} + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv);