diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index a8d5d971fb728f56d5fbf2704a1aa37c86621651..08751287b5109a81160c3e70aadc4ae9d6f33253 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -19,6 +19,7 @@ namespace wolf{
//class CaptureBase
class CaptureBase : public NodeBase, public std::enable_shared_from_this<CaptureBase>
{
+ friend FeatureBase;
friend FactorBase;
private:
@@ -71,9 +72,12 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
virtual void setSensor(const SensorBasePtr sensor_ptr);
// constrained by
+ protected:
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
+ virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
+ public:
unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
+ const FactorBasePtrList& getConstrainedByList() const;
// State blocks
const std::vector<StateBlockPtr>& getStateBlockVec() const;
@@ -199,7 +203,7 @@ inline unsigned int CaptureBase::getHits() const
return constrained_by_list_.size();
}
-inline FactorBasePtrList& CaptureBase::getConstrainedByList()
+inline const FactorBasePtrList& CaptureBase::getConstrainedByList() const
{
return constrained_by_list_;
}
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 884f622a8c00a55d20d6cc2022cb90f2f795ec3f..796579ce398961dd8069b425d063e678855731ca 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -142,23 +142,25 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Returns a pointer to the frame constrained to
**/
FrameBasePtr getFrameOther() const { return frame_other_ptr_.lock(); }
- void setFrameOther(FrameBasePtr _frm_o) { frame_other_ptr_ = _frm_o; }
- /** \brief Returns a pointer to the frame constrained to
+ /** \brief Returns a pointer to the capture constrained to
**/
CaptureBasePtr getCaptureOther() const { return capture_other_ptr_.lock(); }
- void setCaptureOther(CaptureBasePtr _cap_o) { capture_other_ptr_ = _cap_o; }
/** \brief Returns a pointer to the feature constrained to
**/
FeatureBasePtr getFeatureOther() const { return feature_other_ptr_.lock(); }
- void setFeatureOther(FeatureBasePtr _ftr_o) { feature_other_ptr_ = _ftr_o; }
/** \brief Returns a pointer to the landmark constrained to
**/
LandmarkBasePtr getLandmarkOther() const { return landmark_other_ptr_.lock(); }
- void setLandmarkOther(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; }
+// void setFrameOther(FrameBasePtr _frm_o) { frame_other_ptr_ = _frm_o; }
+// void setCaptureOther(CaptureBasePtr _cap_o) { capture_other_ptr_ = _cap_o; }
+// void setFeatureOther(FeatureBasePtr _ftr_o) { feature_other_ptr_ = _ftr_o; }
+// void setLandmarkOther(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; }
+
+ public:
/**
* @brief getProcessor
* @return
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 69d4d4507781106c3b035d485a5b18e1b6b41655..c3a709fd563cafed50de1a1bb393cef1ec36a9e5 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -89,9 +89,8 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
FactorBasePtrList& getFactorList();
- virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
+ const FactorBasePtrList& getConstrainedByList() const;
// all factors
void getFactorList(FactorBasePtrList & _fac_list);
@@ -105,6 +104,8 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
FactorBasePtr addFactor(FactorBasePtr _co_ptr);
+ virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
+ virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
};
}
@@ -128,7 +129,7 @@ inline unsigned int FeatureBase::getHits() const
return constrained_by_list_.size();
}
-inline FactorBasePtrList& FeatureBase::getConstrainedByList()
+inline const FactorBasePtrList& FeatureBase::getConstrainedByList() const
{
return constrained_by_list_;
}
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index b713fb93b93f3ed68c8c543f5b19b4abf9d8a23f..98f50bb773ae2edc949510483b73a09c0aac60d8 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -145,9 +145,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type);
void getFactorList(FactorBasePtrList& _fac_list);
- virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
+ const FactorBasePtrList& getConstrainedByList() const;
void link(TrajectoryBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<FrameBase> emplace(TrajectoryBasePtr _ptr, T&&... all);
@@ -165,6 +164,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
static FrameBasePtr create_POV_3D(const FrameType & _tp,
const TimeStamp& _ts,
const Eigen::VectorXs& _x = Eigen::VectorXs::Zero(10));
+ virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
+ virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
};
} // namespace wolf
@@ -294,7 +295,7 @@ inline void FrameBase::setProblem(ProblemPtr _problem)
cap->setProblem(_problem);
}
-inline FactorBasePtrList& FrameBase::getConstrainedByList()
+inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
{
return constrained_by_list_;
}
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index a772f69dd6c250d35757d17bbea0b4c018312dac..5c27689da7891d8e031e20f4b038a173ceefcd9b 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -22,6 +22,8 @@ namespace wolf {
//class LandmarkBase
class LandmarkBase : public NodeBase, public std::enable_shared_from_this<LandmarkBase>
{
+ friend FactorBase;
+
private:
MapBaseWPtr map_ptr_;
FactorBasePtrList constrained_by_list_;
@@ -86,9 +88,8 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
// Navigate wolf tree
virtual void setProblem(ProblemPtr _problem) final;
- FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
- FactorBasePtrList& getConstrainedByList();
+ const FactorBasePtrList& getConstrainedByList() const;
void setMap(const MapBasePtr _map_ptr);
MapBasePtr getMap();
@@ -96,6 +97,10 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
template<typename classType, typename... T>
static std::shared_ptr<LandmarkBase> emplace(MapBasePtr _map_ptr, T&&... all);
+ protected:
+
+ virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
+ virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
};
}
@@ -145,7 +150,7 @@ inline unsigned int LandmarkBase::getHits() const
return constrained_by_list_.size();
}
-inline FactorBasePtrList& LandmarkBase::getConstrainedByList()
+inline const FactorBasePtrList& LandmarkBase::getConstrainedByList() const
{
return constrained_by_list_;
}
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index d95ef15d9d37e50f7a9b7cdf53e8baf5ad67c73a..5aa5ea3662707d42f7f34b4e85b0919564e39b3d 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -118,6 +118,11 @@ FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
return _fac_ptr;
}
+void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
+{
+ constrained_by_list_.remove(_fac_ptr);
+}
+
StateBlockPtr CaptureBase::getStateBlock(unsigned int _i) const
{
if (getSensor())
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 44e1826d0c01432018fd83366e0b54b8233a54f6..d11c251c8038d9ec24ba7bfa51e61218a09a9384 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -64,7 +64,7 @@ void FactorBase::remove()
FrameBasePtr frm_o = frame_other_ptr_.lock();
if (frm_o)
{
- frm_o->getConstrainedByList().remove(shared_from_this());
+ frm_o->removeConstrainedBy(shared_from_this());
if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
frm_o->remove();
}
@@ -72,7 +72,7 @@ void FactorBase::remove()
CaptureBasePtr cap_o = capture_other_ptr_.lock();
if (cap_o)
{
- cap_o->getConstrainedByList().remove(shared_from_this());
+ cap_o->removeConstrainedBy(shared_from_this());
if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
cap_o->remove();
}
@@ -80,7 +80,7 @@ void FactorBase::remove()
FeatureBasePtr ftr_o = feature_other_ptr_.lock();
if (ftr_o)
{
- ftr_o->getConstrainedByList().remove(shared_from_this());
+ ftr_o->removeConstrainedBy(shared_from_this());
if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
ftr_o->remove();
}
@@ -88,7 +88,7 @@ void FactorBase::remove()
LandmarkBasePtr lmk_o = landmark_other_ptr_.lock();
if (lmk_o)
{
- lmk_o->getConstrainedByList().remove(shared_from_this());
+ lmk_o->removeConstrainedBy(shared_from_this());
if (lmk_o->getConstrainedByList().empty())
lmk_o->remove();
}
@@ -174,4 +174,42 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
auto landmark_other = this->landmark_other_ptr_.lock();
if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this());
}
+
+//void FactorBase::setFrameOther(FrameBasePtr _frm_o)
+//{
+// if (_frm_o)
+// {
+// frame_other_ptr_ = _frm_o;
+// frame_other_ptr_->addConstrainedBy(shared_from_this());
+// }
+//}
+//
+//void FactorBase::setCaptureOther(CaptureBasePtr _cap_o)
+//{
+// if (_cap_o)
+// {
+// capture_other_ptr_ = _cap_o;
+// frame_other_ptr_->addConstrainedBy(shared_from_this());
+// }
+//}
+//
+//void FactorBase::setFeatureOther(FeatureBasePtr _ftr_o)
+//{
+// if (_ftr_o)
+// {
+// feature_other_ptr_ = _ftr_o;
+// frame_other_ptr_->addConstrainedBy(shared_from_this());
+// }
+//}
+//
+//void FactorBase::setLandmarkOther(LandmarkBasePtr _lmk_o)
+//{
+// if (_lmk_o)
+// {
+// landmark_other_ptr_ = _lmk_o;
+// frame_other_ptr_->addConstrainedBy(shared_from_this());
+// }
+//}
+
+
} // namespace wolf
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index dd86c7c8cb7eaa911baa731d67a413d76b3df1b4..26b325da868670ac0c8849998d61a7fbcb678d02 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -78,10 +78,14 @@ FrameBasePtr FeatureBase::getFrame() const
FactorBasePtr FeatureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setFeatureOther(shared_from_this());
return _fac_ptr;
}
+void FeatureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
+{
+ constrained_by_list_.remove(_fac_ptr);
+}
+
FactorBasePtrList& FeatureBase::getFactorList()
{
return factor_list_;
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 49c5ae4f2d9474d34be5b83378f5b2517507cd62..7fd6bf365bcbbcd42ba9ff39f8ea0da3cccbaed6 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -379,10 +379,14 @@ void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setFrameOther(shared_from_this());
return _fac_ptr;
}
+void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
+{
+ constrained_by_list_.remove(_fac_ptr);
+}
+
FrameBasePtr FrameBase::create_PO_2D(const FrameType & _tp,
const TimeStamp& _ts,
const Eigen::VectorXs& _x)
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 5bdfff041cc7a9bf8b2020b779736e61fac97da1..b810b936202031d435ebf2d5fcd9c06b22741ee0 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -188,8 +188,12 @@ void LandmarkBase::link(MapBasePtr _map_ptr)
FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setLandmarkOther(shared_from_this());
return _fac_ptr;
}
+void LandmarkBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
+{
+ constrained_by_list_.remove(_fac_ptr);
+}
+
} // namespace wolf