diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 9facee2ec8de6bddd73e4306641d2d346c97cb0e..db50a89faf643479c251aa6cd8226c06d6498400 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -63,14 +63,14 @@ CaptureBase::~CaptureBase()
void CaptureBase::remove(bool viral_remove_empty_parent)
{
- /* Important to ensure that lower nodes are removed first.
- * Factors should be removed first, and afterwards the rest of nodes containing state blocks.
- * Then, REMOVE notifications are sent to the problem in the correct order.
- * In case multi-threading solver interrupts notifications, in the solver
- * there won't be factors (not yet notified to remove) with already removed state blocks.
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
*/
if (!is_removing_)
{
+ WOLF_INFO("CaptureBase::remove");
is_removing_ = true;
CaptureBasePtr this_C = shared_from_this(); // keep this alive while removing it
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index b3b54c5a435f13772532146c5f07569aa9c8d275..bc54b2f1a719932c95df6d539f14c8d8a29757f1 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -67,14 +67,14 @@ FactorBase::FactorBase(const std::string& _tp,
void FactorBase::remove(bool viral_remove_empty_parent)
{
- /* Important to ensure that lower nodes are removed first.
- * Factors should be removed first, and afterwards the rest of nodes containing state blocks.
- * Then, REMOVE notifications are sent to the problem in the correct order.
- * In case multi-threading solver interrupts notifications, in the solver
- * there won't be factors (not yet notified to remove) with already removed state blocks.
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
*/
if (!is_removing_)
{
+ WOLF_INFO("FactorBase::remove");
is_removing_ = true;
FactorBasePtr this_fac = shared_from_this(); // keep this alive while removing it
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 9f01a18c3a8c0a2725cbc7a299b39985c02db548..8f91944feaa9ca7b765936ba972be493133a30d7 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -39,14 +39,14 @@ FeatureBase::~FeatureBase()
void FeatureBase::remove(bool viral_remove_empty_parent)
{
- /* Important to ensure that lower nodes are removed first.
- * Factors should be removed first, and afterwards the rest of nodes containing state blocks.
- * Then, REMOVE notifications are sent to the problem in the correct order.
- * In case multi-threading solver interrupts notifications, in the solver
- * there won't be factors (not yet notified to remove) with already removed state blocks.
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
*/
if (!is_removing_)
{
+ WOLF_INFO("FeatureBase::remove");
is_removing_ = true;
FeatureBasePtr this_f = shared_from_this(); // keep this alive while removing it
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 8c670de81a35a9b7ed0816c809704e46219b5c3c..47fb6fae2beaacc0d73e5053946f643e2b975731 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -117,14 +117,14 @@ FrameBase::~FrameBase()
void FrameBase::remove(bool viral_remove_empty_parent)
{
- /* Important to ensure that lower nodes are removed first.
- * Factors should be removed first, and afterwards the rest of nodes containing state blocks.
- * Then, REMOVE notifications are sent to the problem in the correct order.
- * In case multi-threading solver interrupts notifications, in the solver
- * there won't be factors (not yet notified to remove) with already removed state blocks.
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
*/
if (!is_removing_)
{
+ WOLF_INFO("FrameBase::remove");
is_removing_ = true;
FrameBasePtr this_F = shared_from_this(); // keep this alive while removing it
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 6510fc671298093180e40bb374c6c58e4a5a85a1..42877a8e383958a52c39baee84bafa28e60a3e43 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -37,11 +37,10 @@ LandmarkBase::~LandmarkBase()
void LandmarkBase::remove(bool viral_remove_empty_parent)
{
- /* Important to ensure that lower nodes are removed first.
- * Factors should be removed first, and afterwards the rest of nodes containing state blocks.
- * Then, REMOVE notifications are sent to the problem in the correct order.
- * In case multi-threading solver interrupts notifications, in the solver
- * there won't be factors (not yet notified to remove) with already removed state blocks.
+ /* Order of removing:
+ * Factors are removed first, and afterwards the rest of nodes containing state blocks.
+ * In case multi-threading, solver can be called while removing.
+ * This order avoids SolverManager to ignore notifications (efficiency)
*/
if (!is_removing_)
{
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index 00c9c44542ab01963d4b8b894edb24c1ac4a5f48..1c3bcc60aa2354318747c3c6c75d7a0fc8a8b631 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -131,7 +131,7 @@ void SolverManager::addFactor(const FactorBasePtr& fac_ptr)
for (const auto& st : fac_ptr->getStateBlockPtrVector())
if (state_blocks_.count(st) == 0)
{
- WOLF_WARN("\n************\n************\nSolverManager::addFactor(): Factor ", fac_ptr->id(), " is notified to ADD but the involved state block ", st, " is not. Skipping, will be added later.");
+ WOLF_WARN("SolverManager::addFactor(): Factor ", fac_ptr->id(), " is notified to ADD but the involved state block ", st, " is not. Skipping, will be added later.");
// put back notification in problem (will be added next update() call) and do nothing
wolf_problem_->notifyFactor(fac_ptr, ADD);
return;
@@ -234,7 +234,7 @@ void SolverManager::removeStateBlock(const StateBlockPtr& state_ptr)
*/
if (!state_blocks_2_factors_.at(state_ptr).empty())
{
- WOLF_WARN("\n************\n************\nSolverManager::addFactor(): StateBlock ", state_ptr, " is notified to REMOVE but ", state_blocks_2_factors_.at(state_ptr).size(), " involved factors still not removed. Skipping, will be removed later.");
+ WOLF_WARN("SolverManager::removeStateBlock(): StateBlock ", state_ptr, " is notified to REMOVE but ", state_blocks_2_factors_.at(state_ptr).size(), " involved factors still not removed. Skipping, will be removed later.");
// put back notification in problem (will be removed next update() call) and do nothing
wolf_problem_->notifyStateBlock(state_ptr, REMOVE);
return;
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index d4ad57425d8dd859c0b5f7535dcf42f4ce58b976..646f43d120a868d83102e50c3bc59f711502698a 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -18,6 +18,7 @@
#include "dummy/solver_manager_dummy.h"
#include <iostream>
+#include <thread>
using namespace wolf;
using namespace Eigen;
@@ -433,7 +434,7 @@ TEST(SolverManager, AddFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// notification
Notification notif;
@@ -460,7 +461,7 @@ TEST(SolverManager, RemoveFactor)
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// update solver
solver_manager_ptr->update();
@@ -494,7 +495,7 @@ TEST(SolverManager, AddRemoveFactor)
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// notification
Notification notif;
@@ -524,12 +525,12 @@ TEST(SolverManager, DoubleRemoveFactor)
Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
- // Create (and add) factor point 2d
+ // Create (and add) factor pose 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
auto f = FeatureBase::emplace<FeatureBase>(C, "FeatureOdom2d", Vector3d::Zero(), Matrix3d::Identity());
- FactorPose2dPtr c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
// update solver
solver_manager_ptr->update();
@@ -550,9 +551,44 @@ TEST(SolverManager, DoubleRemoveFactor)
ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
}
+TEST(SolverManager, MultiThreadingTruncatedNotifications)
+{
+ double Dt = 5.0;
+ ProblemPtr P = Problem::create("PO", 2);
+ SolverManagerDummyPtr solver_manager_ptr = std::make_shared<SolverManagerDummy>(P);
+
+ // loop updating (without sleep)
+ std::thread t([&](){
+ auto start_t = std::chrono::high_resolution_clock::now();
+ while (true)
+ {
+ solver_manager_ptr->update();
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start_t).count() > Dt)
+ break;
+ }});
+
+ // loop emplacing and removing frames (window of 10 KF)
+ auto start = std::chrono::high_resolution_clock::now();
+ while (true)
+ {
+ // Emplace Frame, Capture, feature and factor pose 2d
+ FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
+ auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "FeaturePose2d", Vector3d::Zero(), Matrix3d::Identity());
+ auto c = FactorBase::emplace<FactorPose2d>(f, f, nullptr, false);
+
+ if (P->getTrajectory()->getFrameList().size() > 10)
+ P->getTrajectory()->getFrameList().front()->remove();
+
+ if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
+ break;
+ }
+
+ t.join();
+}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
-