Fixed gtest_emplace

hello_plugin/hello_plugin.cpp

@@ -26,157 +26,178 @@ using namespace wolf;
 using namespace Eigen;
 
 int main(int argc, char** argv) {
-    string file = "";
-    if(argc > 1) file = argv[1];
-    parserYAML parser = parserYAML(file);
-    parser.parse();
-    paramsServer server = paramsServer(parser.getParams(), parser.sensorsSerialization(), parser.processorsSerialization());
-    cout << "PRINTING SERVER MAP" << endl;
-    server.print();
-    cout << "-----------------------------------" << endl;
-    /**
-       It seems to be a requirement that each file is loaded by its own ClassLoader object, otherwise I get
-       a segmentation fault. Likewise, it seems that these ClassLoaders must be allocated at the heap, because
-       the constructor refuses to build an object if I try to do local (stack) allocation, i.e `ClassLoader(it)` is not allowed but `new ClassLoader(it)` is.
-     **/
-    // vector<ClassLoader*> class_loaders = vector<ClassLoader*>();
-    // for(auto it : parser.getFiles()) {
-    //     auto c = new ClassLoader(it);
-    //     class_loaders.push_back(c);
-    // }
-    auto loaders = vector<Loader*>();
-    for(auto it : parser.getFiles()) {
-        auto l = new LoaderRaw(it);
-        loaders.push_back(l);
-    }
-    ProblemPtr problem = Problem::create("PO", 2);
-    auto sensorMap = map<string, SensorBasePtr>();
-    auto procesorMap = map<string, ProcessorBasePtr>();
-    for(auto s : server.getSensors()){
-        cout << s._name << " " << s._type << endl;
-        sensorMap.insert(pair<string, SensorBasePtr>(s._name,problem->installSensor(s._type, s._name, server)));
-    }
-    for(auto s : server.getProcessors()){
-        cout << s._name << " " << s._type << " " << s._name_assoc_sensor << endl;
-        procesorMap.insert(pair<string, ProcessorBasePtr>(s._name,problem->installProcessor(s._type, s._name, s._name_assoc_sensor, server)));
-    }
-
-    problem->print(4,1,1,1);
-    Vector2s motion_data(1.0, 0.0);                     // Will advance 1m at each keyframe, will not turn.
-    Matrix2s motion_cov = 0.1 * Matrix2s::Identity();
-
-    // landmark observations data
-    VectorXi ids;
-    VectorXs ranges, bearings;
-
-
-    // SET OF EVENTS =======================================================
-    std::cout << std::endl;
-    WOLF_TRACE("======== BUILD PROBLEM =======");
-
-    // ceres::Solver::Options options;
-    // options.max_num_iterations              = 1000; // We depart far from solution, need a lot of iterations
-    // CeresManagerPtr ceres                   = std::make_shared<CeresManager>(problem, options);
-    auto ceres = SolverFactory::get().create("Solver", problem, server);
-    // We'll do 3 steps of motion and landmark observations.
-
-    // STEP 1 --------------------------------------------------------------
-
-    // initialize
-    TimeStamp   t(0.0);                     // t : 0.0
-    Vector3s    x(0,0,0);
-    Matrix3s    P = Matrix3s::Identity() * 0.1;
-    problem->setPrior(x, P, t, 0.5);             // KF1 : (0,0,0)
-    auto sensor_rb = sensorMap.find("rb")->second;
-    // observe lmks
-    ids.resize(1); ranges.resize(1); bearings.resize(1);
-    ids         << 1;                       // will observe Lmk 1
-    ranges      << 1.0;                     // see drawing
-    bearings    << M_PI/2;
-    CaptureRangeBearingPtr cap_rb = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
-    sensor_rb   ->process(cap_rb);          // L1 : (1,2)
-
-    // STEP 2 --------------------------------------------------------------
-    t += 1.0;                     // t : 1.0
-
-    // motion
-    auto sensor_odometry = sensorMap.find("odom")->second;
-    CaptureOdom2DPtr cap_motion = std::make_shared<CaptureOdom2D>(t, sensor_odometry, motion_data, motion_cov);
-    sensor_odometry ->process(cap_motion);      // KF2 : (1,0,0)
-
-    // observe lmks
-    ids.resize(2); ranges.resize(2); bearings.resize(2);
-    ids         << 1, 2;                    // will observe Lmks 1 and 2
-    ranges      << sqrt(2.0), 1.0;          // see drawing
-    bearings    << 3*M_PI/4, M_PI/2;
-    cap_rb      = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
-    sensor_rb   ->process(cap_rb);          // L1 : (1,2), L2 : (2,2)
-
-    // STEP 3 --------------------------------------------------------------
-    t += 1.0;                     // t : 2.0
-
-    // motion
-    cap_motion  ->setTimeStamp(t);
-    sensor_odometry  ->process(cap_motion);      // KF3 : (2,0,0)
-    // observe lmks
-    ids.resize(2); ranges.resize(2); bearings.resize(2);
-    ids         << 2, 3;                    // will observe Lmks 2 and 3
-    ranges      << sqrt(2.0), 1.0;          // see drawing
-    bearings    << 3*M_PI/4, M_PI/2;
-    cap_rb      = std::make_shared<CaptureRangeBearing>(t, sensor_rb, ids, ranges, bearings);
-    sensor_rb   ->process(cap_rb);          // L1 : (1,2), L2 : (2,2), L3 : (3,2)
-    problem->print(1,0,1,0);
-
-
-    // SOLVE ================================================================
-
-    // SOLVE with exact initial guess
-    WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
-    std::string report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
-    WOLF_TRACE(report);                     // should show a very low iteration number (possibly 1)
-    problem->print(1,0,1,0);
-
-    // PERTURB initial guess
-    WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
-    for (auto sen : problem->getHardware()->getSensorList())
-        for (auto sb : sen->getStateBlockVec())
-            if (sb && !sb->isFixed())
-                sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5);       // We perturb A LOT !
-    for (auto kf : problem->getTrajectory()->getFrameList())
-        if (kf->isKey())
-            for (auto sb : kf->getStateBlockVec())
-                if (sb && !sb->isFixed())
-                    sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5);   // We perturb A LOT !
-    for (auto lmk : problem->getMap()->getLandmarkList())
-        for (auto sb : lmk->getStateBlockVec())
-            if (sb && !sb->isFixed())
-                sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5);       // We perturb A LOT !
-    problem->print(1,0,1,0);
-
-    // SOLVE again
-    WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
-    report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
-    WOLF_TRACE(report);                     // should show a very high iteration number (more than 10, or than 100!)
-    problem->print(1,0,1,0);
-
-    // GET COVARIANCES of all states
-    WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
-    ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
-    for (auto kf : problem->getTrajectory()->getFrameList()){
-        if (kf->isKey())
-            {
-                Eigen::MatrixXs cov;
-                WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance(cov));
-            }
-        for (auto lmk : problem->getMap()->getLandmarkList()) {
-            Eigen::MatrixXs cov;
-            WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance(cov));
-        }
-    }
-    std::cout << std::endl;
-
-    WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
-    problem->print(4,1,1,1);
-
-    return 0;
+	string file = "";
+	if (argc > 1)
+		file = argv[1];
+	parserYAML parser = parserYAML(file);
+	parser.parse();
+	paramsServer server = paramsServer(parser.getParams(),
+			parser.sensorsSerialization(), parser.processorsSerialization());
+	cout << "PRINTING SERVER MAP" << endl;
+	server.print();
+	cout << "-----------------------------------" << endl;
+	/**
+	 It seems to be a requirement that each file is loaded by its own ClassLoader object, otherwise I get
+	 a segmentation fault. Likewise, it seems that these ClassLoaders must be allocated at the heap, because
+	 the constructor refuses to build an object if I try to do local (stack) allocation, i.e `ClassLoader(it)` is not allowed but `new ClassLoader(it)` is.
+	 **/
+	// vector<ClassLoader*> class_loaders = vector<ClassLoader*>();
+	// for(auto it : parser.getFiles()) {
+	//     auto c = new ClassLoader(it);
+	//     class_loaders.push_back(c);
+	// }
+	auto loaders = vector<Loader*>();
+	for (auto it : parser.getFiles()) {
+		auto l = new LoaderRaw(it);
+		loaders.push_back(l);
+	}
+	ProblemPtr problem = Problem::create("PO", 2);
+	auto sensorMap = map<string, SensorBasePtr>();
+	auto procesorMap = map<string, ProcessorBasePtr>();
+	for (auto s : server.getSensors()) {
+		cout << s._name << " " << s._type << endl;
+		sensorMap.insert(
+				pair<string, SensorBasePtr>(s._name,
+						problem->installSensor(s._type, s._name, server)));
+	}
+	for (auto s : server.getProcessors()) {
+		cout << s._name << " " << s._type << " " << s._name_assoc_sensor
+				<< endl;
+		procesorMap.insert(
+				pair<string, ProcessorBasePtr>(s._name,
+						problem->installProcessor(s._type, s._name,
+								s._name_assoc_sensor, server)));
+	}
+
+	problem->print(4, 1, 1, 1);
+	Vector2s motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
+	Matrix2s motion_cov = 0.1 * Matrix2s::Identity();
+
+	// landmark observations data
+	VectorXi ids;
+	VectorXs ranges, bearings;
+
+	// SET OF EVENTS =======================================================
+	std::cout << std::endl;
+	WOLF_TRACE("======== BUILD PROBLEM =======");
+
+	// ceres::Solver::Options options;
+	// options.max_num_iterations              = 1000; // We depart far from solution, need a lot of iterations
+	// CeresManagerPtr ceres                   = std::make_shared<CeresManager>(problem, options);
+	auto ceres = SolverFactory::get().create("Solver", problem, server);
+	// We'll do 3 steps of motion and landmark observations.
+
+	// STEP 1 --------------------------------------------------------------
+
+	// initialize
+	TimeStamp t(0.0);                     // t : 0.0
+	Vector3s x(0, 0, 0);
+	Matrix3s P = Matrix3s::Identity() * 0.1;
+	problem->setPrior(x, P, t, 0.5);             // KF1 : (0,0,0)
+	auto sensor_rb = sensorMap.find("rb")->second;
+	// observe lmks
+	ids.resize(1);
+	ranges.resize(1);
+	bearings.resize(1);
+	ids << 1;                       // will observe Lmk 1
+	ranges << 1.0;                     // see drawing
+	bearings << M_PI / 2;
+	CaptureRangeBearingPtr cap_rb = std::make_shared < CaptureRangeBearing
+			> (t, sensor_rb, ids, ranges, bearings);
+	sensor_rb->process(cap_rb);          // L1 : (1,2)
+
+	// STEP 2 --------------------------------------------------------------
+	t += 1.0;                     // t : 1.0
+
+	// motion
+	auto sensor_odometry = sensorMap.find("odom")->second;
+	CaptureOdom2DPtr cap_motion = std::make_shared < CaptureOdom2D
+			> (t, sensor_odometry, motion_data, motion_cov);
+	sensor_odometry->process(cap_motion);      // KF2 : (1,0,0)
+
+	// observe lmks
+	ids.resize(2);
+	ranges.resize(2);
+	bearings.resize(2);
+	ids << 1, 2;                    // will observe Lmks 1 and 2
+	ranges << sqrt(2.0), 1.0;          // see drawing
+	bearings << 3 * M_PI / 4, M_PI / 2;
+	cap_rb = std::make_shared < CaptureRangeBearing
+			> (t, sensor_rb, ids, ranges, bearings);
+	sensor_rb->process(cap_rb);          // L1 : (1,2), L2 : (2,2)
+
+	// STEP 3 --------------------------------------------------------------
+	t += 1.0;                     // t : 2.0
+
+	// motion
+	cap_motion->setTimeStamp(t);
+	sensor_odometry->process(cap_motion);      // KF3 : (2,0,0)
+	// observe lmks
+	ids.resize(2);
+	ranges.resize(2);
+	bearings.resize(2);
+	ids << 2, 3;                    // will observe Lmks 2 and 3
+	ranges << sqrt(2.0), 1.0;          // see drawing
+	bearings << 3 * M_PI / 4, M_PI / 2;
+	cap_rb = std::make_shared < CaptureRangeBearing
+			> (t, sensor_rb, ids, ranges, bearings);
+	sensor_rb->process(cap_rb);          // L1 : (1,2), L2 : (2,2), L3 : (3,2)
+	problem->print(1, 0, 1, 0);
+
+	// SOLVE ================================================================
+
+	// SOLVE with exact initial guess
+	WOLF_TRACE("======== SOLVE PROBLEM WITH EXACT PRIORS =======")
+	std::string report = ceres->solve(
+			wolf::SolverManager::ReportVerbosity::FULL);
+	WOLF_TRACE(report);  // should show a very low iteration number (possibly 1)
+	problem->print(1, 0, 1, 0);
+
+	// PERTURB initial guess
+	WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
+	for (auto sen : problem->getHardware()->getSensorList())
+		for (auto sb : sen->getStateBlockVec())
+			if (sb && !sb->isFixed())
+				sb->setState(
+						sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+	for (auto kf : problem->getTrajectory()->getFrameList())
+		if (kf->isKey())
+			for (auto sb : kf->getStateBlockVec())
+				if (sb && !sb->isFixed())
+					sb->setState(
+							sb->getState()
+									+ VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+	for (auto lmk : problem->getMap()->getLandmarkList())
+		for (auto sb : lmk->getStateBlockVec())
+			if (sb && !sb->isFixed())
+				sb->setState(
+						sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+	problem->print(1, 0, 1, 0);
+
+	// SOLVE again
+	WOLF_TRACE("======== SOLVE PROBLEM WITH PERTURBED PRIORS =======")
+	report = ceres->solve(wolf::SolverManager::ReportVerbosity::FULL);
+	WOLF_TRACE(report); // should show a very high iteration number (more than 10, or than 100!)
+	problem->print(1, 0, 1, 0);
+
+	// GET COVARIANCES of all states
+	WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
+	ceres->computeCovariances(
+			SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
+	for (auto kf : problem->getTrajectory()->getFrameList()) {
+		if (kf->isKey()) {
+			Eigen::MatrixXs cov;
+			WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance(cov));
+		}
+		for (auto lmk : problem->getMap()->getLandmarkList()) {
+			Eigen::MatrixXs cov;
+			WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance(cov));
+		}
+	}
+	std::cout << std::endl;
+
+	WOLF_TRACE("======== FINAL PRINT FOR INTERPRETATION =======")
+	problem->print(4, 1, 1, 1);
+
+	return 0;
 }

include/core/ceres_wrapper/solver_manager.h

-#ifndef SOLVER_MANAGER_H_
-#define SOLVER_MANAGER_H_
-
-//wolf includes
-#include "core/common/wolf.h"
-#include "core/state_block/state_block.h"
-#include "core/factor/factor_base.h"
-
-namespace wolf {
-
-/** \brief Enumeration of covariance blocks to be computed
- *
- * Enumeration of covariance blocks to be computed
- *
- */
-typedef enum
-{
-    ALL, ///< All blocks and all cross-covariances
-    ALL_MARGINALS, ///< All marginals
-    ROBOT_LANDMARKS ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks
-} CovarianceBlocksToBeComputed;
-
-WOLF_PTR_TYPEDEFS(SolverManager);
-
-/** \brief Solver manager for WOLF
- *
- */
-
-class SolverManager
-{
-	protected:
-		ProblemPtr wolf_problem_;
-
-	public:
-        SolverManager(ProblemPtr _wolf_problem);
-
-		virtual ~SolverManager();
-
-		virtual std::string solve(const unsigned int& _report_level) = 0;
-
-		virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS) = 0;
-
-		virtual void computeCovariances(const StateBlockPtrList& st_list) = 0;
-
-		virtual void update();
-
-        virtual ProblemPtr getProblem();
-
-	private:
-
-		virtual void addFactor(FactorBasePtr _fac_ptr) = 0;
-
-		virtual void removeFactor(FactorBasePtr _fac_ptr) = 0;
-
-		virtual void addStateBlock(StateBlockPtr _st_ptr) = 0;
-
-		virtual void removeStateBlock(StateBlockPtr _st_ptr) = 0;
-
-		virtual void updateStateBlockStatus(StateBlockPtr _st_ptr) = 0;
-};
-
-} // namespace wolf
-
-#endif

