From 972b44f726b597d8c016aff41c2cf2da40876c53 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 29 May 2020 16:37:33 +0200
Subject: [PATCH] working on removal order
---
src/capture/capture_base.cpp | 10 +++----
src/factor/factor_base.cpp | 10 +++----
src/feature/feature_base.cpp | 10 +++----
src/frame/frame_base.cpp | 10 +++----
src/landmark/landmark_base.cpp | 9 +++----
src/solver/solver_manager.cpp | 4 +--
test/gtest_solver_manager.cpp | 48 +++++++++++++++++++++++++++++-----
7 files changed, 68 insertions(+), 33 deletions(-)
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 9facee2ec..db50a89fa 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 b3b54c5a4..bc54b2f1a 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 9f01a18c3..8f91944fe 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 8c670de81..47fb6fae2 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 6510fc671..42877a8e3 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 00c9c4454..1c3bcc60a 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 d4ad57425..646f43d12 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();
}
-
--
GitLab