diff --git a/include/base/problem.h b/include/base/problem.h
index 84b51c2a6ce9638fef140c84c86240ebb596738d..0c82993f302c5ef5ca8f548e16a9247892eb9f6a 100644
--- a/include/base/problem.h
+++ b/include/base/problem.h
@@ -38,7 +38,6 @@ class Problem : public std::enable_shared_from_this<Problem>
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
ProcessorMotionPtr processor_motion_ptr_;
- StateBlockPtrList state_block_list_;
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
@@ -243,10 +242,6 @@ class Problem : public std::enable_shared_from_this<Problem>
// Solver management ----------------------------------------
- /** \brief Gets a reference to the state blocks list
- */
- StateBlockPtrList& getStateBlockPtrList();
-
/** \brief Notifies a new state block to be added to the solver manager
*/
StateBlockPtr addStateBlock(StateBlockPtr _state_ptr);
diff --git a/src/problem.cpp b/src/problem.cpp
index b9cd0a1e6bd63bfb40a3eb67bda35e1b5c6096b2..e88983fa90e6eedc7044bd8ddcd9831da7b53e78 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -368,14 +368,6 @@ void Problem::addLandmarkList(LandmarkBasePtrList& _lmk_list)
StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
{
//std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;
- if(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) != state_block_list_.end())
- {
- WOLF_WARN("Adding a state block that has already been added");
- return _state_ptr;
- }
-
- // add the state unit to the list
- state_block_list_.push_back(_state_ptr);
// Add add notification
// Check if there is already a notification for this state block
@@ -393,15 +385,6 @@ StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
void Problem::removeStateBlock(StateBlockPtr _state_ptr)
{
//std::cout << "Problem::removeStateBlockPtr " << _state_ptr.get() << std::endl;
- //assert(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) != state_block_list_.end() && "Removing a state_block that hasn't been added or already removed");
- if(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) == state_block_list_.end())
- {
- WOLF_WARN("Removing a state_block that hasn't been added or already removed");
- return;
- }
-
- // add the state unit to the list
- state_block_list_.remove(_state_ptr);
// Check if there is already a notification for this state block
auto notification_it = state_block_notification_map_.find(_state_ptr);
@@ -666,11 +649,6 @@ FrameBasePtr Problem::getLastKeyFrame()
return trajectory_ptr_->getLastKeyFrame();
}
-StateBlockPtrList& Problem::getStateBlockPtrList()
-{
- return state_block_list_;
-}
-
FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov, const TimeStamp& _ts, const Scalar _time_tolerance)
{
if ( ! prior_is_set_ )
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 493bc9dcd60683a2836811175a6c57c13c63ff98..f8a1e25eb565672922e224bcb48606d2cf3de67c 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -46,7 +46,7 @@ void SolverManager::update()
}
else
{
- WOLF_DEBUG("Tried adding an already registered StateBlock.");
+ WOLF_DEBUG("Tried to add an already added !");
}
}
else
@@ -77,12 +77,9 @@ void SolverManager::update()
}
// UPDATE STATE BLOCKS (state, fix or local parameterization)
- for (auto state_ptr : wolf_problem_->getStateBlockPtrList())
+ for (auto state_pair : state_blocks_)
{
- if (state_blocks_.find(state_ptr)==state_blocks_.end())
- continue;
-
- assert(state_blocks_.find(state_ptr)!=state_blocks_.end() && "Updating the state of an unregistered StateBlock !");
+ auto state_ptr = state_pair.first;
// state update
if (state_ptr->stateUpdated())
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 8ee7623dc68b7f637677fd24ceb6f90b083a4b39..dc1a90b80fdea318dd8a68629089b6535a688aa9 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -207,12 +207,10 @@ TEST(Problem, StateBlocks)
// 2 state blocks, fixed
SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
- ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) 2);
ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) 2);
// 3 state blocks, fixed
SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
- ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) (2 + 3));
ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) (2 + 3));
ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
@@ -226,15 +224,13 @@ TEST(Problem, StateBlocks)
// 2 state blocks, estimated
P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
- ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2));
ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 3 + 2));
// P->print(4,1,1,1);
// change some SB properties
St->unfixExtrinsics();
- ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2));
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),(unsigned int)(2 + 3 + 2 /*+ 2*/)); // XXX: 2 more notifications on the same SB!
+ ASSERT_EQ(P->getStateBlockNotificationMap().size(),(unsigned int)(2 + 3 + 2 /*+ 2*/)); // No more notifications on since the same SB!
// P->print(4,1,1,1);
}
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index d77defcbc697d87cf827a465d1ef99e9221293a6..daafef0ac3d0c2038229bcc994ecb0fe2607b954 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -118,21 +118,18 @@ TEST(TrajectoryBase, Add_Remove_Frame)
T->addFrame(f1); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2);
ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
std::cout << __LINE__ << std::endl;
T->addFrame(f2); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 4);
ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
std::cout << __LINE__ << std::endl;
T->addFrame(f3); // F
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 3);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 4);
ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
std::cout << __LINE__ << std::endl;
@@ -147,7 +144,6 @@ TEST(TrajectoryBase, Add_Remove_Frame)
f2->remove(); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2);
ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
std::cout << __LINE__ << std::endl;
@@ -158,7 +154,6 @@ TEST(TrajectoryBase, Add_Remove_Frame)
f3->remove(); // F
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2);
ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
std::cout << __LINE__ << std::endl;
@@ -167,7 +162,6 @@ TEST(TrajectoryBase, Add_Remove_Frame)
f1->remove(); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 0);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 0);
ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
std::cout << __LINE__ << std::endl;