diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h index 1a7230987849715583334ef1828bb55a08930b5d..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; } @@ -65,7 +65,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture public: virtual void setProblem(ProblemPtr _problem) final; - FeatureBasePtrList& getFeatureList(); + const FeatureBasePtrList& getFeatureList() const; void getFactorList(FactorBasePtrList& _fac_list); @@ -191,7 +191,7 @@ inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr) frame_ptr_ = _frm_ptr; } -inline FeatureBasePtrList& CaptureBase::getFeatureList() +inline const FeatureBasePtrList& CaptureBase::getFeatureList() const { return feature_list_; } diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h index 09a6c11b1cd0cd685fdb6db40ea0c3ad64a20142..210d7d84cf790a8dfae68155d231772f3d5a5b85 100644 --- a/include/core/common/wolf.h +++ b/include/core/common/wolf.h @@ -215,8 +215,9 @@ struct MatrixSizeCheck #define WOLF_LIST_TYPEDEFS(ClassName) \ class ClassName; \ - typedef std::list<ClassName##Ptr> ClassName##PtrList; \ + typedef std::list<ClassName##Ptr> ClassName##PtrList; \ typedef ClassName##PtrList::iterator ClassName##Iter; \ + typedef ClassName##PtrList::const_iterator ClassName##ConstIter; \ typedef ClassName##PtrList::reverse_iterator ClassName##RevIter; #define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \ 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 2d3506fc96cea72a4db1ed6102edf60169e54682..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; @@ -86,7 +86,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature CaptureBasePtr getCapture() const; - FactorBasePtrList& getFactorList(); + const FactorBasePtrList& getFactorList() const; unsigned int getHits() const; const FactorBasePtrList& getConstrainedByList() const; diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 23f3d0975565dfd2d9f123d4b78c291925bba810..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: @@ -136,7 +136,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase FrameBasePtr getPreviousFrame() const; FrameBasePtr getNextFrame() const; - CaptureBasePtrList& getCaptureList(); + const CaptureBasePtrList& getCaptureList() const; CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr); CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type); CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr); @@ -265,7 +265,7 @@ inline TrajectoryBasePtr FrameBase::getTrajectory() const return trajectory_ptr_.lock(); } -inline CaptureBasePtrList& FrameBase::getCaptureList() +inline const CaptureBasePtrList& FrameBase::getCaptureList() const { return capture_list_; } diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h index 99a4179542d3857ed670b4dd95ac8607c1e588b5..a309e7a03c8a5e27f459b772ce6bff5b0994f714 100644 --- a/include/core/hardware/hardware_base.h +++ b/include/core/hardware/hardware_base.h @@ -25,7 +25,7 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa HardwareBase(); virtual ~HardwareBase(); - SensorBasePtrList& getSensorList(); + const SensorBasePtrList& getSensorList() const; private: virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr); @@ -37,7 +37,7 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa namespace wolf { -inline SensorBasePtrList& HardwareBase::getSensorList() +inline const SensorBasePtrList& HardwareBase::getSensorList() const { return sensor_list_; } 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/include/core/map/map_base.h b/include/core/map/map_base.h index d4d5f4fb7de8356cf53fe05283c53093842ebb6c..6d09191e54619d5a00fce39e1ecf002aace66d3a 100644 --- a/include/core/map/map_base.h +++ b/include/core/map/map_base.h @@ -33,7 +33,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase> virtual void removeLandmark(LandmarkBasePtr _landmark_ptr); public: - LandmarkBasePtrList& getLandmarkList(); + const LandmarkBasePtrList& getLandmarkList() const; void load(const std::string& _map_file_yaml); void save(const std::string& _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf"); @@ -42,7 +42,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase> std::string dateTimeNow(); }; -inline LandmarkBasePtrList& MapBase::getLandmarkList() +inline const LandmarkBasePtrList& MapBase::getLandmarkList() const { return landmark_list_; } diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 3db21f9fe7245e54fcd6637adc8c5606ce9bd6a9..dcd73665457f2b78e1d9aed0678dabbc63e4da4c 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -109,7 +109,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa void removeProcessor(ProcessorBasePtr _proc_ptr); public: - ProcessorBasePtrList& getProcessorList(); + const ProcessorBasePtrList& getProcessorList() const; CaptureBasePtr lastKeyCapture(void); CaptureBasePtr lastCapture(const TimeStamp& _ts); @@ -229,7 +229,7 @@ inline unsigned int SensorBase::id() return sensor_id_; } -inline ProcessorBasePtrList& SensorBase::getProcessorList() +inline const ProcessorBasePtrList& SensorBase::getProcessorList() const { return processor_list_; } diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h index c41bf09830811009d415a748fbdca7a918a72f72..0645be841b4dd9c68cc2a6f557146101f199b26e 100644 --- a/include/core/trajectory/trajectory_base.h +++ b/include/core/trajectory/trajectory_base.h @@ -38,7 +38,6 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj std::string getFrameStructure() const; // Frames - FrameBasePtrList& getFrameList(); const FrameBasePtrList& getFrameList() const; FrameBasePtr getLastFrame() const; FrameBasePtr getLastKeyFrame() const; @@ -57,15 +56,10 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj void getFactorList(FactorBasePtrList & _fac_list); protected: - FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr); - void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place); + FrameBaseConstIter computeFrameOrder(FrameBasePtr _frame_ptr); + void moveFrame(FrameBasePtr _frm_ptr, FrameBaseConstIter _place); }; -inline FrameBasePtrList& TrajectoryBase::getFrameList() -{ - return frame_list_; -} - inline const FrameBasePtrList& TrajectoryBase::getFrameList() const { return frame_list_; diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index c4dcb6a0f55f7ed8dd51303135e5357faa4fcaa1..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_) { @@ -78,19 +78,19 @@ void CaptureBase::remove() FrameBasePtr F = frame_ptr_.lock(); if (F) { - F->getCaptureList().remove(this_C); - if (F->getCaptureList().empty() && F->getConstrainedByList().empty()) - F->remove(); // remove upstream + F->removeCapture(this_C); + 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 } } } @@ -315,6 +315,7 @@ void CaptureBase::move(FrameBasePtr _frm_ptr) void CaptureBase::link(FrameBasePtr _frm_ptr) { + assert(!is_removing_ && "linking a removed capture"); assert(this->getFrame() == nullptr && "linking a capture already linked"); if(_frm_ptr) diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp index a653807ebbaf849ab8fea0dd81c07592f197fb14..f64a1464b0c441f43c731b1a4420cddc3d3794fd 100644 --- a/src/factor/factor_base.cpp +++ b/src/factor/factor_base.cpp @@ -44,54 +44,54 @@ 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_) { is_removing_ = true; - FactorBasePtr this_c = shared_from_this(); // keep this alive while removing it + FactorBasePtr this_fac = shared_from_this(); // keep this alive while removing it FeatureBasePtr f = feature_ptr_.lock(); if (f) { - f->getFactorList().remove(shared_from_this()); // remove from upstream - if (f->getFactorList().empty() && f->getConstrainedByList().empty()) - f->remove(); // remove upstream + f->removeFactor(this_fac); // remove from 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) - getProblem()->notifyFactor(shared_from_this(),REMOVE); + getProblem()->notifyFactor(this_fac,REMOVE); // remove other: {Frame, Capture, Feature, Landmark} FrameBasePtr frm_o = frame_other_ptr_.lock(); if (frm_o) { - frm_o->removeConstrainedBy(shared_from_this()); + 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(); if (cap_o) { - cap_o->removeConstrainedBy(shared_from_this()); + 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(); if (ftr_o) { - ftr_o->removeConstrainedBy(shared_from_this()); + 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(); if (lmk_o) { - lmk_o->removeConstrainedBy(shared_from_this()); + 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; @@ -143,6 +143,7 @@ void FactorBase::setStatus(FactorStatus _status) void FactorBase::link(FeatureBasePtr _ftr_ptr) { + assert(!is_removing_ && "linking a removed factor"); assert(this->getFeature() == nullptr && "linking an already linked factor"); // not link if nullptr diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp index 2929eb17fc4bb44f7b6b6094c05590c62a9e6ceb..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_) { @@ -47,19 +47,19 @@ void FeatureBase::remove() CaptureBasePtr C = capture_ptr_.lock(); if (C) { - C->getFeatureList().remove(this_f); // remove from upstream - if (C->getFeatureList().empty()) - C->remove(); // remove upstream + C->removeFeature(this_f); // remove from 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 } } } @@ -91,7 +91,7 @@ void FeatureBase::removeConstrainedBy(FactorBasePtr _fac_ptr) constrained_by_list_.remove(_fac_ptr); } -FactorBasePtrList& FeatureBase::getFactorList() +const FactorBasePtrList& FeatureBase::getFactorList() const { return factor_list_; } @@ -157,6 +157,7 @@ Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) con void FeatureBase::link(CaptureBasePtr _cpt_ptr) { + assert(!is_removing_ && "linking a removed feature"); assert(this->getCapture() == nullptr && "linking an already linked feature"); if(_cpt_ptr) diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index aee1ecfaec2139251015f1469eddab6d859d3747..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_) { @@ -97,27 +97,23 @@ void FrameBase::remove() TrajectoryBasePtr T = trajectory_ptr_.lock(); if (T) { - T->getFrameList().remove(this_F); // remove from upstream + T->removeFrame(this_F); // remove from upstream } // 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 if ( isKeyOrAux() ) removeStateBlocks(); - - if (getTrajectory()->getLastKeyFrame()->id() == this_F->id() || getTrajectory()->getLastKeyOrAuxFrame()->id() == this_F->id()) - getTrajectory()->updateLastFrames(); - // std::cout << "Removed F" << id() << std::endl; } } @@ -408,6 +404,7 @@ void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr) void FrameBase::link(TrajectoryBasePtr _trj_ptr) { + assert(!is_removing_ && "linking a removed frame"); assert(this->getTrajectory() == nullptr && "linking an already linked frame"); if(_trj_ptr) diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index cb3f7cb21d3c0172e99ee482018f48365206d15f..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_) { @@ -39,12 +39,12 @@ void LandmarkBase::remove() // remove from upstream auto M = map_ptr_.lock(); if (M) - M->getLandmarkList().remove(shared_from_this()); + M->removeLandmark(this_L); // 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 @@ -164,6 +164,7 @@ YAML::Node LandmarkBase::saveToYaml() const void LandmarkBase::link(MapBasePtr _map_ptr) { + assert(!is_removing_ && "linking a removed landmark"); assert(this->getMap() == nullptr && "linking an already linked landmark"); if(_map_ptr) diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp index 6dcc3c3417a34f68ba7b56502b4b53f75bc996c9..4d9e937028360e651dfbb0377927ad2186244b8e 100644 --- a/src/processor/processor_base.cpp +++ b/src/processor/processor_base.cpp @@ -75,12 +75,13 @@ void ProcessorBase::remove() // remove from upstream SensorBasePtr sen = sensor_ptr_.lock(); if(sen) - sen->getProcessorList().remove(this_p); + sen->removeProcessor(this_p); } } void ProcessorBase::link(SensorBasePtr _sen_ptr) { + assert(!is_removing_ && "linking a removed processor"); assert(this->getSensor() == nullptr && "linking an already linked processor"); if(_sen_ptr) diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 2f5a11c9b25ed40d9613f20d1f68d22d57b72bf1..accc54225043d1a4352596f65f4622d8c54464ca 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -409,6 +409,7 @@ void SensorBase::setProblem(ProblemPtr _problem) void SensorBase::link(HardwareBasePtr _hw_ptr) { + assert(!is_removing_ && "linking a removed sensor"); assert(this->getHardware() == nullptr && "linking an already linked sensor"); if(_hw_ptr) { diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp index 34921e3f468a0e4960f4b305978eb33ba01109f4..35a1f9997e7ab5bffa8f4dbf4f8ac7c44415b66b 100644 --- a/src/trajectory/trajectory_base.cpp +++ b/src/trajectory/trajectory_base.cpp @@ -55,7 +55,7 @@ void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr) moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr)); } -void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place) +void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseConstIter _place) { if (*_place != _frm_ptr) { @@ -65,7 +65,7 @@ void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place) } } -FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr) +FrameBaseConstIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr) { for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); frm_rit++) if ((*frm_rit)!= _frame_ptr && (*frm_rit)->isKeyOrAux() && (*frm_rit)->getTimeStamp() <= _frame_ptr->getTimeStamp())