diff --git a/include/base/ceres_wrapper/ceres_manager.h b/include/base/ceres_wrapper/ceres_manager.h
index c15355fbd0c2cdaae9ea1c39c7032bb311b3b3e9..a20d684d8a5301c325638d90dba4039935ca87de 100644
--- a/include/base/ceres_wrapper/ceres_manager.h
+++ b/include/base/ceres_wrapper/ceres_manager.h
@@ -9,7 +9,7 @@
//wolf includes
#include "base/solver/solver_manager.h"
#include "base/ceres_wrapper/cost_function_wrapper.h"
-#include "local_parametrization_wrapper.h"
+#include "base/ceres_wrapper/local_parametrization_wrapper.h"
#include "base/ceres_wrapper/create_numeric_diff_cost_function.h"
namespace ceres {
@@ -50,6 +50,11 @@ class CeresManager : public SolverManager
ceres::Solver::Summary getSummary();
+ std::unique_ptr<ceres::Problem>& getCeresProblem()
+ {
+ return ceres_problem_;
+ }
+
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
= CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
diff --git a/include/base/problem/problem.h b/include/base/problem/problem.h
index bfae71978ca916f52616d8f3af7c88160ca056c4..85838819bfc2febcefde4d6fc0867e445bea8256 100644
--- a/include/base/problem/problem.h
+++ b/include/base/problem/problem.h
@@ -22,6 +22,7 @@ struct ProcessorParamsBase;
#include "base/state_block/state_block.h"
// std includes
+#include <mutex>
namespace wolf {
@@ -41,11 +42,13 @@ class Problem : public std::enable_shared_from_this<Problem>
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
ProcessorMotionPtr processor_motion_ptr_;
- StateBlockPtrList state_block_list_;
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
std::map<StateBlockPtr, Notification> state_block_notification_map_;
+ mutable std::mutex mut_factor_notifications_;
+ mutable std::mutex mut_state_block_notifications_;
+ mutable std::mutex mut_covariances_;
bool prior_is_set_;
private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !!
@@ -257,10 +260,6 @@ class Problem : public std::enable_shared_from_this<Problem>
// Solver management ----------------------------------------
- /** \brief Gets a reference to the state blocks list
- */
- StateBlockPtrList& getStateBlockPtrList();
-
/** \brief Notifies a new state block to be added to the solver manager
*/
StateBlockPtr addStateBlock(StateBlockPtr _state_ptr);
@@ -269,9 +268,9 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
void removeStateBlock(StateBlockPtr _state_ptr);
- /** \brief Gets a map of factor notification to be handled by the solver
+ /** \brief Returns the map of factor notification to be handled by the solver (the map stored in this is emptied)
*/
- std::map<StateBlockPtr,Notification>& getStateBlockNotificationMap();
+ std::map<StateBlockPtr,Notification> consumeStateBlockNotificationMap();
/** \brief Notifies a new factor to be added to the solver manager
*/
@@ -281,9 +280,9 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
void removeFactor(FactorBasePtr _factor_ptr);
- /** \brief Gets a map of factor notification to be handled by the solver
+ /** \brief Returns the map of factor notification to be handled by the solver (the map stored in this is emptied)
*/
- std::map<FactorBasePtr, Notification>& getFactorNotificationMap();
+ std::map<FactorBasePtr, Notification> consumeFactorNotificationMap();
// Print and check ---------------------------------------
/**
@@ -322,14 +321,16 @@ inline ProcessorMotionPtr& Problem::getProcessorMotion()
return processor_motion_ptr_;
}
-inline std::map<StateBlockPtr,Notification>& Problem::getStateBlockNotificationMap()
+inline std::map<StateBlockPtr,Notification> Problem::consumeStateBlockNotificationMap()
{
- return state_block_notification_map_;
+ std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
+ return std::move(state_block_notification_map_);
}
-inline std::map<FactorBasePtr,Notification>& Problem::getFactorNotificationMap()
+inline std::map<FactorBasePtr,Notification> Problem::consumeFactorNotificationMap()
{
- return factor_notification_map_;
+ std::lock_guard<std::mutex> lock(mut_factor_notifications_);
+ return std::move(factor_notification_map_);
}
} // namespace wolf
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 2b9b1bc6a52fe7556488000539fa5da2ff88bb84..3d97d7a1df857c7a950b762e4b8082eb99c0056c 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -267,6 +267,9 @@ void CeresManager::addFactor(const FactorBasePtr& fac_ptr)
auto loss_func_ptr = (fac_ptr->getApplyLossFunction()) ? new ceres::CauchyLoss(0.5) : nullptr;
+ //std::cout << "Added residual block corresponding to constraint " << std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->getType() << std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->id() <<std::endl;
+
+ assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
assert(fac_2_residual_idx_.find(fac_ptr) == fac_2_residual_idx_.end() && "adding a factor that is already in the fac_2_residual_idx_ map");
fac_2_residual_idx_[fac_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(),
@@ -304,11 +307,15 @@ void CeresManager::addStateBlock(const StateBlockPtr& state_ptr)
state_ptr->getSize(),
local_parametrization_ptr);
+ if (state_ptr->isFixed())
+ ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
+
updateStateBlockStatus(state_ptr);
}
void CeresManager::removeStateBlock(const StateBlockPtr& state_ptr)
{
+ //std::cout << "CeresManager::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
assert(state_ptr);
ceres_problem_->RemoveParameterBlock(getAssociatedMemBlockPtr(state_ptr));
state_blocks_local_param_.erase(state_ptr);
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 0d9f055b259a1558822ded587872299feeb93a1a..8e607b0d4e7891e7978d4b2b127a8eabbe367cac 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -425,15 +425,8 @@ void Problem::addLandmarkList(LandmarkBasePtrList& _lmk_list)
StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
{
+ std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
//std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;
- if(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) != state_block_list_.end())
- {
- WOLF_WARN("Adding a state block that has already been added");
- return _state_ptr;
- }
-
- // add the state unit to the list
- state_block_list_.push_back(_state_ptr);
// Add add notification
// Check if there is already a notification for this state block
@@ -450,16 +443,8 @@ StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
void Problem::removeStateBlock(StateBlockPtr _state_ptr)
{
+ std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
//std::cout << "Problem::removeStateBlockPtr " << _state_ptr.get() << std::endl;
- //assert(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) != state_block_list_.end() && "Removing a state_block that hasn't been added or already removed");
- if(std::find(state_block_list_.begin(),state_block_list_.end(),_state_ptr) == state_block_list_.end())
- {
- WOLF_WARN("Removing a state_block that hasn't been added or already removed");
- return;
- }
-
- // add the state unit to the list
- state_block_list_.remove(_state_ptr);
// Check if there is already a notification for this state block
auto notification_it = state_block_notification_map_.find(_state_ptr);
@@ -482,6 +467,7 @@ void Problem::removeStateBlock(StateBlockPtr _state_ptr)
FactorBasePtr Problem::addFactor(FactorBasePtr _factor_ptr)
{
+ std::lock_guard<std::mutex> lock(mut_factor_notifications_);
//std::cout << "Problem::addFactorPtr " << _factor_ptr->id() << std::endl;
// Add ADD notification
@@ -500,6 +486,7 @@ FactorBasePtr Problem::addFactor(FactorBasePtr _factor_ptr)
void Problem::removeFactor(FactorBasePtr _factor_ptr)
{
+ std::lock_guard<std::mutex> lock(mut_factor_notifications_);
//std::cout << "Problem::removeFactorPtr " << _factor_ptr->id() << std::endl;
// Check if there is already a notification for this state block
@@ -521,6 +508,7 @@ void Problem::removeFactor(FactorBasePtr _factor_ptr)
void Problem::clearCovariance()
{
+ std::lock_guard<std::mutex> lock(mut_covariances_);
covariances_.clear();
}
@@ -529,6 +517,7 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, c
assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
assert(_state2->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
+ std::lock_guard<std::mutex> lock(mut_covariances_);
covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] = _cov;
}
@@ -537,6 +526,7 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _
assert(_state1->getSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
assert(_state1->getSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
+ std::lock_guard<std::mutex> lock(mut_covariances_);
covariances_[std::make_pair(_state1, _state1)] = _cov;
}
@@ -556,6 +546,8 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
assert(_row + _state1->getSize() <= _cov.rows() && _col + _state2->getSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
+ std::lock_guard<std::mutex> lock(mut_covariances_);
+
if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
_cov.block(_row, _col, _state1->getSize(), _state2->getSize()) =
covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)];
@@ -573,6 +565,8 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov)
{
+ std::lock_guard<std::mutex> lock(mut_covariances_);
+
// fill covariance
for (auto it1 = _sb_2_idx.begin(); it1 != _sb_2_idx.end(); it1++)
for (auto it2 = it1; it2 != _sb_2_idx.end(); it2++)
@@ -719,11 +713,6 @@ FrameBasePtr Problem::getLastKeyFrame()
return trajectory_ptr_->getLastKeyFrame();
}
-StateBlockPtrList& Problem::getStateBlockPtrList()
-{
- return state_block_list_;
-}
-
FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov, const TimeStamp& _ts, const Scalar _time_tolerance)
{
if ( ! prior_is_set_ )
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index adc0dabd9d9ae889b95ba2b2aaf33b7237094d3a..2a914ef672f7b31463d8f5309e147c52016101b6 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -13,26 +13,30 @@ SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
void SolverManager::update()
{
+ // Consume notification maps
+ auto fac_notification_map = wolf_problem_->consumeFactorNotificationMap();
+ auto sb_notification_map = wolf_problem_->consumeStateBlockNotificationMap();
+
// REMOVE CONSTRAINTS
- auto fac_notification_it = wolf_problem_->getFactorNotificationMap().begin();
- while ( fac_notification_it != wolf_problem_->getFactorNotificationMap().end() )
+ for (auto fac_notification_it = fac_notification_map.begin();
+ fac_notification_it != fac_notification_map.end();
+ /* nothing, next is handled within the for */)
{
if (fac_notification_it->second == REMOVE)
{
removeFactor(fac_notification_it->first);
- fac_notification_it = wolf_problem_->getFactorNotificationMap().erase(fac_notification_it);
+ fac_notification_it = fac_notification_map.erase(fac_notification_it);
}
else
fac_notification_it++;
}
// ADD/REMOVE STATE BLOCS
- auto sb_notification_it = wolf_problem_->getStateBlockNotificationMap().begin();
- while ( sb_notification_it != wolf_problem_->getStateBlockNotificationMap().end() )
+ while ( !sb_notification_map.empty() )
{
- StateBlockPtr state_ptr = sb_notification_it->first;
+ StateBlockPtr state_ptr = sb_notification_map.begin()->first;
- if (sb_notification_it->second == ADD)
+ if (sb_notification_map.begin()->second == ADD)
{
// only add if not added
if (state_blocks_.find(state_ptr) == state_blocks_.end())
@@ -46,7 +50,7 @@ void SolverManager::update()
}
else
{
- WOLF_DEBUG("Tried adding an already registered StateBlock.");
+ WOLF_DEBUG("Tried to add an already added !");
}
}
else
@@ -63,26 +67,24 @@ void SolverManager::update()
}
}
// next notification
- sb_notification_it = wolf_problem_->getStateBlockNotificationMap().erase(sb_notification_it);
+ sb_notification_map.erase(sb_notification_map.begin());
}
// ADD CONSTRAINTS
- fac_notification_it = wolf_problem_->getFactorNotificationMap().begin();
- while (fac_notification_it != wolf_problem_->getFactorNotificationMap().end())
+ while (!fac_notification_map.empty())
{
- assert(wolf_problem_->getFactorNotificationMap().begin()->second == ADD && "unexpected factor notification value after all REMOVE have been processed, this should be ADD");
+ assert(fac_notification_map.begin()->second == ADD && "unexpected factor notification value after all REMOVE have been processed, this should be ADD");
- addFactor(wolf_problem_->getFactorNotificationMap().begin()->first);
- fac_notification_it = wolf_problem_->getFactorNotificationMap().erase(fac_notification_it);
+ // add factor
+ addFactor(fac_notification_map.begin()->first);
+ // remove notification
+ fac_notification_map.erase(fac_notification_map.begin());
}
// UPDATE STATE BLOCKS (state, fix or local parameterization)
- for (auto state_ptr : wolf_problem_->getStateBlockPtrList())
+ for (auto state_pair : state_blocks_)
{
- if (state_blocks_.find(state_ptr)==state_blocks_.end())
- continue;
-
- assert(state_blocks_.find(state_ptr)!=state_blocks_.end() && "Updating the state of an unregistered StateBlock !");
+ auto state_ptr = state_pair.first;
// state update
if (state_ptr->stateUpdated())
@@ -108,9 +110,6 @@ void SolverManager::update()
state_ptr->resetLocalParamUpdated();
}
}
-
- //assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update");
- //assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
}
wolf::ProblemPtr SolverManager::getProblem()
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 78492ae19bdc78f500f50830ea093bcc4f16c35f..9b096f7c82d24aa1b2b939919f46a5190f8a65c0 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -398,12 +398,10 @@ TEST(CeresManager, 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)));
- ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c);
-
// remove factor
P->removeFactor(c);
- ASSERT_TRUE(P->getFactorNotificationMap().empty());
+ ASSERT_TRUE(P->consumeFactorNotificationMap().empty()); // add+remove = empty
// update solver
ceres_manager_ptr->update();
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 2c5102882ac6ee6c9bd25aeb46c847df56b7162a..213e29b9e8acb4c4af36da0c6365d1a538aa2d69 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -207,13 +207,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->getStateBlockPtrList().size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) 2);
+ ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int) 2);
// 3 state blocks, fixed
SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
- ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) (2 + 3));
- ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) (2 + 3));
+ ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int) (/*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;
@@ -226,15 +224,13 @@ TEST(Problem, StateBlocks)
// 2 state blocks, estimated
P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
- ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2));
- ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 3 + 2));
+ ASSERT_EQ(P->consumeStateBlockNotificationMap().size(), (unsigned int)(/*2 + 3*/ + 2)); // consume empties the notification map, so only should contain notification since last call
// P->print(4,1,1,1);
// change some SB properties
St->unfixExtrinsics();
- ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2));
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),(unsigned int)(2 + 3 + 2 /*+ 2*/)); // XXX: 2 more notifications on the same SB!
+ ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // changes in state_blocks status (fix/state/localparam) does not raise a notification in problem, only ADD/REMOVE
// P->print(4,1,1,1);
}
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index be68ea5ec11cbad46c3154c874ce9bec29d97c33..9de5032b38af32cd564429fae2d0833c60817ef6 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -445,13 +445,14 @@ TEST(SolverManager, AddUpdatedStateBlock)
ASSERT_TRUE(sb_ptr->fixUpdated());
ASSERT_TRUE(sb_ptr->stateUpdated());
- // == When an ADD is notified, all previous notifications should be cleared (if not consumption of notifs) ==
+ // == When an ADD is notified: a ADD notification should be stored ==
// add state_block
P->addStateBlock(sb_ptr);
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),1);
- ASSERT_EQ(P->getStateBlockNotificationMap().begin()->second,ADD);
+ auto state_block_notification_map = P->consumeStateBlockNotificationMap();
+ ASSERT_EQ(state_block_notification_map.size(),1);
+ ASSERT_EQ(state_block_notification_map.begin()->second,ADD);
// == Insert OTHER notifications ==
@@ -462,37 +463,30 @@ TEST(SolverManager, AddUpdatedStateBlock)
// Fix --> FLAG
sb_ptr->unfix();
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),1); // only ADD notification (fix and state are flags in sb)
+ ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // No new notifications (fix and set state are flags in sb)
- // == REMOVE should clear the previous notification (ADD) in the stack ==
+ // == When an REMOVE is notified: a REMOVE notification should be stored ==
// remove state_block
P->removeStateBlock(sb_ptr);
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),0);// ADD + REMOVE = EMPTY
+ state_block_notification_map = P->consumeStateBlockNotificationMap();
+ ASSERT_EQ(state_block_notification_map.size(),1);
+ ASSERT_EQ(state_block_notification_map.begin()->second,REMOVE);
- // == UPDATES + REMOVE should clear the list of notifications ==
+ // == ADD + REMOVE: notification map should be empty ==
+ P->addStateBlock(sb_ptr);
+ P->removeStateBlock(sb_ptr);
+ ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty());
+ // == UPDATES should clear the list of notifications ==
// add state_block
P->addStateBlock(sb_ptr);
// update solver
solver_manager_ptr->update();
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),0); // After solver_manager->update, notifications should be empty
-
- // Fix
- sb_ptr->fix();
-
- // Set State
- state_2 = 2*state;
- sb_ptr->setState(state_2);
-
- // remove state_block
- P->removeStateBlock(sb_ptr);
-
- ASSERT_EQ(P->getStateBlockNotificationMap().size(),1);
- ASSERT_EQ(P->getStateBlockNotificationMap().begin()->second, REMOVE);
+ ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // After solver_manager->update, notifications should be empty
}
TEST(SolverManager, AddFactor)
@@ -560,12 +554,10 @@ 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)));
- ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c);
-
// add factor
P->removeFactor(c);
- ASSERT_TRUE(P->getFactorNotificationMap().empty());
+ ASSERT_TRUE(P->consumeFactorNotificationMap().empty()); // ADD+REMOVE = empty
// update solver
solver_manager_ptr->update();
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index e74519b58be66ea7efb68f805e3e3e15e022e0d7..de582e26b0e3fbfa6ca61876e82d3b9870ea0ff8 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -31,10 +31,12 @@ struct DummyNotificationProcessor
FAIL() << "problem_ is nullptr !";
}
- auto sb_noti_pair = problem_->getStateBlockNotificationMap().begin();
- while (sb_noti_pair != problem_->getStateBlockNotificationMap().end())
+ 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_pair->second)
+ switch (sb_noti_map.begin()->second)
{
case ADD:
{
@@ -47,9 +49,9 @@ struct DummyNotificationProcessor
default:
throw std::runtime_error("SolverManager::update: State Block notification must be ADD or REMOVE.");
}
- sb_noti_pair = problem_->getStateBlockNotificationMap().erase(sb_noti_pair);
+ sb_noti_map.erase(sb_noti_map.begin());
}
- ASSERT_TRUE(problem_->getStateBlockNotificationMap().empty());
+ ASSERT_TRUE(sb_noti_map.empty());
}
ProblemPtr problem_;
@@ -118,22 +120,18 @@ TEST(TrajectoryBase, Add_Remove_Frame)
T->addFrame(f1); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
+ ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 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(P->getStateBlockPtrList(). size(), (unsigned int) 4);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
std::cout << __LINE__ << std::endl;
- T->addFrame(f3); // F
+ 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->getStateBlockPtrList(). size(), (unsigned int) 4);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
+ ASSERT_EQ(P->consumeStateBlockNotificationMap(). size(), (unsigned int) 2); // consumeStateBlockNotificationMap empties the notification map, 2 state blocks were notified since last call to consumeStateBlockNotificationMap
std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastFrame()->id(), f3->id());
@@ -141,14 +139,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
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->getStateBlockPtrList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockNotificationMap(). 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
std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastFrame()->id(), f3->id());
@@ -158,8 +156,6 @@ 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(P->getStateBlockPtrList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
std::cout << __LINE__ << std::endl;
ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id());
@@ -167,13 +163,11 @@ TEST(TrajectoryBase, Add_Remove_Frame)
f1->remove(); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 0);
- ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 0);
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
std::cout << __LINE__ << std::endl;
N.update();
- ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 0);
+ ASSERT_TRUE(P->consumeStateBlockNotificationMap().empty()); // consumeStateBlockNotificationMap was called in update() so it should be empty
std::cout << __LINE__ << std::endl;
}