diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h index 08751287b5109a81160c3e70aadc4ae9d6f33253..8732820b4db10cd2a561c83d63defd82108f5ee4 100644 --- a/include/core/capture/capture_base.h +++ b/include/core/capture/capture_base.h @@ -110,6 +110,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture SizeEigen computeCalibSize() const; FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr); + void removeFeature(FeatureBasePtr _ft_ptr); private: void updateCalibSize(); }; diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h index 796579ce398961dd8069b425d063e678855731ca..b4ea5b7c7d51b306e7c1aa0408419984ad476e75 100644 --- a/include/core/factor/factor_base.h +++ b/include/core/factor/factor_base.h @@ -113,7 +113,6 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa /** \brief Returns a pointer to the feature constrained from **/ FeatureBasePtr getFeature() const; - void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;} /** \brief Returns a pointer to its capture **/ @@ -177,6 +176,9 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa template<typename classType, typename... T> static std::shared_ptr<FactorBase> emplace(FeatureBasePtr _oth_ptr, T&&... all); + protected: + + void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;} // protected: // template<typename D> // void print(const std::string& name, const Eigen::MatrixBase<D>& mat) const; // Do nothing if input Scalar type is ceres::Jet diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h index c3a709fd563cafed50de1a1bb393cef1ec36a9e5..5e4b6cc9d45367c6a611ff75436bfc764a29bdb8 100644 --- a/include/core/feature/feature_base.h +++ b/include/core/feature/feature_base.h @@ -85,7 +85,6 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature FrameBasePtr getFrame() const; CaptureBasePtr getCapture() const; - void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;} FactorBasePtrList& getFactorList(); @@ -103,7 +102,10 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature protected: Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const; + + void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;} FactorBasePtr addFactor(FactorBasePtr _co_ptr); + void removeFactor(FactorBasePtr _co_ptr); virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr); virtual void removeConstrainedBy(FactorBasePtr _fac_ptr); }; diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 98f50bb773ae2edc949510483b73a09c0aac60d8..bc059138890b4817bf5fa7b69683e7154b05d6a9 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -130,7 +130,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase virtual void setProblem(ProblemPtr _problem) final; TrajectoryBasePtr getTrajectory() const; - void setTrajectory(TrajectoryBasePtr _trj_ptr); FrameBasePtr getPreviousFrame() const; FrameBasePtr getNextFrame() const; @@ -154,6 +153,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase protected: CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr); + void removeCapture(CaptureBasePtr _capt_ptr); + void setTrajectory(TrajectoryBasePtr _trj_ptr); public: static FrameBasePtr create_PO_2D (const FrameType & _tp, const TimeStamp& _ts, diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h index 5c27689da7891d8e031e20f4b038a173ceefcd9b..fc8b87c08b5a7dc6802ee6c48dadf65820b76eee 100644 --- a/include/core/landmark/landmark_base.h +++ b/include/core/landmark/landmark_base.h @@ -91,7 +91,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma unsigned int getHits() const; const FactorBasePtrList& getConstrainedByList() const; - void setMap(const MapBasePtr _map_ptr); MapBasePtr getMap(); void link(MapBasePtr); template<typename classType, typename... T> @@ -99,6 +98,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma protected: + void setMap(const MapBasePtr _map_ptr); virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr); virtual void removeConstrainedBy(FactorBasePtr _fac_ptr); }; diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h index ee6d245283c625fd4f16538620c7b14868ce5850..136f42f80aab5c41063a8de140de9c707f2e1032 100644 --- a/include/core/map/map_base.h +++ b/include/core/map/map_base.h @@ -30,6 +30,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase> protected: virtual LandmarkBasePtr addLandmark(LandmarkBasePtr _landmark_ptr); + virtual void removeLandmark(LandmarkBasePtr _landmark_ptr); public: LandmarkBasePtrList& getLandmarkList(); diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h index a6f8ac588195b3402ba2aebab998a8d8f6fcf1f7..7c171a11f3761fbb313655b314bdf35c090b6665 100644 --- a/include/core/processor/processor_base.h +++ b/include/core/processor/processor_base.h @@ -204,8 +204,10 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce SensorBasePtr getSensor(); const SensorBasePtr getSensor() const; + protected: void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;} + public: virtual bool isMotion(); void setTimeTolerance(Scalar _time_tolerance); diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 0f1678fd17cdd02aa8935c721ba3ceb48f5071c1..010d3fb78b1674a5407319d2932d4871ad8c4521 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -101,6 +101,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa protected: void setHardware(const HardwareBasePtr _hw_ptr); ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr); + void removeProcessor(ProcessorBasePtr _proc_ptr); public: ProcessorBasePtrList& getProcessorList(); @@ -186,6 +187,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa Eigen::MatrixXs getNoiseCov(); void setExtrinsicDynamic(bool _extrinsic_dynamic); void setIntrinsicDynamic(bool _intrinsic_dynamic); + void link(HardwareBasePtr); template<typename classType, typename... T> static std::shared_ptr<SensorBase> emplace(HardwareBasePtr _hwd_ptr, T&&... all); diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h index 339a41b3dad5a1cab9743360a9f7e2bd6f55c380..5d22163797d38016d1a62f14f4bceeae9cd066b2 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 - FrameBasePtr addFrame(FrameBasePtr _frame_ptr); FrameBasePtrList& getFrameList(); const FrameBasePtrList& getFrameList() const; FrameBasePtr getLastFrame() const; @@ -51,6 +50,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj protected: FrameBasePtr addFrame(FrameBasePtr _frame_ptr); + void removeFrame(FrameBasePtr _frame_ptr); public: // factors diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index 5aa5ea3662707d42f7f34b4e85b0919564e39b3d..f531a781e5b2e3ffeda42966b1e1382d5e5c6d79 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -102,7 +102,9 @@ FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr) return _ft_ptr; } +void CaptureBase::removeFeature(FeatureBasePtr _ft_ptr) { + feature_list_.remove(_ft_ptr); } void CaptureBase::getFactorList(FactorBasePtrList& _fac_list) @@ -114,7 +116,6 @@ void CaptureBase::getFactorList(FactorBasePtrList& _fac_list) FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr) { constrained_by_list_.push_back(_fac_ptr); - _fac_ptr->setCaptureOther(shared_from_this()); return _fac_ptr; } diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp index 26b325da868670ac0c8849998d61a7fbcb678d02..33ee54e3dbf7591049f1e009835852bcf972b30e 100644 --- a/src/feature/feature_base.cpp +++ b/src/feature/feature_base.cpp @@ -70,6 +70,11 @@ FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr) return _co_ptr; } +void FeatureBase::removeFactor(FactorBasePtr _co_ptr) +{ + factor_list_.remove(_co_ptr); +} + FrameBasePtr FeatureBase::getFrame() const { return capture_ptr_.lock()->getFrame(); diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index 7fd6bf365bcbbcd42ba9ff39f8ea0da3cccbaed6..02914eb1a2e7910c5c720742d3e001ac59af092b 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -323,6 +323,11 @@ CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr) return _capt_ptr; } +void FrameBase::removeCapture(CaptureBasePtr _capt_ptr) +{ + capture_list_.remove(_capt_ptr); +} + CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) { for (CaptureBasePtr capture_ptr : getCaptureList()) diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp index 204fc7b8ffd3d1b1a8c49030c74c0e803e11b7c4..0da491faa394a88e791e1a9b85fa408279296152 100644 --- a/src/map/map_base.cpp +++ b/src/map/map_base.cpp @@ -33,20 +33,9 @@ LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr) return _landmark_ptr; } -void MapBase::addLandmarkList(LandmarkBasePtrList& _landmark_list) +void MapBase::removeLandmark(LandmarkBasePtr _landmark_ptr) { - for (auto lmk : _landmark_list) - addLandmark(lmk); - - //TEMPORARY FIX, should be made compliant with the new emplace methodology - LandmarkBasePtrList lmk_list_copy = _landmark_list; //since _landmark_list will be empty after addDownNodeList() - for (LandmarkBasePtr landmark_ptr : lmk_list_copy) - { - landmark_ptr->setMap(shared_from_this()); - landmark_ptr->setProblem(getProblem()); - landmark_ptr->registerNewStateBlocks(); - } - landmark_list_.splice(landmark_list_.end(), _landmark_list); + landmark_list_.remove(_landmark_ptr); } void MapBase::load(const std::string& _map_file_dot_yaml) diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 87819e750035cc5062fb8ad0c4588a4b717987f9..455c72b996f9c7c1b519eb80345a2b196103a9d0 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -430,16 +430,6 @@ void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _proces processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance); } -LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr) -{ - getMap()->addLandmark(_lmk_ptr); - return _lmk_ptr; -} - -void Problem::addLandmarkList(LandmarkBasePtrList& _lmk_list) -{ - getMap()->addLandmarkList(_lmk_list); -} StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr) { diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index c633261ade4a3835e38969eb122d59d5be014928..4986a3521f82f2e42fb1a627acb572e651d227f8 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -339,6 +339,11 @@ ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr) return _proc_ptr; } +void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr) +{ + processor_list_.remove(_proc_ptr); +} + StateBlockPtr SensorBase::getStateBlock(unsigned int _i) { CaptureBasePtr cap; diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp index a2395ffae0c1512ca4157fc335bd02ed62855d6a..34921e3f468a0e4960f4b305978eb33ba01109f4 100644 --- a/src/trajectory/trajectory_base.cpp +++ b/src/trajectory/trajectory_base.cpp @@ -34,6 +34,16 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr) return _frame_ptr; } +void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr) +{ + // add to list + frame_list_.remove(_frame_ptr); + + // update last_estimated and last_key + if (_frame_ptr->isKeyOrAux()) + updateLastFrames(); +} + void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list) { for(auto fr_ptr : getFrameList())