diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index ef299cecee9c4c4c4afa71c585957f0131f8640d..f5c7a657f2305ae3d9f540865ef6c26b81f8ec8f 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -47,7 +47,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
StateBlockPtr _intr_ptr = nullptr);
virtual ~CaptureBase();
- virtual void remove();
+ virtual void remove(bool viral_remove_empty_parent=false);
// Type
virtual bool isMotion() const { return false; }
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index fbc07e89af8850d9e4b4228817b5bbb6e5abac53..a6890dca220baa11ce1b33d5a85f17290a3e3626 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -78,7 +78,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
virtual ~FactorBase() = default;
- virtual void remove();
+ virtual void remove(bool viral_remove_empty_parent=false);
unsigned int id() const;
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 0a7feceed3f3ff224608147c779136a0200e25ae..2e3719dd6e58b9b7f473c66779ce4326b3747278 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -53,7 +53,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_uncertainty, UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
virtual ~FeatureBase();
- virtual void remove();
+ virtual void remove(bool viral_remove_empty_parent=false);
virtual void setProblem(ProblemPtr _problem) final;
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 569032675cd9e59a77a89995bf1c2877b4814d59..853b8f64c8883fb307128d714d79b62d7c0a8905 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -71,7 +71,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x);
virtual ~FrameBase();
- virtual void remove();
+ virtual void remove(bool viral_remove_empty_parent=false);
// Frame properties -----------------------------------------------
public:
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 57d294acfe4404cb16fc3abdcc15e5225d2053ac..ce1ab0860d1767b396a302f7f8671aa09498e22a 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -48,7 +48,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
**/
LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
virtual ~LandmarkBase();
- virtual void remove();
+ virtual void remove(bool viral_remove_empty_parent=false);
virtual YAML::Node saveToYaml() const;
// Properties
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 1dde6230203c92faf50d64b76af917eb7f56e1f5..899327294dadc122747223403e26b6d0e58b67f4 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -64,7 +64,7 @@ CaptureBase::~CaptureBase()
removeStateBlocks();
}
-void CaptureBase::remove()
+void CaptureBase::remove(bool viral_remove_empty_parent)
{
if (!is_removing_)
{
@@ -79,18 +79,18 @@ void CaptureBase::remove()
if (F)
{
F->removeCapture(this_C);
- if (F->getCaptureList().empty() && F->getConstrainedByList().empty())
- F->remove(); // remove upstream
+ if (viral_remove_empty_parent && F->getCaptureList().empty() && F->getConstrainedByList().empty())
+ F->remove(viral_remove_empty_parent); // remove upstream
}
// remove downstream
while (!feature_list_.empty())
{
- feature_list_.front()->remove(); // remove downstream
+ feature_list_.front()->remove(viral_remove_empty_parent); // remove downstream
}
while (!constrained_by_list_.empty())
{
- constrained_by_list_.front()->remove(); // remove constrained by
+ constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained by
}
}
}
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index ba66157fef97f3de1cdcc24990275526f0da4da2..f64a1464b0c441f43c731b1a4420cddc3d3794fd 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -44,7 +44,7 @@ FactorBase::FactorBase(const std::string& _tp,
// std::cout << "constructed +c" << id() << std::endl;
}
-void FactorBase::remove()
+void FactorBase::remove(bool viral_remove_empty_parent)
{
if (!is_removing_)
{
@@ -54,8 +54,8 @@ void FactorBase::remove()
if (f)
{
f->removeFactor(this_fac); // remove from upstream
- if (f->getFactorList().empty() && f->getConstrainedByList().empty())
- f->remove(); // remove upstream
+ if (viral_remove_empty_parent && f->getFactorList().empty() && f->getConstrainedByList().empty())
+ f->remove(viral_remove_empty_parent); // remove upstream
}
// add factor to be removed from solver
if (getProblem() != nullptr)
@@ -67,7 +67,7 @@ void FactorBase::remove()
{
frm_o->removeConstrainedBy(this_fac);
if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
- frm_o->remove();
+ frm_o->remove(viral_remove_empty_parent);
}
CaptureBasePtr cap_o = capture_other_ptr_.lock();
@@ -75,7 +75,7 @@ void FactorBase::remove()
{
cap_o->removeConstrainedBy(this_fac);
if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
- cap_o->remove();
+ cap_o->remove(viral_remove_empty_parent);
}
FeatureBasePtr ftr_o = feature_other_ptr_.lock();
@@ -83,7 +83,7 @@ void FactorBase::remove()
{
ftr_o->removeConstrainedBy(this_fac);
if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
- ftr_o->remove();
+ ftr_o->remove(viral_remove_empty_parent);
}
LandmarkBasePtr lmk_o = landmark_other_ptr_.lock();
@@ -91,7 +91,7 @@ void FactorBase::remove()
{
lmk_o->removeConstrainedBy(this_fac);
if (lmk_o->getConstrainedByList().empty())
- lmk_o->remove();
+ lmk_o->remove(viral_remove_empty_parent);
}
// std::cout << "Removed c" << id() << std::endl;
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 61b7b8d0bfbfbaaf9f5a81cfb84d0191de043e88..f2b4dfca9b65bd7cf0ad64bca7de55d8dc53d830 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -36,7 +36,7 @@ FeatureBase::~FeatureBase()
// std::cout << "destructed -f" << id() << std::endl;
}
-void FeatureBase::remove()
+void FeatureBase::remove(bool viral_remove_empty_parent)
{
if (!is_removing_)
{
@@ -48,18 +48,18 @@ void FeatureBase::remove()
if (C)
{
C->removeFeature(this_f); // remove from upstream
- if (C->getFeatureList().empty())
- C->remove(); // remove upstream
+ if (viral_remove_empty_parent && C->getFeatureList().empty() && C->getConstrainedByList().empty())
+ C->remove(viral_remove_empty_parent); // remove upstream
}
// remove downstream
while (!factor_list_.empty())
{
- factor_list_.front()->remove(); // remove downstream
+ factor_list_.front()->remove(viral_remove_empty_parent); // remove downstream
}
while (!constrained_by_list_.empty())
{
- constrained_by_list_.front()->remove(); // remove constrained
+ constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained
}
}
}
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index e7c4f234937dc3d29da2fd0dbd8e257656b60ea0..8a87eae58494bc5c8b56bdf4b068e5ffb42bfdcf 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -86,7 +86,7 @@ FrameBase::~FrameBase()
{
}
-void FrameBase::remove()
+void FrameBase::remove(bool viral_remove_empty_parent)
{
if (!is_removing_)
{
@@ -103,11 +103,11 @@ void FrameBase::remove()
// remove downstream
while (!capture_list_.empty())
{
- capture_list_.front()->remove(); // remove downstream
+ capture_list_.front()->remove(viral_remove_empty_parent); // remove downstream
}
while (!constrained_by_list_.empty())
{
- constrained_by_list_.front()->remove(); // remove constrained
+ constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained
}
// Remove Frame State Blocks
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 87e1f2fef9820f2a3fe93c1cf6cc57e9a66f6d5f..3da95fb8d36b218b1f91d10ce409638f256ab1ae 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -29,7 +29,7 @@ LandmarkBase::~LandmarkBase()
// std::cout << "destructed -L" << id() << std::endl;
}
-void LandmarkBase::remove()
+void LandmarkBase::remove(bool viral_remove_empty_parent)
{
if (!is_removing_)
{
@@ -44,7 +44,7 @@ void LandmarkBase::remove()
// remove constrained by
while (!constrained_by_list_.empty())
{
- constrained_by_list_.front()->remove();
+ constrained_by_list_.front()->remove(viral_remove_empty_parent);
}
// Remove State Blocks