src/ceres_wrapper/solver_manager.cpp

-#include "core/solver/solver_manager.h"
-#include "core/trajectory/trajectory_base.h"
-#include "core/map/map_base.h"
-#include "core/landmark/landmark_base.h"
-
-namespace wolf {
-
-SolverManager::SolverManager(ProblemPtr _wolf_problem) :
-    wolf_problem_(_wolf_problem)
-{
-}
-
-SolverManager::~SolverManager()
-{
-}
-
-void SolverManager::update()
-{
-    //std::cout << "SolverManager: updating... " << std::endl;
-    //std::cout << wolf_problem_->getStateBlockNotificationList().size() << " state block notifications" << std::endl;
-    //std::cout << wolf_problem_->getFactorNotificationList().size() << " factor notifications" << std::endl;
-
-	// REMOVE CONSTRAINTS
-	auto fac_notification_it = wolf_problem_->getFactorNotificationList().begin();
-	while ( fac_notification_it != wolf_problem_->getFactorNotificationList().end() )
-		if (fac_notification_it->notification_ == REMOVE)
-		{
-			removeFactor(fac_notification_it->factor_ptr_);
-			fac_notification_it = wolf_problem_->getFactorNotificationList().erase(fac_notification_it);
-		}
-		else
-			fac_notification_it++;
-
-	// REMOVE STATE BLOCKS
-	auto state_notification_it = wolf_problem_->getStateBlockNotificationList().begin();
-	while ( state_notification_it != wolf_problem_->getStateBlockNotificationList().end() )
-		if (state_notification_it->notification_ == REMOVE)
-		{
-			removeStateBlock(state_notification_it->state_block_ptr_);
-			state_notification_it = wolf_problem_->getStateBlockNotificationList().erase(state_notification_it);
-		}
-		else
-			state_notification_it++;
-
-    // ADD/UPDATE STATE BLOCKS
-    while (!wolf_problem_->getStateBlockNotificationList().empty())
-    {
-        switch (wolf_problem_->getStateBlockNotificationList().front().notification_)
-        {
-            case ADD:
-            {
-                addStateBlock(wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_);
-                if (wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_->isFixed())
-                    updateStateBlockStatus(wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_);
-                break;
-            }
-            case UPDATE:
-            {
-                updateStateBlockStatus(wolf_problem_->getStateBlockNotificationList().front().state_block_ptr_);
-                break;
-            }
-            default:
-                throw std::runtime_error("SolverManager::update: State Block notification must be ADD, UPATE or REMOVE.");
-        }
-        wolf_problem_->getStateBlockNotificationList().pop_front();
-    }
-    // ADD CONSTRAINTS
-    while (!wolf_problem_->getFactorNotificationList().empty())
-    {
-        switch (wolf_problem_->getFactorNotificationList().front().notification_)
-        {
-            case ADD:
-            {
-                addFactor(wolf_problem_->getFactorNotificationList().front().factor_ptr_);
-                break;
-            }
-            default:
-                throw std::runtime_error("SolverManager::update: Factor notification must be ADD or REMOVE.");
-        }
-        wolf_problem_->getFactorNotificationList().pop_front();
-    }
-
-    assert(wolf_problem_->getFactorNotificationList().empty() && "wolf problem's factors notification list not empty after update");
-    assert(wolf_problem_->getStateBlockNotificationList().empty() && "wolf problem's state_blocks notification list not empty after update");
-}
-
-ProblemPtr SolverManager::getProblem()
-{
-    return wolf_problem_;
-}
-
-} // namespace wolf
-

