From c4f664b352dc1e0ffbd0b1ad0c512d081d3e5187 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Mon, 27 May 2019 11:19:47 +0200
Subject: [PATCH] registering functions protected and moved to viral setProblem
---
include/core/capture/capture_base.h | 7 -------
include/core/factor/factor_base.h | 1 +
include/core/feature/feature_base.h | 2 +-
include/core/frame/frame_base.h | 8 +-------
include/core/landmark/landmark_base.h | 6 +-----
include/core/problem/problem.h | 2 --
include/core/sensor/sensor_base.h | 7 +++++--
src/capture/capture_base.cpp | 13 ++++++++++++-
src/factor/factor_base.cpp | 23 +++++++++++++---------
src/frame/frame_base.cpp | 20 +++++++++++++++----
src/landmark/landmark_base.cpp | 9 ++++++++-
src/processor/processor_base.cpp | 28 ++++++++++++++++-----------
src/sensor/sensor_base.cpp | 6 ++++++
13 files changed, 82 insertions(+), 50 deletions(-)
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index bdec47c6e..7f8554f2b 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -136,13 +136,6 @@ inline SizeEigen CaptureBase::getCalibSize() const
return calib_size_;
}
-inline void CaptureBase::setProblem(ProblemPtr _problem)
-{
- NodeBase::setProblem(_problem);
- for (auto ft : feature_list_)
- ft->setProblem(_problem);
-}
-
inline void CaptureBase::updateCalibSize()
{
calib_size_ = computeCalibSize();
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index b4ea5b7c7..9baf4488a 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -173,6 +173,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
void setProcessor(const ProcessorBasePtr& _processor_ptr);
void link(FeatureBasePtr);
+ virtual void setProblem(ProblemPtr) final;
template<typename classType, typename... T>
static std::shared_ptr<FactorBase> emplace(FeatureBasePtr _oth_ptr, T&&... all);
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 5e4b6cc9d..804ff4491 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -94,7 +94,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
// all factors
void getFactorList(FactorBasePtrList & _fac_list);
- void link(CaptureBasePtr);
+ void link(CaptureBasePtr cap_ptr);
template<typename classType, typename... T>
static std::shared_ptr<FeatureBase> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index d6ec479ad..356664ebc 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -108,6 +108,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
void setP(const StateBlockPtr _p_ptr);
void setO(const StateBlockPtr _o_ptr);
void setV(const StateBlockPtr _v_ptr);
+ protected:
void registerNewStateBlocks();
void removeStateBlocks();
@@ -288,13 +289,6 @@ inline unsigned int FrameBase::getHits() const
return constrained_by_list_.size();
}
-inline void FrameBase::setProblem(ProblemPtr _problem)
-{
- NodeBase::setProblem(_problem);
- for (auto cap : capture_list_)
- cap->setProblem(_problem);
-}
-
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 fc8b87c08..5670c34ab 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -71,12 +71,12 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
StateBlockPtr getO() const;
void setP(const StateBlockPtr _p_ptr);
void setO(const StateBlockPtr _o_ptr);
- virtual void registerNewStateBlocks();
Eigen::VectorXs getState() const;
void getState(Eigen::VectorXs& _state) const;
bool getCovariance(Eigen::MatrixXs& _cov) const;
protected:
+ virtual void registerNewStateBlocks();
virtual void removeStateBlocks();
// Descriptor
@@ -118,10 +118,6 @@ std::shared_ptr<LandmarkBase> LandmarkBase::emplace(MapBasePtr _map_ptr, T&&...
lmk->link(_map_ptr);
return lmk;
}
-inline void LandmarkBase::setProblem(ProblemPtr _problem)
-{
- NodeBase::setProblem(_problem);
-}
inline MapBasePtr LandmarkBase::getMap()
{
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index ba7e88eb6..bc0a3966e 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -272,8 +272,6 @@ class Problem : public std::enable_shared_from_this<Problem>
// Map branch -----------------------------------------
MapBasePtr getMap();
- LandmarkBasePtr addLandmark(LandmarkBasePtr _lmk_ptr);
- void addLandmarkList(LandmarkBasePtrList& _lmk_list);
void loadMap(const std::string& _filename_dot_yaml);
void saveMap(const std::string& _filename_dot_yaml, //
const std::string& _map_name = "Map automatically saved by Wolf");
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 010d3fb78..277abfc1e 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -134,7 +134,12 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
void setP(const StateBlockPtr _p_ptr);
void setO(const StateBlockPtr _o_ptr);
void setIntrinsic(const StateBlockPtr _intr_ptr);
+
+ protected:
void removeStateBlocks();
+ virtual void registerNewStateBlocks();
+
+ public:
void fix();
void unfix();
@@ -172,8 +177,6 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
SizeEigen getCalibSize() const;
Eigen::VectorXs getCalibration() const;
- virtual void registerNewStateBlocks();
-
bool isExtrinsicDynamic() const;
bool isIntrinsicDynamic() const;
bool hasCapture() const {return has_capture_;}
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index f531a781e..a0abe0bd3 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -296,7 +296,6 @@ void CaptureBase::link(FrameBasePtr _frm_ptr)
_frm_ptr->addCapture(shared_from_this());
this->setFrame(_frm_ptr);
this->setProblem(_frm_ptr->getProblem());
- this->registerNewStateBlocks();
}
else
{
@@ -304,5 +303,17 @@ void CaptureBase::link(FrameBasePtr _frm_ptr)
}
}
+void CaptureBase::setProblem(ProblemPtr _problem)
+{
+ if (_problem == nullptr || _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+ this->registerNewStateBlocks();
+
+ for (auto ft : feature_list_)
+ ft->setProblem(_problem);
+}
+
} // namespace wolf
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index d11c251c8..7d406f29d 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -153,18 +153,14 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
this->setFeature(_ftr_ptr);
this->setProblem(_ftr_ptr->getProblem());
// add factor to be added in solver
- if (this->getProblem() != nullptr)
- {
- if (this->getStatus() == FAC_ACTIVE)
- this->getProblem()->addFactor(shared_from_this());
- }
- else
- WOLF_WARN("ADDING CONSTRAINT ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " NOT CONNECTED WITH PROBLEM.");
+ if (this->getProblem() == nullptr)
+ {
+ WOLF_WARN("ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " NOT CONNECTED WITH PROBLEM.");
+ }
}
else
- {
WOLF_WARN("Linking with nullptr");
- }
+
auto frame_other = this->frame_other_ptr_.lock();
if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this());
auto capture_other = this->capture_other_ptr_.lock();
@@ -175,6 +171,15 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this());
}
+void FactorBase::setProblem(ProblemPtr _problem)
+{
+ if (_problem == nullptr || _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+ if (this->getStatus() == FAC_ACTIVE)
+ this->getProblem()->notifyFactor(shared_from_this(),ADD);
+}
//void FactorBase::setFrameOther(FrameBasePtr _frm_o)
//{
// if (_frm_o)
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 789c93ec9..0aa7712d3 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -425,11 +425,11 @@ FrameBasePtr FrameBase::create_POV_3D(const FrameType & _tp,
void FrameBase::link(TrajectoryBasePtr _ptr)
{
if(_ptr)
+ if(_trj_ptr)
{
- _ptr->addFrame(shared_from_this());
- this->setTrajectory(_ptr);
- this->setProblem(_ptr->getProblem());
- if (this->isKey()) this->registerNewStateBlocks();
+ _trj_ptr->addFrame(shared_from_this());
+ this->setTrajectory(_trj_ptr);
+ this->setProblem(_trj_ptr->getProblem());
}
else
{
@@ -444,4 +444,16 @@ namespace wolf
namespace{ const bool WOLF_UNUSED Frame_PO_2D_Registered = FrameFactory::get().registerCreator("PO 2D", FrameBase::create_PO_2D ); }
namespace{ const bool WOLF_UNUSED Frame_PO_3D_Registered = FrameFactory::get().registerCreator("PO 3D", FrameBase::create_PO_3D ); }
namespace{ const bool WOLF_UNUSED Frame_POV_3D_Registered = FrameFactory::get().registerCreator("POV 3D", FrameBase::create_POV_3D); }
+void FrameBase::setProblem(ProblemPtr _problem)
+{
+ if (_problem == nullptr || _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+ if (this->isKey())
+ this->registerNewStateBlocks();
+
+ for (auto cap : capture_list_)
+ cap->setProblem(_problem);
+}
} // namespace wolf
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index b810b9362..6b964b657 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -177,7 +177,6 @@ void LandmarkBase::link(MapBasePtr _map_ptr)
this->setMap(_map_ptr);
_map_ptr->addLandmark(shared_from_this());
this->setProblem(_map_ptr->getProblem());
- this->registerNewStateBlocks();
}
else
{
@@ -185,6 +184,14 @@ void LandmarkBase::link(MapBasePtr _map_ptr)
}
}
+void LandmarkBase::setProblem(ProblemPtr _problem)
+{
+ if (_problem == nullptr || _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+ this->registerNewStateBlocks();
+}
FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 07da090c9..ba7d2fdf0 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -78,19 +78,25 @@ void ProcessorBase::remove()
sen->getProcessorList().remove(this_p);
}
}
- void ProcessorBase::link(SensorBasePtr _sen_ptr)
+
+void ProcessorBase::link(SensorBasePtr _sen_ptr)
+{
+ if(this->getSensor() && this->getSensor() != _sen_ptr)
{
- if(_sen_ptr)
- {
- _sen_ptr->addProcessor(shared_from_this());
- this->setSensor(_sen_ptr);
- this->setProblem(_sen_ptr->getProblem());
- }
- else
- {
- WOLF_WARN("Linking with a nullptr");
- }
+ WOLF_WARN("Trying to link an already linked processor with a different sensor. Skipping");
+ return;
}
+ if(_sen_ptr)
+ {
+ _sen_ptr->addProcessor(shared_from_this());
+ this->setSensor(_sen_ptr);
+ this->setProblem(_sen_ptr->getProblem());
+ }
+ else
+ {
+ WOLF_WARN("Linking with a nullptr");
+ }
+}
/////////////////////////////////////////////////////////////////////////////////////////
void PackKeyFrameBuffer::removeUpTo(const TimeStamp& _time_stamp)
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 4986a3521..efe4cad6f 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -412,6 +412,12 @@ void SensorBase::setProblem(ProblemPtr _problem)
void SensorBase::link(HardwareBasePtr _hw_ptr)
{
std::cout << "Linking SensorBase" << std::endl;
+ if(this->getHardware() && this->getHardware() != _hw_ptr)
+ {
+ WOLF_WARN("Trying to link an already linked sensor with a different hardware. Skipping");
+ return;
+ }
+
if(_hw_ptr)
{
this->setHardware(_hw_ptr);
--
GitLab