Skip to content
Snippets Groups Projects
Commit f73732a1 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

adapted tests to protected consume functions and new getters

parent e8658ddd
No related branches found
No related tags found
1 merge request!272Problem notification lists API
Pipeline #2921 passed
......@@ -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();
......
......@@ -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)
......
......@@ -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();
......
......@@ -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;
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment