diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index 21d38df4b8b1cc1825d5990095f9af5a581c35b8..b4271a1a09bc6b4085acba5995706c7826fdcdf8 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 df530b78cdc310c4ec427159d79fd380b142e6ac..30e1db1531de9c8aec3e48ac13cefd33e2328a51 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 34f625fa16e5efc5f7a4feb1b64dd03e4a79b690..41a23ad30d9a13f00950f76e0281ec6e8060c72a 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 3318d01708db96e5bbd836cb92ecfb2437dda804..e5f42afb0981a19d3fd6c803c29fcbf565cb576e 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