From b8e7112f29b1ea6462d2ea2347f6d1ff9d854832 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Wed, 1 Apr 2020 01:48:00 +0200
Subject: [PATCH] Implement isConstrainedBy(factor)
---
include/core/capture/capture_base.h | 3 +++
include/core/feature/feature_base.h | 3 +++
include/core/frame/frame_base.h | 1 +
include/core/landmark/landmark_base.h | 2 ++
src/capture/capture_base.cpp | 16 ++++++++++++++++
src/feature/feature_base.cpp | 15 +++++++++++++++
src/frame/frame_base.cpp | 15 +++++++++++++++
src/landmark/landmark_base.cpp | 15 +++++++++++++++
8 files changed, 70 insertions(+)
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index aaa76c922..a5eac842e 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 720342591..a2ba3bc60 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 3745de048..eea8873b3 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 2f80a3d6f..254e03e48 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 54e5d2a3c..d07b9b055 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 df5d1f1e7..8dc3801ef 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 34e36705f..a3c679b62 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 7c9b8b664..bc57f2729 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 >();
--
GitLab