test/CMakeLists.txt

@@ -62,8 +62,8 @@ target_link_libraries(gtest_eigen_predicates ${PROJECT_NAME})
 
 # Node Emplace test
 # TODO: TO BE FIXED
-# wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
-# target_link_libraries(gtest_emplace ${PROJECT_NAME})
+wolf_add_gtest(gtest_emplace gtest_emplace.cpp)
+target_link_libraries(gtest_emplace ${PROJECT_NAME})
 
 # FeatureBase classes test
 wolf_add_gtest(gtest_feature_base gtest_feature_base.cpp)

test/gtest_emplace.cpp

@@ -9,14 +9,12 @@
 #include "core/utils/logging.h"
 
 #include "core/problem/problem.h"
-#include "core/capture/capture_IMU.h"
 #include "core/sensor/sensor_base.h"
+#include "core/sensor/sensor_odom_2D.h"
 #include "core/sensor/sensor_odom_3D.h"
-#include "core/sensor/sensor_IMU.h"
 #include "core/processor/processor_odom_3D.h"
 #include "core/processor/processor_odom_2D.h"
 #include "core/feature/feature_odom_2D.h"
-#include "core/feature/feature_IMU.h"
 #include "core/processor/processor_tracker_feature_dummy.h"
 
 #include <iostream>
@@ -122,20 +120,18 @@ TEST(Emplace, EmplaceDerived)
 
     auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(),  KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
     // LandmarkBase::emplace<LandmarkBase>(MapBaseWPtr(P->getMap()),"Dummy", nullptr, nullptr);
-    auto sen = SensorBase::emplace<SensorIMU>(P->getHardware(), Eigen::VectorXs(7), IntrinsicsIMU());
+    auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXs(3), IntrinsicsOdom2D());
     auto cov = Eigen::MatrixXs(2,2);
     cov(0,0) = 1;
     cov(1,1) = 1;
-    auto cpt = CaptureBase::emplace<CaptureIMU>(frm, TimeStamp(0), sen, Eigen::Vector6s(), cov,
-                                                Eigen::Vector6s(), frm);
-    auto cpt2 = std::static_pointer_cast<CaptureIMU>(cpt);
+    auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::Vector6s(), cov, frm);
+    auto cpt2 = std::static_pointer_cast<CaptureOdom2D>(cpt);
     auto m = Eigen::Matrix<Scalar,9,6>();
     for(int i = 0; i < 9; i++)
         for(int j = 0; j < 6; j++)
             m(i,j) = 1;
 
-    auto ftr = FeatureBase::emplace<FeatureIMU>(cpt, Eigen::VectorXs(5), cov,
-                                                Eigen::Vector6s(), m, cpt2);
+    auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(5), cov);
     ASSERT_EQ(sen, P->getHardware()->getSensorList().front());
     ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
 }

