diff --git a/include/base/problem/problem.h b/include/base/problem/problem.h
index a8ce14e2744fa63a79e81a8b456b7d63131d7226..9c1d768e0d1be2fbac9cf2c9d926e69d8769564e 100644
--- a/include/base/problem/problem.h
+++ b/include/base/problem/problem.h
@@ -3,6 +3,7 @@
// Fwd refs
namespace wolf{
+class SolverManager;
class HardwareBase;
class TrajectoryBase;
class MapBase;
@@ -33,6 +34,7 @@ enum Notification
*/
class Problem : public std::enable_shared_from_this<Problem>
{
+ friend SolverManager; // Enable SolverManager to acces protected functions (consumeXXXNotificationMap())
protected:
HardwareBasePtr hardware_ptr_;
@@ -271,9 +273,13 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
void removeStateBlock(StateBlockPtr _state_ptr);
- /** \brief Returns the map of factor notification to be handled by the solver (the map stored in this is emptied)
+ /** \brief Returns the size of the map of state block notification
*/
- std::map<StateBlockPtr,Notification> consumeStateBlockNotificationMap();
+ SizeStd getStateBlockNotificationMapSize() const;
+
+ /** \brief Returns if the state block has been notified, and the notification via parameter
+ */
+ bool getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const;
/** \brief Notifies a new factor to be added to the solver manager
*/
@@ -283,10 +289,24 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
void removeFactor(FactorBasePtr _factor_ptr);
+ /** \brief Returns the size of the map of factor notification
+ */
+ SizeStd getFactorNotificationMapSize() const;
+
+ /** \brief Returns if the factor has been notified, and the notification via parameter
+ */
+ bool getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const;
+
+ protected:
+ /** \brief Returns the map of state block notification to be handled by the solver (the map stored in this is emptied)
+ */
+ std::map<StateBlockPtr,Notification> consumeStateBlockNotificationMap();
+
/** \brief Returns the map of factor notification to be handled by the solver (the map stored in this is emptied)
*/
std::map<FactorBasePtr, Notification> consumeFactorNotificationMap();
+ public:
// Print and check ---------------------------------------
/**
* \brief print wolf tree
@@ -330,12 +350,25 @@ inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificati
return std::move(state_block_notification_map_);
}
+inline SizeStd Problem::getStateBlockNotificationMapSize() const
+{
+ std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
+ return state_block_notification_map_.size();
+}
+
inline std::map<FactorBasePtr,Notification> Problem::consumeFactorNotificationMap()
{
std::lock_guard<std::mutex> lock(mut_factor_notifications_);
return std::move(factor_notification_map_);
}
+inline wolf::SizeStd Problem::getFactorNotificationMapSize() const
+{
+ std::lock_guard<std::mutex> lock(mut_factor_notifications_);
+ return factor_notification_map_.size();
+}
+
+
} // namespace wolf
#endif // PROBLEM_H
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 171efc37c149187587567841a69bb19fd3eae6f6..17c30ca6bb3b07708c331f85a283b3bc87a7cea7 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -425,6 +425,16 @@ void Problem::removeStateBlock(StateBlockPtr _state_ptr)
state_block_notification_map_[_state_ptr] = REMOVE;
}
+bool Problem::getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const
+{
+ std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
+ if (state_block_notification_map_.find(sb_ptr) == state_block_notification_map_.end())
+ return false;
+
+ notif = state_block_notification_map_.at(sb_ptr);
+ return true;
+}
+
FactorBasePtr Problem::addFactor(FactorBasePtr _factor_ptr)
{
std::lock_guard<std::mutex> lock(mut_factor_notifications_);
@@ -466,6 +476,16 @@ void Problem::removeFactor(FactorBasePtr _factor_ptr)
factor_notification_map_[_factor_ptr] = REMOVE;
}
+bool Problem::getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const
+{
+ std::lock_guard<std::mutex> lock(mut_factor_notifications_);
+ if (factor_notification_map_.find(fac_ptr) == factor_notification_map_.end())
+ return false;
+
+ notif = factor_notification_map_.at(fac_ptr);
+ return true;
+}
+
void Problem::clearCovariance()
{
std::lock_guard<std::mutex> lock(mut_covariances_);
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index ad8852abc92a528381ed5f8b859f9909309617f0..3c5f2502cbce65184e2d63d59b85145776a86f5c 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -401,7 +401,7 @@ TEST(CeresManager, AddRemoveFactor)
// remove factor
P->removeFactor(c);
- ASSERT_TRUE(P->consumeFactorNotificationMap().empty()); // add+remove = empty
+ ASSERT_TRUE(P->getFactorNotificationMapSize() == 0); // add+remove = empty
// update solver
ceres_manager_ptr->update();
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index eb65356844d8160d0be7b79930c090db2e523895..143b66549f99b4d385bb3f61c5250a19979be257 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -9,6 +9,7 @@
#include "base/utils/logging.h"
#include "base/problem/problem.h"
+#include "base/solver/solver_manager.h"
#include "base/sensor/sensor_base.h"
#include "base/sensor/sensor_odom_3D.h"
#include "base/processor/processor_odom_3D.h"
@@ -19,11 +20,36 @@
using namespace wolf;
using namespace Eigen;
+
+WOLF_PTR_TYPEDEFS(DummySolverManager);
+
+struct DummySolverManager : public SolverManager
+{
+ DummySolverManager(const ProblemPtr& _problem)
+ : SolverManager(_problem)
+ {
+ //
+ }
+ virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
+ virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
+ virtual bool hasConverged(){return true;};
+ virtual SizeStd iterations(){return 0;};
+ virtual Scalar initialCost(){return 0;};
+ virtual Scalar finalCost(){return 0;};
+ virtual std::string solveImpl(const ReportVerbosity report_level){return std::string("");};
+ virtual void addFactor(const FactorBasePtr& fac_ptr){};
+ virtual void removeFactor(const FactorBasePtr& fac_ptr){};
+ virtual void addStateBlock(const StateBlockPtr& state_ptr){};
+ virtual void removeStateBlock(const StateBlockPtr& state_ptr){};
+ virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
+ virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
+};
+
TEST(Problem, create)
{
ProblemPtr P = Problem::create("POV 3D");
- // check double ointers to branches
+ // check double pointers to branches
ASSERT_EQ(P, P->getHardware()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getMap()->getProblem());
@@ -109,20 +135,20 @@ TEST(Problem, SetOrigin_PO_2D)
P->setPrior(x0, P0, t0, 1.0);
// check that no sensor has been added
- ASSERT_EQ(P->getHardware()->getSensorList().size(), (unsigned int) 0);
+ ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
// check that the state is correct
ASSERT_MATRIX_APPROX(x0, P->getCurrentState(), Constants::EPS_SMALL );
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
- ASSERT_EQ(T->getFrameList().size(), (unsigned int) 1);
+ ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
FrameBasePtr F = P->getLastFrame();
- ASSERT_EQ(F->getCaptureList().size(), (unsigned int) 1);
+ ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
CaptureBasePtr C = F->getCaptureList().front();
- ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1);
+ ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 1);
FeatureBasePtr f = C->getFeatureList().front();
- ASSERT_EQ(f->getFactorList().size(), (unsigned int) 1);
+ ASSERT_EQ(f->getFactorList().size(), (SizeStd) 1);
// check that the factor is absolute (no pointers to other F, f, or L)
FactorBasePtr c = f->getFactorList().front();
@@ -146,20 +172,20 @@ TEST(Problem, SetOrigin_PO_3D)
P->setPrior(x0, P0, t0, 1.0);
// check that no sensor has been added
- ASSERT_EQ(P->getHardware()->getSensorList().size(), (unsigned int) 0);
+ ASSERT_EQ(P->getHardware()->getSensorList().size(), (SizeStd) 0);
// check that the state is correct
ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
// check that we have one frame, one capture, one feature, one factor
TrajectoryBasePtr T = P->getTrajectory();
- ASSERT_EQ(T->getFrameList().size(), (unsigned int) 1);
+ ASSERT_EQ(T->getFrameList().size(), (SizeStd) 1);
FrameBasePtr F = P->getLastFrame();
- ASSERT_EQ(F->getCaptureList().size(), (unsigned int) 1);
+ ASSERT_EQ(F->getCaptureList().size(), (SizeStd) 1);
CaptureBasePtr C = F->getCaptureList().front();
- ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1);
+ ASSERT_EQ(C->getFeatureList().size(), (SizeStd) 1);
FeatureBasePtr f = C->getFeatureList().front();
- ASSERT_EQ(f->getFactorList().size(), (unsigned int) 1);
+ ASSERT_EQ(f->getFactorList().size(), (SizeStd) 1);
// check that the factor is absolute (no pointers to other F, f, or L)
FactorBasePtr c = f->getFactorList().front();
@@ -190,7 +216,7 @@ TEST(Problem, emplaceFrame_factory)
ASSERT_EQ(f2->getType().compare("POV 3D"), 0);
// check that all frames are effectively in the trajectory
- ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (unsigned int) 3);
+ ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (SizeStd) 3);
// check that all frames are linked to Problem
ASSERT_EQ(f0->getProblem(), P);
@@ -207,11 +233,11 @@ 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->consumeStateBlockNotificationMap().size(), (unsigned int) 2);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
// 3 state blocks, fixed
SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
- ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int) (/*2 + */3)); // consume empties the notification map, so only should contain notification since last call
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (2 + 3));
ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
params->time_tolerance = 0.1;
@@ -223,16 +249,40 @@ TEST(Problem, StateBlocks)
ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
// 2 state blocks, estimated
- P->emplaceFrame("PO 3D", KEY, xs, 0);
- ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int)(/*2 + 3*/ + 2)); // consume empties the notification map, so only should contain notification since last call
+ auto KF = P->emplaceFrame("PO 3D", KEY, xs, 0);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 3 + 2));
+
+ // Notifications
+ Notification notif;
+ ASSERT_TRUE(P->getStateBlockNotification(KF->getP(), notif));
+ ASSERT_EQ(notif, ADD);
+ ASSERT_TRUE(P->getStateBlockNotification(KF->getO(), notif));
+ ASSERT_EQ(notif, ADD);
// P->print(4,1,1,1);
// change some SB properties
St->unfixExtrinsics();
- ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 3 + 2)); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE
// P->print(4,1,1,1);
+
+ // consume notifications
+ DummySolverManagerPtr SM = std::make_shared<DummySolverManager>(P);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2 + 3 + 2));
+ SM->update(); // calls P->consumeStateBlockNotificationMap();
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) (0)); // consume empties the notification map
+
+ // remove frame
+ auto SB_P = KF->getP();
+ auto SB_O = KF->getO();
+ KF->remove();
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(2));
+ ASSERT_TRUE(P->getStateBlockNotification(SB_P, notif));
+ ASSERT_EQ(notif, REMOVE);
+ ASSERT_TRUE(P->getStateBlockNotification(SB_O, notif));
+ ASSERT_EQ(notif, REMOVE);
+
}
TEST(Problem, Covariances)
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index d5c33db6145648f3168f3c8a39ec02d1b2c8fce9..91ec5dba639cc2a89dc99a94de23e5483adf981b 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -450,9 +450,10 @@ TEST(SolverManager, AddUpdatedStateBlock)
// add state_block
P->addStateBlock(sb_ptr);
- auto state_block_notification_map = P->consumeStateBlockNotificationMap();
- ASSERT_EQ(state_block_notification_map.size(),1);
- ASSERT_EQ(state_block_notification_map.begin()->second,ADD);
+ Notification notif;
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
+ ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ ASSERT_EQ(notif, ADD);
// == Insert OTHER notifications ==
@@ -463,21 +464,25 @@ TEST(SolverManager, AddUpdatedStateBlock)
// Fix --> FLAG
sb_ptr->unfix();
- ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // No new notifications (fix and set state are flags in sb)
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(),1); // No new notifications (fix and set state are flags in sb)
+
+ // == consume empties the notification map ==
+ solver_manager_ptr->update(); // it calls P->consumeStateBlockNotificationMap();
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(),0);
// == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
P->removeStateBlock(sb_ptr);
- state_block_notification_map = P->consumeStateBlockNotificationMap();
- ASSERT_EQ(state_block_notification_map.size(),1);
- ASSERT_EQ(state_block_notification_map.begin()->second,REMOVE);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(),1);
+ ASSERT_TRUE(P->getStateBlockNotification(sb_ptr, notif));
+ ASSERT_EQ(notif, REMOVE);
// == ADD + REMOVE: notification map should be empty ==
P->addStateBlock(sb_ptr);
P->removeStateBlock(sb_ptr);
- ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty());
+ ASSERT_TRUE(P->getStateBlockNotificationMapSize() == 0);
// == UPDATES should clear the list of notifications ==
// add state_block
@@ -486,7 +491,7 @@ TEST(SolverManager, AddUpdatedStateBlock)
// update solver
solver_manager_ptr->update();
- ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // After solver_manager->update, notifications should be empty
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(),0); // After solver_manager->update, notifications should be empty
}
TEST(SolverManager, AddFactor)
@@ -504,6 +509,11 @@ TEST(SolverManager, AddFactor)
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+ // notification
+ Notification notif;
+ ASSERT_TRUE(P->getFactorNotification(c,notif));
+ ASSERT_EQ(notif, ADD);
+
// update solver
solver_manager_ptr->update();
@@ -532,6 +542,11 @@ TEST(SolverManager, RemoveFactor)
// add factor
P->removeFactor(c);
+ // notification
+ Notification notif;
+ ASSERT_TRUE(P->getFactorNotification(c,notif));
+ ASSERT_EQ(notif, REMOVE);
+
// update solver
solver_manager_ptr->update();
@@ -554,10 +569,17 @@ TEST(SolverManager, AddRemoveFactor)
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
+ // notification
+ Notification notif;
+ ASSERT_TRUE(P->getFactorNotification(c,notif));
+ ASSERT_EQ(notif, ADD);
+
// add factor
P->removeFactor(c);
- ASSERT_TRUE(P->consumeFactorNotificationMap().empty()); // ADD+REMOVE = empty
+ // notification
+ ASSERT_EQ(P->getFactorNotificationMapSize(), 0); // ADD+REMOVE cancels out
+ ASSERT_FALSE(P->getFactorNotification(c, notif)); // ADD+REMOVE cancels out
// update solver
solver_manager_ptr->update();
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index ab7818e7f64da636f3b21b15cd79daceaa7d251b..1cd7c1a201806e04b59d56b33b83ab2681aa4e1f 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -9,6 +9,7 @@
#include "base/utils/logging.h"
#include "base/problem/problem.h"
+#include "base/solver/solver_manager.h"
#include "base/trajectory/trajectory_base.h"
#include "base/frame/frame_base.h"
@@ -16,45 +17,26 @@
using namespace wolf;
-struct DummyNotificationProcessor
+struct DummySolverManager : public SolverManager
{
- DummyNotificationProcessor(ProblemPtr _problem)
- : problem_(_problem)
+ DummySolverManager(const ProblemPtr& _problem)
+ : SolverManager(_problem)
{
//
}
-
- void update()
- {
- if (problem_ == nullptr)
- {
- FAIL() << "problem_ is nullptr !";
- }
-
- auto sb_noti_map = problem_->consumeStateBlockNotificationMap();
- ASSERT_TRUE(problem_->consumeStateBlockNotificationMap().empty()); // consume should empty the notification map
-
- while (!sb_noti_map.empty())
- {
- switch (sb_noti_map.begin()->second)
- {
- case ADD:
- {
- break;
- }
- case REMOVE:
- {
- break;
- }
- default:
- throw std::runtime_error("SolverManager::update: State Block notification must be ADD or REMOVE.");
- }
- sb_noti_map.erase(sb_noti_map.begin());
- }
- ASSERT_TRUE(sb_noti_map.empty());
- }
-
- ProblemPtr problem_;
+ virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
+ virtual void computeCovariances(const std::vector<StateBlockPtr>& st_list){};
+ virtual bool hasConverged(){return true;};
+ virtual SizeStd iterations(){return 0;};
+ virtual Scalar initialCost(){return 0;};
+ virtual Scalar finalCost(){return 0;};
+ virtual std::string solveImpl(const ReportVerbosity report_level){return std::string("");};
+ virtual void addFactor(const FactorBasePtr& fac_ptr){};
+ virtual void removeFactor(const FactorBasePtr& fac_ptr){};
+ virtual void addStateBlock(const StateBlockPtr& state_ptr){};
+ virtual void removeStateBlock(const StateBlockPtr& state_ptr){};
+ virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr){};
+ virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr){};
};
/// Set to true if you want debug info
@@ -144,7 +126,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ProblemPtr P = Problem::create("PO 2D");
TrajectoryBasePtr T = P->getTrajectory();
- DummyNotificationProcessor N(P);
+ DummySolverManager N(P);
// Trajectory status:
// KF1 KF2 F3 frames
@@ -160,19 +142,20 @@ TEST(TrajectoryBase, Add_Remove_Frame)
// add frames and keyframes
T->addFrame(F1); // KF
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1);
- ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2);
+ ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 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(T->getFrameList(). size(), (SizeStd) 2);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
T->addFrame(F3); // F (not KF so state blocks are not notified)
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 3);
- ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2); // consumeStateBlockNotificationMap empties the notification map, 2 state blocks were notified since last call to consumeStateBlockNotificationMap
+ ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 3);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastFrame()->id(), F3->id());
@@ -180,14 +163,14 @@ TEST(TrajectoryBase, Add_Remove_Frame)
std::cout << __LINE__ << std::endl;
N.update();
- ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // consumeStateBlockNotificationMap was called in update() so it should be empty
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 0); // consumeStateBlockNotificationMap was called in update() so it should be empty
std::cout << __LINE__ << std::endl;
// remove frames and keyframes
F2->remove(); // KF
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2); // consumeStateBlockNotificationMap empties the notification map, 2 state blocks were notified since last call to consumeStateBlockNotificationMap
+ ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2);
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastFrame()->id(), F3->id());
@@ -196,19 +179,19 @@ 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(T->getFrameList(). size(), (SizeStd) 1);
std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastKeyFrame()->id(), F1->id());
F1->remove(); // KF
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 0);
+ ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 0);
std::cout << __LINE__ << std::endl;
N.update();
- ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // consumeStateBlockNotificationMap was called in update() so it should be empty
+ ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 0); // consumeStateBlockNotificationMap was called in update() so it should be empty
std::cout << __LINE__ << std::endl;
}