diff --git a/include/base/ceres_wrapper/ceres_manager.h b/include/base/ceres_wrapper/ceres_manager.h
index bb62a9d683beb5320c925a4793914acb54197bc1..2bdc1d4bd002deaa4e01edfac6817618fae40951 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 {
@@ -48,6 +48,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 8c88b0eaa2283044b2457c79649290625daaade4..938074f78020ebff96905714d70d7f1259a168e1 100644
--- a/include/base/problem/problem.h
+++ b/include/base/problem/problem.h
@@ -19,6 +19,7 @@ struct ProcessorParamsBase;
#include "base/state_block/state_block.h"
// std includes
+#include <mutex>
namespace wolf {
@@ -38,11 +39,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 !!
@@ -241,10 +244,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);
@@ -253,9 +252,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
*/
@@ -265,9 +264,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 ---------------------------------------
/**
@@ -306,14 +305,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 7168863762e4c7a602f84b451221c091348485ba..c8cebf9f4279c3e1801c93a2a7e2cccf14493ce5 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -258,6 +258,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(),
@@ -295,11 +298,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/examples/processor_imu.yaml b/src/examples/processor_imu.yaml
index bdb1708b91d759ab97425a0aa189df61105eef36..9d3d4fb807f1cecd9b3fe99ffe733a69622af4be 100644
--- a/src/examples/processor_imu.yaml
+++ b/src/examples/processor_imu.yaml
@@ -1,6 +1,5 @@
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
-time tolerance: 0.001
unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 2.0 # seconds
diff --git a/src/examples/processor_imu_no_vote.yaml b/src/examples/processor_imu_no_vote.yaml
index 019829ca64b35e2fa166177ecb5b82b33a079a04..4a1dd140e695cefc4590feb9213df76639cc7282 100644
--- a/src/examples/processor_imu_no_vote.yaml
+++ b/src/examples/processor_imu_no_vote.yaml
@@ -1,6 +1,5 @@
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
-time tolerance: 0.001
unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 999999.0 # seconds
diff --git a/src/examples/processor_imu_t1.yaml b/src/examples/processor_imu_t1.yaml
index ee626106b15b543ecfee17c110916a1cece28f02..e290b5ac495cffcd20ae7b8bd2ee4a809fc05c9e 100644
--- a/src/examples/processor_imu_t1.yaml
+++ b/src/examples/processor_imu_t1.yaml
@@ -1,6 +1,5 @@
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
-time tolerance: 0.001
unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 0.9999 # seconds
diff --git a/src/examples/processor_imu_t6.yaml b/src/examples/processor_imu_t6.yaml
index e9dcfe3fa9c3855fa12f3b0ec7fba9d5deccad70..b864e9859a4df06048e61ac83ec48ce18fbcf96c 100644
--- a/src/examples/processor_imu_t6.yaml
+++ b/src/examples/processor_imu_t6.yaml
@@ -1,6 +1,5 @@
processor type: "IMU" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
processor name: "Main imu" # This is ignored. The name provided to the SensorFactory prevails
-time tolerance: 0.001
unmeasured perturbation std: 0.00001
keyframe vote:
max time span: 5.9999 # seconds
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index b137c11b5870420cf98ffc041610a9f0edfbf023..3b476f4fd9e13390160adf993d19f900f8233f3c 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -361,15 +361,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
@@ -386,16 +379,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);
@@ -418,6 +403,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
@@ -436,6 +422,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
@@ -457,6 +444,7 @@ void Problem::removeFactor(FactorBasePtr _factor_ptr)
void Problem::clearCovariance()
{
+ std::lock_guard<std::mutex> lock(mut_covariances_);
covariances_.clear();
}
@@ -465,6 +453,7 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, c
assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
assert(_state2->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
+ std::lock_guard<std::mutex> lock(mut_covariances_);
covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] = _cov;
}
@@ -473,6 +462,7 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _
assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
assert(_state1->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
+ std::lock_guard<std::mutex> lock(mut_covariances_);
covariances_[std::make_pair(_state1, _state1)] = _cov;
}
@@ -492,6 +482,8 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
assert(_row + _state1->getLocalSize() <= _cov.rows() && _col + _state2->getLocalSize() <= _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->getLocalSize(), _state2->getLocalSize()) =
covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)];
@@ -509,6 +501,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++)
@@ -655,11 +649,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/src/yaml/processor_IMU_yaml.cpp b/src/yaml/processor_IMU_yaml.cpp
index ee4125bc6cdc7f1cb7c7fd996434e06fd2368e6d..161e3960f31a877f8d0dbc9a1aa8da12e0cd7c46 100644
--- a/src/yaml/processor_IMU_yaml.cpp
+++ b/src/yaml/processor_IMU_yaml.cpp
@@ -32,6 +32,7 @@ static ProcessorParamsBasePtr createProcessorIMUParams(const std::string & _file
params->time_tolerance = config["time tolerance"] .as<Scalar>();
params->unmeasured_perturbation_std_ = config["unmeasured perturbation std"].as<Scalar>();
+ params->unmeasured_perturbation_std = config["unmeasured perturbation std"].as<Scalar>();
params->max_time_span = kf_vote["max time span"] .as<Scalar>();
params->max_buff_length = kf_vote["max buffer length"] .as<SizeEigen >();
params->dist_traveled = kf_vote["dist traveled"] .as<Scalar>();
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_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 2c8ab239bb890532f8a26f1e41bb713d9c6e0019..604ef489478f5779ac4b79a14095c37436c81be4 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -329,7 +329,7 @@ TEST(Odom2D, KF_callback)
params->angle_turned = 6.28;
params->max_time_span = 100;
params->cov_det = 100;
- params->unmeasured_perturbation_std = 0.0000001;
+ params->unmeasured_perturbation_std = 0.000001;
Matrix3s unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3s::Identity();
ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params);
ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index e554de22f3f96af2483efa16ef5f411e37d3d53d..b65bea49219ff2fa5e2c29233d3e4d21a8e35704 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 ec8610b49da08aacbf54efc29de57d2a24cc75e4..1928d5a30fc7cd7c5a63cfcefb684b17fa675709 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;
}