From 928494d54c93597381969046a5b4f5fe2132b066 Mon Sep 17 00:00:00 2001
From: Jeremie Deray <jeremie.deray@pal-robotics.com>
Date: Mon, 30 Oct 2017 16:48:28 +0100
Subject: [PATCH] removed formed stateblock notif list
---
src/problem.cpp | 17 +----------------
src/problem.h | 10 ----------
src/test/gtest_problem.cpp | 8 ++++----
src/test/gtest_trajectory.cpp | 12 ++++++------
4 files changed, 11 insertions(+), 36 deletions(-)
diff --git a/src/problem.cpp b/src/problem.cpp
index 361a8b242..c5169f931 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -363,9 +363,8 @@ StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
// add the state unit to the list
state_block_list_.push_back(_state_ptr);
- // queue for solver manager
- state_block_notification_list_.push_back(StateBlockNotification({ADD,_state_ptr}));
+ // queue for solver manager
_state_ptr->notify(StateBlock::Notification::ADD);
notified_state_block_list_.push_back(_state_ptr);
notified_state_block_list_.unique();
@@ -378,8 +377,6 @@ void Problem::updateStateBlockPtr(StateBlockPtr _state_ptr)
//std::cout << "Problem::updateStateBlockPtr " << _state_ptr.get() << std::endl;
// queue for solver manager
- state_block_notification_list_.push_back(StateBlockNotification({UPDATE,_state_ptr}));
-
_state_ptr->notify(StateBlock::Notification::FIX_UPDATE);
notified_state_block_list_.push_back(_state_ptr);
notified_state_block_list_.unique();
@@ -392,19 +389,7 @@ void Problem::removeStateBlockPtr(StateBlockPtr _state_ptr)
// add the state unit to the list
state_block_list_.remove(_state_ptr);
- // Check if the state addition is still as a notification
- auto state_notif_it = state_block_notification_list_.begin();
- for (; state_notif_it != state_block_notification_list_.end(); state_notif_it++)
- if (state_notif_it->notification_ == ADD && state_notif_it->state_block_ptr_ == _state_ptr)
- break;
-
- // Remove addition notification
- if (state_notif_it != state_block_notification_list_.end())
- state_block_notification_list_.erase(state_notif_it);
// Add remove notification
- else
- state_block_notification_list_.push_back(StateBlockNotification({REMOVE, _state_ptr}));
-
_state_ptr->notify(StateBlock::Notification::REMOVE);
notified_state_block_list_.push_back(_state_ptr);
notified_state_block_list_.unique();
diff --git a/src/problem.h b/src/problem.h
index eac0239ec..f25e4dfdc 100644
--- a/src/problem.h
+++ b/src/problem.h
@@ -52,7 +52,6 @@ class Problem : public std::enable_shared_from_this<Problem>
ProcessorMotionPtr processor_motion_ptr_;
StateBlockList state_block_list_;
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
- std::list<StateBlockNotification> state_block_notification_list_;
std::list<ConstraintNotification> constraint_notification_list_;
StateBlockList notified_state_block_list_;
bool origin_is_set_;
@@ -272,10 +271,6 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
void removeStateBlockPtr(StateBlockPtr _state_ptr);
- /** \brief Gets a queue of state blocks notification to be handled by the solver
- */
- std::list<StateBlockNotification>& getStateBlockNotificationList();
-
/** \brief Gets a list of state blocks which state has been changed to be handled by the solver
*/
StateBlockList& getNotifiedStateBlockList();
@@ -317,11 +312,6 @@ inline wolf::ProcessorMotionPtr& Problem::getProcessorMotionPtr()
namespace wolf
{
-inline std::list<StateBlockNotification>& Problem::getStateBlockNotificationList()
-{
- return state_block_notification_list_;
-}
-
inline std::list<ConstraintNotification>& Problem::getConstraintNotificationList()
{
return constraint_notification_list_;
diff --git a/src/test/gtest_problem.cpp b/src/test/gtest_problem.cpp
index 314c406c5..553a6ebb0 100644
--- a/src/test/gtest_problem.cpp
+++ b/src/test/gtest_problem.cpp
@@ -216,12 +216,12 @@ 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->getStateBlockList().size(), 2);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 2);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2);
// 3 state blocks, fixed
SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
ASSERT_EQ(P->getStateBlockList().size(), 2 + 3);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 2 + 3);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2 + 3);
ProcessorBasePtr pt = P->installProcessor("IMAGE LANDMARK", "ORB landmark tracker", "camera", wolf_root + "/src/examples/processor_image_ORB.yaml");
ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
@@ -229,7 +229,7 @@ TEST(Problem, StateBlocks)
// 2 state blocks, estimated
P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
ASSERT_EQ(P->getStateBlockList().size(), 2 + 3 + 2);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 2 + 3 + 2);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2 + 3 + 2);
// P->print(4,1,1,1);
@@ -237,7 +237,7 @@ TEST(Problem, StateBlocks)
// change some SB properties
St->unfixExtrinsics();
ASSERT_EQ(P->getStateBlockList().size(), 2 + 3 + 2);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 2 + 3 + 2 + 2); // XXX: 2 more notifications on the same SB!
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2 + 3 + 2 + 2); // XXX: 2 more notifications on the same SB!
// P->print(4,1,1,1);
}
diff --git a/src/test/gtest_trajectory.cpp b/src/test/gtest_trajectory.cpp
index f6835809e..ec53ef66f 100644
--- a/src/test/gtest_trajectory.cpp
+++ b/src/test/gtest_trajectory.cpp
@@ -77,19 +77,19 @@ TEST(TrajectoryBase, Add_Remove_Frame)
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 1);
ASSERT_EQ(P->getStateBlockList(). size(), 2);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 2);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2);
T->addFrame(f2); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 2);
ASSERT_EQ(P->getStateBlockList(). size(), 4);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 4);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 4);
T->addFrame(f3); // F
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 3);
ASSERT_EQ(P->getStateBlockList(). size(), 4);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 4);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 4);
ASSERT_EQ(T->getLastFramePtr()->id(), f3->id());
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
@@ -99,7 +99,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 2);
ASSERT_EQ(P->getStateBlockList(). size(), 2);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 2);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2);
ASSERT_EQ(T->getLastFramePtr()->id(), f3->id());
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id());
@@ -108,7 +108,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 1);
ASSERT_EQ(P->getStateBlockList(). size(), 2);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 2);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 2);
ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id());
@@ -116,7 +116,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), 0);
ASSERT_EQ(P->getStateBlockList(). size(), 0);
- ASSERT_EQ(P->getStateBlockNotificationList().size(), 0);
+ ASSERT_EQ(P->getNotifiedStateBlockList().size(), 0);
}
--
GitLab