From 2016e347100833b5339a877b00b05755491405a8 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Mon, 12 Mar 2018 12:34:46 +0100
Subject: [PATCH] Improve the remove() strategy for Captures after providing
 them with state blocks

---
 src/capture_base.cpp    |  6 +++++-
 src/constraint_base.cpp | 10 +++++++++-
 src/feature_base.cpp    |  5 ++++-
 src/frame_base.cpp      |  3 +++
 4 files changed, 21 insertions(+), 3 deletions(-)

diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index 21d38df4b..b4271a1a0 100644
--- a/src/capture_base.cpp
+++ b/src/capture_base.cpp
@@ -90,6 +90,10 @@ void CaptureBase::remove()
         {
             feature_list_.front()->remove(); // remove downstream
         }
+        while (!constrained_by_list_.empty())
+        {
+            constrained_by_list_.front()->remove(); // remove constrained by
+        }
     }
 }
 
@@ -146,7 +150,7 @@ void CaptureBase::removeStateBlocks()
 {
     for (unsigned int i = 0; i < state_block_vec_.size(); i++)
     {
-        auto sbp = getStateBlockPtr(i);
+        auto sbp = state_block_vec_[i];
         if (sbp != nullptr)
         {
             if (getProblem() != nullptr)
diff --git a/src/constraint_base.cpp b/src/constraint_base.cpp
index df530b78c..30e1db153 100644
--- a/src/constraint_base.cpp
+++ b/src/constraint_base.cpp
@@ -62,7 +62,7 @@ void ConstraintBase::remove()
         if (getProblem() != nullptr)
             getProblem()->removeConstraintPtr(shared_from_this());
 
-        // remove other: {Frame, feature, Landmark}
+        // remove other: {Frame, Capture, Feature, Landmark}
         FrameBasePtr frm_o = frame_other_ptr_.lock();
         if (frm_o)
         {
@@ -71,6 +71,14 @@ void ConstraintBase::remove()
                 frm_o->remove();
         }
 
+        CaptureBasePtr cap_o = capture_other_ptr_.lock();
+        if (cap_o)
+        {
+            cap_o->getConstrainedByList().remove(shared_from_this());
+            if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
+                cap_o->remove();
+        }
+
         FeatureBasePtr ftr_o = feature_other_ptr_.lock();
         if (ftr_o)
         {
diff --git a/src/feature_base.cpp b/src/feature_base.cpp
index 34f625fa1..41a23ad30 100644
--- a/src/feature_base.cpp
+++ b/src/feature_base.cpp
@@ -30,6 +30,8 @@ void FeatureBase::remove()
     {
         is_removing_ = true;
         FeatureBasePtr this_f = shared_from_this(); // keep this alive while removing it
+
+        // remove from upstream
         CaptureBasePtr C = capture_ptr_.lock();
         if (C)
         {
@@ -37,6 +39,8 @@ void FeatureBase::remove()
             if (C->getFeatureList().empty())
                 C->remove(); // remove upstream
         }
+
+        // remove downstream
         while (!constraint_list_.empty())
         {
             constraint_list_.front()->remove(); // remove downstream
@@ -45,7 +49,6 @@ void FeatureBase::remove()
         {
             constrained_by_list_.front()->remove(); // remove constrained
         }
-//        std::cout << "Removed           f" << id() << std::endl;
     }
 }
 
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 3318d0170..e5f42afb0 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -69,12 +69,15 @@ void FrameBase::remove()
     {
         is_removing_ = true;
         FrameBasePtr this_F = shared_from_this(); // keep this alive while removing it
+
+        // remove from upstream
         TrajectoryBasePtr T = trajectory_ptr_.lock();
         if (T)
         {
             T->getFrameList().remove(this_F); // remove from upstream
         }
 
+        // remove downstream
         while (!capture_list_.empty())
         {
             capture_list_.front()->remove(); // remove downstream
-- 
GitLab