test/gtest_problem.cpp

@@ -24,7 +24,7 @@ using namespace Eigen;
 
 WOLF_PTR_TYPEDEFS(DummySolverManager);
 
-struct DummySolverManager : public SolverManager
+class DummySolverManager : public SolverManager
 {
   DummySolverManager(const ProblemPtr& _problem)
     : SolverManager(_problem)
@@ -239,11 +239,11 @@ TEST(Problem, StateBlocks)
 
     // 2 state blocks, fixed
     SensorBasePtr    Sm = P->installSensor   ("ODOM 3D", "odometer",xs3d, wolf_root + "/src/examples/sensor_odom_3D.yaml");
-    ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int) 2);
+    ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
 
     // 2 state blocks, fixed
     SensorBasePtr    St = P->installSensor   ("ODOM 2D", "other odometer", xs2d, "");
-    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*/)); // consume empties the notification map, so only should contain notification since last call
 
     ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
     params->time_tolerance            = 0.1;
@@ -255,16 +255,23 @@ 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, xs3d, 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", 3, KEY, xs3d, 0);
+    ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(/*2 + 3*/ + 2)); // consume empties the notification map, so only should contain notification since last call
+
+    // 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->getStateBlockPtrList().size(), (unsigned int)(2 + 2 + 2));
-    // ASSERT_EQ(P->getStateBlockNotificationMap().size(),(unsigned int)(2 + 2 + 2 /*+ 2*/)); // XXX: 2 more notifications on the same SB!
+    ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd)(0)); // consume empties the notification map, so only should contain notification since last call
+    // 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->getStateBlockPtrList().size(), (SizeStd)(2 + 2 + 2));
+    // ASSERT_EQ(P->getStateBlockNotificationMap().size(),(SizeStd)(2 + 2 + 2 /*+ 2*/)); // XXX: 2 more notifications on the same SB!
 
     //    P->print(4,1,1,1);