Merge branch '280-hasstateblocks-registerstateblocks-problemptr' into 'devel'

Resolve "hasStateBlocks::registerStateBlocks(ProblemPtr)"

Closes #280

See merge request !337

CMakeLists.txt

@@ -304,6 +304,7 @@ SET(SRCS_FRAME
   src/frame/frame_base.cpp
   )
 SET(SRCS_STATE_BLOCK
+  src/state_block/has_state_blocks.cpp
   src/state_block/local_parametrization_base.cpp
   src/state_block/local_parametrization_homogeneous.cpp
   src/state_block/local_parametrization_quaternion.cpp

include/core/capture/capture_base.h

@@ -87,8 +87,6 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
         StateBlockPtr getSensorP() const;
         StateBlockPtr getSensorO() const;
         StateBlockPtr getSensorIntrinsic() const;
-        void removeStateBlocks();
-        virtual void registerNewStateBlocks() const;
 
         virtual void fix() override;
         virtual void unfix() override;

include/core/frame/frame_base.h

@@ -99,8 +99,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
         StateBlockPtr getV() const;
         void setV(const StateBlockPtr _v_ptr);
     protected:
-        void registerNewStateBlocks() const;
-        void removeStateBlocks();
         virtual void setProblem(ProblemPtr _problem) final;
 
         // States

include/core/landmark/landmark_base.h

@@ -62,10 +62,6 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
         std::vector<StateBlockPtr> getUsedStateBlockVec() const;
         bool getCovariance(Eigen::MatrixXd& _cov) const;
 
-    protected:
-        virtual void registerNewStateBlocks() const;
-        virtual void removeStateBlocks();
-
         // Descriptor
     public:
         const Eigen::VectorXd& getDescriptor() const;

include/core/state_block/has_state_blocks.h

@@ -59,6 +59,10 @@ class HasStateBlocks
         template<typename ... Args>
         inline StateBlockPtr emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor);
 
+        // Register/remove state blocks to/from wolf::Problem
+        void registerNewStateBlocks(ProblemPtr _problem) const;
+        void removeStateBlocks(ProblemPtr _problem);
+
         // States
         virtual void setState(const Eigen::VectorXd& _state, const bool _notify = true);
         Eigen::VectorXd getState() const;

src/capture/capture_base.cpp

@@ -58,7 +58,7 @@ CaptureBase::CaptureBase(const std::string& _type,
 
 CaptureBase::~CaptureBase()
 {
-    removeStateBlocks();
+    removeStateBlocks(getProblem());
 }
 
 void CaptureBase::remove(bool viral_remove_empty_parent)
@@ -69,7 +69,7 @@ void CaptureBase::remove(bool viral_remove_empty_parent)
         CaptureBasePtr this_C = shared_from_this();  // keep this alive while removing it
 
         // Remove State Blocks
-        removeStateBlocks();
+        removeStateBlocks(getProblem());
 
         // remove from upstream
         FrameBasePtr F = frame_ptr_.lock();
@@ -151,22 +151,6 @@ StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
         return HasStateBlocks::getStateBlock(_key);
 }
 
-void CaptureBase::removeStateBlocks()
-{
-    for (const auto& key : getStructure()) // note: key is a char
-    {
-        auto sbp = HasStateBlocks::getStateBlock(key);
-        if (sbp != nullptr)
-        {
-            if (getProblem() != nullptr)
-            {
-                getProblem()->notifyStateBlock(sbp, REMOVE);
-            }
-        }
-        removeStateBlock(key);
-    }
-}
-
 void CaptureBase::fix()
 {
     HasStateBlocks::fix();
@@ -179,16 +163,6 @@ void CaptureBase::unfix()
     updateCalibSize();
 }
 
-void CaptureBase::registerNewStateBlocks() const
-{
-    if (getProblem() != nullptr)
-    {
-        for (auto pair_key_sbp : getStateBlockMap())
-            if (pair_key_sbp.second != nullptr)
-                getProblem()->notifyStateBlock(pair_key_sbp.second,ADD);
-    }
-}
-
 SizeEigen CaptureBase::computeCalibSize() const
 {
     SizeEigen sz = 0;
@@ -281,7 +255,7 @@ void CaptureBase::setProblem(ProblemPtr _problem)
         return;
 
     NodeBase::setProblem(_problem);
-    this->registerNewStateBlocks();
+    registerNewStateBlocks(_problem);
 
     for (auto ft : feature_list_)
         ft->setProblem(_problem);

src/frame/frame_base.cpp

@@ -125,7 +125,7 @@ void FrameBase::remove(bool viral_remove_empty_parent)
 
         // Remove Frame State Blocks
         if ( isKeyOrAux() )
-            removeStateBlocks();
+            removeStateBlocks(getProblem());
     }
 }
 
@@ -136,37 +136,11 @@ void FrameBase::setTimeStamp(const TimeStamp& _ts)
         getTrajectory()->sortFrame(shared_from_this());
 }
 
