diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h index aaa76c9220c4351f1cca5b6a38bf290ef8e69c58..a5eac842ef7afa410d15a42e7e749ddccc97635c 100644 --- a/include/core/capture/capture_base.h +++ b/include/core/capture/capture_base.h @@ -79,6 +79,8 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s public: unsigned int getHits() const; const FactorBasePtrList& getConstrainedByList() const; + bool isConstrainedBy(const FactorBasePtr &_factor) const; + // State blocks const std::string& getStructure() const; @@ -183,6 +185,7 @@ inline const FactorBasePtrList& CaptureBase::getConstrainedByList() const return constrained_by_list_; } + inline TimeStamp CaptureBase::getTimeStamp() const { return time_stamp_; diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h index 7203425913de8bb5a3ff5a0fb267b4aef98f9fd5..a2ba3bc60f5d0db16d6c486099ac5e3e9b3116ec 100644 --- a/include/core/feature/feature_base.h +++ b/include/core/feature/feature_base.h @@ -95,6 +95,9 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature unsigned int getHits() const; const FactorBasePtrList& getConstrainedByList() const; + bool isConstrainedBy(const FactorBasePtr &_factor) const; + + // all factors void getFactorList(FactorBasePtrList & _fac_list) const; diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index 3745de04806c78f65463c39da49bcf9ba42cf0f8..eea8873b3e804743f4cd224bffd6442d4c17febf 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -126,6 +126,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha void getFactorList(FactorBasePtrList& _fac_list) const; unsigned int getHits() const; const FactorBasePtrList& getConstrainedByList() const; + bool isConstrainedBy(const FactorBasePtr& _factor) const; void link(TrajectoryBasePtr); template<typename classType, typename... T> static std::shared_ptr<classType> emplace(TrajectoryBasePtr _ptr, T&&... all); diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h index 2f80a3d6fbe26d171126a82d280e65a3acaf4349..254e03e480f8471029c38c8f59626b230dc962f9 100644 --- a/include/core/landmark/landmark_base.h +++ b/include/core/landmark/landmark_base.h @@ -71,6 +71,8 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_ unsigned int getHits() const; const FactorBasePtrList& getConstrainedByList() const; + bool isConstrainedBy(const FactorBasePtr &_factor) const; + MapBasePtr getMap() const; void link(MapBasePtr); diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index 54e5d2a3cdedb301a7580e691cad9543df3c24c2..d07b9b055d8197593dbdde99d05f6a4eddd46bfe 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -130,6 +130,22 @@ void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr) constrained_by_list_.remove(_fac_ptr); } +bool CaptureBase::isConstrainedBy(const FactorBasePtr &_factor) const +{ + FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(), + constrained_by_list_.end(), + [_factor](const FactorBasePtr & cby) + { + return cby == _factor; + } + ); + if (cby_it != constrained_by_list_.end()) + return true; + else + return false; +} + + const std::string& CaptureBase::getStructure() const { if (getSensor()) diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp index df5d1f1e7aa0f214adab2e0f74dc496623e7cf48..8dc3801efa5f93e5bd0df593442e4860cb75135e 100644 --- a/src/feature/feature_base.cpp +++ b/src/feature/feature_base.cpp @@ -92,6 +92,21 @@ void FeatureBase::removeConstrainedBy(FactorBasePtr _fac_ptr) constrained_by_list_.remove(_fac_ptr); } +bool FeatureBase::isConstrainedBy(const FactorBasePtr &_factor) const +{ + FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(), + constrained_by_list_.end(), + [_factor](const FactorBasePtr & cby) + { + return cby == _factor; + } + ); + if (cby_it != constrained_by_list_.end()) + return true; + else + return false; +} + const FactorBasePtrList& FeatureBase::getFactorList() const { return factor_list_; diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index 34e36705f665fa0d09cf6080b6f17697d9d58d55..a3c679b62a78fd7131bed4f7fee11cf6a76bdf70 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -297,6 +297,21 @@ void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr) constrained_by_list_.remove(_fac_ptr); } +bool FrameBase::isConstrainedBy(const FactorBasePtr &_factor) const +{ + FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(), + constrained_by_list_.end(), + [_factor](const FactorBasePtr & cby) + { + return cby == _factor; + } + ); + if (cby_it != constrained_by_list_.end()) + return true; + else + return false; +} + void FrameBase::link(TrajectoryBasePtr _trj_ptr) { assert(!is_removing_ && "linking a removed frame"); diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index 7c9b8b664573bd296902ad420c1e5a161605e635..bc57f27294e6a0b05d67cbcf44725dd2b819f63b 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -138,6 +138,21 @@ void LandmarkBase::removeConstrainedBy(FactorBasePtr _fac_ptr) constrained_by_list_.remove(_fac_ptr); } +bool LandmarkBase::isConstrainedBy(const FactorBasePtr &_factor) const +{ + FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(), + constrained_by_list_.end(), + [_factor](const FactorBasePtr & cby) + { + return cby == _factor; + } + ); + if (cby_it != constrained_by_list_.end()) + return true; + else + return false; +} + LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node) { unsigned int id = _node["id"] .as< unsigned int >();