diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index bdec47c6e898b15c6464a050e1819fc04c42743d..7f8554f2b9a6dd39b6d26309917775066f47fc22 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 b4ea5b7c7d51b306e7c1aa0408419984ad476e75..9baf4488a74026363f55871988318c2a64cda3df 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 5e4b6cc9d45367c6a611ff75436bfc764a29bdb8..804ff449117ae2f3a7bfd5f8bfa2355c3b1886f9 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 d6ec479adc0dab8505023add9fc2c9f4b41b1b2b..356664ebcb2e88b4a0e854df94559e38770b4e08 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 fc8b87c08b5a7dc6802ee6c48dadf65820b76eee..5670c34ab9a211c8a4510262352fee0a5bcb1be5 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 ba7e88eb69aecd960055959206ca9a407ee312d4..bc0a3966e06890a48ae7030ad0343738f02f064e 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 010d3fb78b1674a5407319d2932d4871ad8c4521..277abfc1ee9e6d7aefd5883785e376e6bddeee58 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 f531a781e5b2e3ffeda42966b1e1382d5e5c6d79..a0abe0bd39accb53ecc469beb567896694048034 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 d11c251c8038d9ec24ba7bfa51e61218a09a9384..7d406f29d580afab693012998fbfd6d1a252a837 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 789c93ec962000c611716d041b4639d075e0c5f8..0aa7712d3c3abead1fbf0d2df423e72c97ade501 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 b810b936202031d435ebf2d5fcd9c06b22741ee0..6b964b6575c4557a16aaa2521023a5b4dd7236ae 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 07da090c9b6e789f3d942a8aef226665927972cf..ba7d2fdf003a667a5351b64d8b282bfa9556688c 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 4986a3521f82f2e42fb1a627acb572e651d227f8..efe4cad6f9de9ba83fa1918a00e265f343c63279 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);