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
diff --git a/src/problem.cpp b/src/problem.cpp
index e36dc43c0828fc688a964905a7169c42e2a3afe0..46cdb2eb85d804a215a6d74dd3456142201f40d1 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -697,7 +697,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
cout << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardwarePtr()->getSensorList().size()) + "S") : "") << endl;
if (depth >= 1)
{
- // Sensors
+ // Sensors =======================================================================================
for (auto S : getHardwarePtr()->getSensorList())
{
cout << " S" << S->id() << " " << S->getType();
@@ -742,7 +742,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
}
if (depth >= 2)
{
- // Processors
+ // Processors =======================================================================================
for (auto p : S->getProcessorList())
{
if (p->isMotion())
@@ -781,7 +781,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
cout << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectoryPtr()->getFrameList().size()) + "F") : "") << endl;
if (depth >= 1)
{
- // Frames
+ // Frames =======================================================================================
for (auto F : getTrajectoryPtr()->getFrameList())
{
cout << (F->isKey() ? " KF" : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
@@ -809,7 +809,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
}
if (depth >= 2)
{
- // Captures
+ // Captures =======================================================================================
for (auto C : F->getCaptureList())
{
cout << " C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
@@ -865,7 +865,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
if (depth >= 3)
{
- // Features
+ // Features =======================================================================================
for (auto f : C->getFeatureList())
{
cout << " f" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getConstraintList().size()) + "c " : "");
@@ -881,7 +881,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
<< " )" << endl;
if (depth >= 4)
{
- // Constraints
+ // Constraints =======================================================================================
for (auto c : f->getConstraintList())
{
cout << " c" << c->id() << " " << c->getType() << " -->";
@@ -907,7 +907,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
cout << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMapPtr()->getLandmarkList().size()) + "L") : "") << endl;
if (depth >= 1)
{
- // Landmarks
+ // Landmarks =======================================================================================
for (auto L : getMapPtr()->getLandmarkList())
{
cout << " L" << L->id() << " " << L->getType();
@@ -967,6 +967,8 @@ bool Problem::check(int verbose_level)
}
// check pointer to Problem
is_consistent = is_consistent && (H->getProblem().get() == P_raw);
+
+ // Sensors =======================================================================================
for (auto S : H->getSensorList())
{
if (verbose_level > 0)
@@ -989,6 +991,8 @@ bool Problem::check(int verbose_level)
// check problem and hardware pointers
is_consistent = is_consistent && (S->getProblem().get() == P_raw);
is_consistent = is_consistent && (S->getHardwarePtr() == H);
+
+ // Processors =======================================================================================
for (auto p : S->getProcessorList())
{
if (verbose_level > 0)
@@ -1012,6 +1016,8 @@ bool Problem::check(int verbose_level)
}
// check pointer to Problem
is_consistent = is_consistent && (T->getProblem().get() == P_raw);
+
+ // Frames =======================================================================================
for (auto F : T->getFrameList())
{
if (verbose_level > 0)
@@ -1057,6 +1063,8 @@ bool Problem::check(int verbose_level)
}
}
}
+
+ // Captures =======================================================================================
for (auto C : F->getCaptureList())
{
if (verbose_level > 0)
@@ -1082,6 +1090,8 @@ bool Problem::check(int verbose_level)
// check problem and frame pointers
is_consistent = is_consistent && (C->getProblem().get() == P_raw);
is_consistent = is_consistent && (C->getFramePtr() == F);
+
+ // Features =======================================================================================
for (auto f : C->getFeatureList())
{
if (verbose_level > 0)
@@ -1104,6 +1114,8 @@ bool Problem::check(int verbose_level)
// check constrained_by pointer to this feature
is_consistent = is_consistent && (cby->getFeatureOtherPtr() == f);
}
+
+ // Constraints =======================================================================================
for (auto c : f->getConstraintList())
{
if (verbose_level > 0)
@@ -1267,6 +1279,8 @@ bool Problem::check(int verbose_level)
cout << "M @ " << M.get() << endl;
// check pointer to Problem
is_consistent = is_consistent && (M->getProblem().get() == P_raw);
+
+ // Landmarks =======================================================================================
for (auto L : M->getLandmarkList())
{
if (verbose_level > 0)