diff --git a/hello_plugin/hello_plugin.cpp b/hello_plugin/hello_plugin.cpp
index f15d005abd80bf110809135922228661c0ab2f6c..cc837fa4395dc660a4711817d4b71d4cd23687f7 100644
--- a/hello_plugin/hello_plugin.cpp
+++ b/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;
}
diff --git a/include/core/ceres_wrapper/solver_manager.h b/include/core/ceres_wrapper/solver_manager.h
deleted file mode 100644
index f7bad6f57e5d11173ed5a583b67d492d2d107439..0000000000000000000000000000000000000000
--- a/include/core/ceres_wrapper/solver_manager.h
+++ /dev/null
@@ -1,64 +0,0 @@
-#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
diff --git a/src/ceres_wrapper/solver_manager.cpp b/src/ceres_wrapper/solver_manager.cpp
deleted file mode 100644
index 586d76034d7001008ecfd0426f308c92b4f4c819..0000000000000000000000000000000000000000
--- a/src/ceres_wrapper/solver_manager.cpp
+++ /dev/null
@@ -1,93 +0,0 @@
-#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
-
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 9a9d1d36147dc655deda32e39b7dcb4eb9286cba..2f224b98496619e2c1d947d0b9dbce33b648d8db 100644
--- a/test/CMakeLists.txt
+++ b/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)
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index ee32a9b429a0aac16a8a211338ce56fde9b96708..8a2f6ed2ce0d5d89bb54d1d169c1f94920c335f6 100644
--- a/test/gtest_emplace.cpp
+++ b/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());
}
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 477dcb3d44e44755c2bee6564233217daefe3a02..5f44b20579a001f85075842e5bbc81fbec8ba9ae 100644
--- a/test/gtest_problem.cpp
+++ b/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);