Skip to content
Snippets Groups Projects
Commit 972b44f7 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

working on removal order

parent f235c440
No related branches found
No related tags found
1 merge request!368Resolve "Ceres assertion raised"
Pipeline #5478 passed
This commit is part of merge request !368. Comments created here will be created in the context of that merge request.
......@@ -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
......
......@@ -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
......
......@@ -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
......
......@@ -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
......
......@@ -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_)
{
......
......@@ -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;
......
......@@ -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();
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment