Implement isConstrainedBy(factor)

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_;

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;

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);

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);

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())

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_;

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");

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     >();