-void FrameBase::registerNewStateBlocks() const
-{
-    if (getProblem() != nullptr)
-    {
-        for (auto pair_key_sbp : getStateBlockMap())
-            if (pair_key_sbp.second != nullptr)
-                getProblem()->notifyStateBlock(pair_key_sbp.second,ADD);
-    }
-}
-
-void FrameBase::removeStateBlocks()
-{
-    for (const char key : getStructure()) // note: key is a char
-    {
-        auto sbp = getStateBlock(key);
-        if (sbp != nullptr)
-        {
-            if (getProblem() != nullptr)
-            {
-                getProblem()->notifyStateBlock(sbp,REMOVE);
-            }
-        }
-        removeStateBlock(key);
-    }
-}
-
 void FrameBase::setNonEstimated()
 {
     // unregister if previously estimated
     if (isKeyOrAux())
-        removeStateBlocks();
+        removeStateBlocks(getProblem());
 
     type_ = NON_ESTIMATED;
     if (getTrajectory())
@@ -180,7 +154,7 @@ void FrameBase::setKey()
 {
     // register if previously not estimated
     if (!isKeyOrAux())
-        registerNewStateBlocks();
+        registerNewStateBlocks(getProblem());
 
     // WOLF_DEBUG("Set Key", this->id());
     type_ = KEY;
@@ -194,7 +168,7 @@ void FrameBase::setKey()
 void FrameBase::setAux()
 {
     if (!isKeyOrAux())
-        registerNewStateBlocks();
+        registerNewStateBlocks(getProblem());
 
     // WOLF_DEBUG("Set Auxiliary", this->id());
     type_ = AUXILIARY;
@@ -347,7 +321,7 @@ void FrameBase::setProblem(ProblemPtr _problem)
 
     NodeBase::setProblem(_problem);
     if (this->isKey())
-        this->registerNewStateBlocks();
+        registerNewStateBlocks(getProblem());
 
     for (auto cap : capture_list_)
         cap->setProblem(_problem);

src/landmark/landmark_base.cpp

@@ -32,7 +32,7 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
 
 LandmarkBase::~LandmarkBase()
 {
-    removeStateBlocks();
+    removeStateBlocks(getProblem());
 }
 
 void LandmarkBase::remove(bool viral_remove_empty_parent)
@@ -54,7 +54,7 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
         }
 
         // Remove State Blocks
-        removeStateBlocks();
+        removeStateBlocks(getProblem());
     }
 }
 
@@ -78,37 +78,11 @@ std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec() const
     return used_state_block_vec;
 }
 
-void LandmarkBase::registerNewStateBlocks() const
-{
-    if (getProblem() != nullptr)
-    {
-        for (auto pair_key_sbp : getStateBlockMap())
-            if (pair_key_sbp.second != nullptr)
-                getProblem()->notifyStateBlock(pair_key_sbp.second,ADD);
-    }
-}
-
 bool LandmarkBase::getCovariance(Eigen::MatrixXd& _cov) const
 {
     return getProblem()->getLandmarkCovariance(shared_from_this(), _cov);
 }
 
-void LandmarkBase::removeStateBlocks()
-{
-    for (const char key : getStructure()) // note: key is a char
-    {
-        auto sbp = getStateBlock(key);
-        if (sbp != nullptr)
-        {
-            if (getProblem() != nullptr)
-            {
-                getProblem()->notifyStateBlock(sbp,REMOVE);
-            }
-        }
-        removeStateBlock(key);
-    }
-}
-
 YAML::Node LandmarkBase::saveToYaml() const
 {
     YAML::Node node;
@@ -150,7 +124,7 @@ void LandmarkBase::setProblem(ProblemPtr _problem)
         return;
 
     NodeBase::setProblem(_problem);
-    this->registerNewStateBlocks();
+    registerNewStateBlocks(_problem);
 }
 
 FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)

src/state_block/has_state_blocks.cpp

+
+#include "core/state_block/has_state_blocks.h"
+
+namespace wolf
+{
+
+void HasStateBlocks::registerNewStateBlocks(ProblemPtr _problem) const
+{
+    if (_problem != nullptr)
+    {
+        for (auto pair_key_sbp : getStateBlockMap())
+            if (pair_key_sbp.second != nullptr)
+                _problem->notifyStateBlock(pair_key_sbp.second, ADD);
+    }
+}
+
+void HasStateBlocks::removeStateBlocks(ProblemPtr _problem)
+{
+    for (const char key : getStructure()) // note: key is a char
+    {
+        auto sbp = getStateBlock(key);
+        if (sbp != nullptr)
+        {
+            if (_problem != nullptr)
+            {
+                _problem->notifyStateBlock(sbp,REMOVE);
+            }
+        }
+        removeStateBlock(key);
+    }
+}
+
+
+}