diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 9b096f7c82d24aa1b2b939919f46a5190f8a65c0..57efea34c4fd4a4786dedf1f33d8d29c4e125705 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 213e29b9e8acb4c4af36da0c6365d1a538aa2d69..3b0ed8e164900db7f7bbe5e244e7a424f16758c9 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 StateBlockPtrList& 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_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();
@@ -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_FRAME, 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_FRAME, 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 9de5032b38af32cd564429fae2d0833c60817ef6..a38a7ee467e95f23a3f922c4b7bceb117e06cd14 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 de582e26b0e3fbfa6ca61876e82d3b9870ea0ff8..ec2b314fc2dda90f7b6b53bb83713095fe46e257 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 StateBlockPtrList& 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
@@ -103,7 +85,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
@@ -119,19 +101,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());
@@ -139,14 +122,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());
@@ -155,19 +138,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;
}