diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9550fa132cdda3faaa839b8d2cd0045d53ec0f7a..3e9f448b29f414133134d03a475ec96eaac1872b 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 172ccff4c9986851ca7760afde526b04bbc1e3f7..aaa76c9220c4351f1cca5b6a38bf290ef8e69c58 100644
--- a/include/core/capture/capture_base.h
+++ b/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;
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index eb4e6cd3a038a9f9d3ee934aa4cc9fd3d61ad3f5..14bcc2139d66765f8a3c78f2358d2c4606b91221 100644
--- a/include/core/frame/frame_base.h
+++ b/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
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index f157a961b99d3eda1450cb8d269ad295c5a57d03..2f80a3d6fbe26d171126a82d280e65a3acaf4349 100644
--- a/include/core/landmark/landmark_base.h
+++ b/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;
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index c5938cef8777b47daea6f3205fde75554915288b..ade367fb88e7e0e987b087d31130dee0e78736ff 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/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;
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index d566ba5abad8ac6c7848e347babb434876653284..6e78e69a888482381e870a0a00fef5d1aeaf2514 100644
--- a/src/capture/capture_base.cpp
+++ b/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);
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 974b72e9012621b1db494b1755925782c452552f..ca5918624d01bb1d0d23481d4685f9d4006dd028 100644
--- a/src/frame/frame_base.cpp
+++ b/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);
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index da4cb5a57ac6ef8e003f3a5a37d790c0789c0db9..1e24627be754f7bebdf9cf2945fa0fb6bc502bee 100644
--- a/src/landmark/landmark_base.cpp
+++ b/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)
diff --git a/src/state_block/has_state_blocks.cpp b/src/state_block/has_state_blocks.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..651f22f2fa2cc3d22c1ef4bd2536ff507468d048
--- /dev/null
+++ b/src/state_block/has_state_blocks.cpp
@@ -0,0 +1,34 @@
+
+#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);
+ }
+}
+
+
+}