Skip to content
Snippets Groups Projects
Commit 3431f94c authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Move registerNewStateBlocks() and removeStateBlocks() to HasStateBlocks class

parent bf0f7487
No related branches found
No related tags found
1 merge request!337Resolve "hasStateBlocks::registerStateBlocks(ProblemPtr)"
Pipeline #4910 passed
This commit is part of merge request !337. Comments created here will be created in the context of that merge request.
...@@ -304,6 +304,7 @@ SET(SRCS_FRAME ...@@ -304,6 +304,7 @@ SET(SRCS_FRAME
src/frame/frame_base.cpp src/frame/frame_base.cpp
) )
SET(SRCS_STATE_BLOCK SET(SRCS_STATE_BLOCK
src/state_block/has_state_blocks.cpp
src/state_block/local_parametrization_base.cpp src/state_block/local_parametrization_base.cpp
src/state_block/local_parametrization_homogeneous.cpp src/state_block/local_parametrization_homogeneous.cpp
src/state_block/local_parametrization_quaternion.cpp src/state_block/local_parametrization_quaternion.cpp
......
...@@ -87,8 +87,6 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s ...@@ -87,8 +87,6 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
StateBlockPtr getSensorP() const; StateBlockPtr getSensorP() const;
StateBlockPtr getSensorO() const; StateBlockPtr getSensorO() const;
StateBlockPtr getSensorIntrinsic() const; StateBlockPtr getSensorIntrinsic() const;
void removeStateBlocks();
virtual void registerNewStateBlocks() const;
virtual void fix() override; virtual void fix() override;
virtual void unfix() override; virtual void unfix() override;
......
...@@ -99,8 +99,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha ...@@ -99,8 +99,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
StateBlockPtr getV() const; StateBlockPtr getV() const;
void setV(const StateBlockPtr _v_ptr); void setV(const StateBlockPtr _v_ptr);
protected: protected:
void registerNewStateBlocks() const;
void removeStateBlocks();
virtual void setProblem(ProblemPtr _problem) final; virtual void setProblem(ProblemPtr _problem) final;
// States // States
......
...@@ -62,10 +62,6 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_ ...@@ -62,10 +62,6 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
std::vector<StateBlockPtr> getUsedStateBlockVec() const; std::vector<StateBlockPtr> getUsedStateBlockVec() const;
bool getCovariance(Eigen::MatrixXd& _cov) const; bool getCovariance(Eigen::MatrixXd& _cov) const;
protected:
virtual void registerNewStateBlocks() const;
virtual void removeStateBlocks();
// Descriptor // Descriptor
public: public:
const Eigen::VectorXd& getDescriptor() const; const Eigen::VectorXd& getDescriptor() const;
......
...@@ -59,6 +59,10 @@ class HasStateBlocks ...@@ -59,6 +59,10 @@ class HasStateBlocks
template<typename ... Args> template<typename ... Args>
inline StateBlockPtr emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor); 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 // States
virtual void setState(const Eigen::VectorXd& _state, const bool _notify = true); virtual void setState(const Eigen::VectorXd& _state, const bool _notify = true);
Eigen::VectorXd getState() const; Eigen::VectorXd getState() const;
......
...@@ -58,7 +58,7 @@ CaptureBase::CaptureBase(const std::string& _type, ...@@ -58,7 +58,7 @@ CaptureBase::CaptureBase(const std::string& _type,
CaptureBase::~CaptureBase() CaptureBase::~CaptureBase()
{ {
removeStateBlocks(); removeStateBlocks(getProblem());
} }
void CaptureBase::remove(bool viral_remove_empty_parent) void CaptureBase::remove(bool viral_remove_empty_parent)
...@@ -69,7 +69,7 @@ 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 CaptureBasePtr this_C = shared_from_this(); // keep this alive while removing it
// Remove State Blocks // Remove State Blocks
removeStateBlocks(); removeStateBlocks(getProblem());
// remove from upstream // remove from upstream
FrameBasePtr F = frame_ptr_.lock(); FrameBasePtr F = frame_ptr_.lock();
...@@ -151,22 +151,6 @@ StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const ...@@ -151,22 +151,6 @@ StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
return HasStateBlocks::getStateBlock(_key); 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() void CaptureBase::fix()
{ {
HasStateBlocks::fix(); HasStateBlocks::fix();
...@@ -179,16 +163,6 @@ void CaptureBase::unfix() ...@@ -179,16 +163,6 @@ void CaptureBase::unfix()
updateCalibSize(); 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 CaptureBase::computeCalibSize() const
{ {
SizeEigen sz = 0; SizeEigen sz = 0;
...@@ -281,7 +255,7 @@ void CaptureBase::setProblem(ProblemPtr _problem) ...@@ -281,7 +255,7 @@ void CaptureBase::setProblem(ProblemPtr _problem)
return; return;
NodeBase::setProblem(_problem); NodeBase::setProblem(_problem);
this->registerNewStateBlocks(); registerNewStateBlocks(_problem);
for (auto ft : feature_list_) for (auto ft : feature_list_)
ft->setProblem(_problem); ft->setProblem(_problem);
......
...@@ -125,7 +125,7 @@ void FrameBase::remove(bool viral_remove_empty_parent) ...@@ -125,7 +125,7 @@ void FrameBase::remove(bool viral_remove_empty_parent)
// Remove Frame State Blocks // Remove Frame State Blocks
if ( isKeyOrAux() ) if ( isKeyOrAux() )
removeStateBlocks(); removeStateBlocks(getProblem());
} }
} }
...@@ -136,37 +136,11 @@ void FrameBase::setTimeStamp(const TimeStamp& _ts) ...@@ -136,37 +136,11 @@ void FrameBase::setTimeStamp(const TimeStamp& _ts)
getTrajectory()->sortFrame(shared_from_this()); 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() void FrameBase::setNonEstimated()
{ {
// unregister if previously estimated // unregister if previously estimated
if (isKeyOrAux()) if (isKeyOrAux())
removeStateBlocks(); removeStateBlocks(getProblem());
type_ = NON_ESTIMATED; type_ = NON_ESTIMATED;
if (getTrajectory()) if (getTrajectory())
...@@ -180,7 +154,7 @@ void FrameBase::setKey() ...@@ -180,7 +154,7 @@ void FrameBase::setKey()
{ {
// register if previously not estimated // register if previously not estimated
if (!isKeyOrAux()) if (!isKeyOrAux())
registerNewStateBlocks(); registerNewStateBlocks(getProblem());
// WOLF_DEBUG("Set Key", this->id()); // WOLF_DEBUG("Set Key", this->id());
type_ = KEY; type_ = KEY;
...@@ -194,7 +168,7 @@ void FrameBase::setKey() ...@@ -194,7 +168,7 @@ void FrameBase::setKey()
void FrameBase::setAux() void FrameBase::setAux()
{ {
if (!isKeyOrAux()) if (!isKeyOrAux())
registerNewStateBlocks(); registerNewStateBlocks(getProblem());
// WOLF_DEBUG("Set Auxiliary", this->id()); // WOLF_DEBUG("Set Auxiliary", this->id());
type_ = AUXILIARY; type_ = AUXILIARY;
...@@ -347,7 +321,7 @@ void FrameBase::setProblem(ProblemPtr _problem) ...@@ -347,7 +321,7 @@ void FrameBase::setProblem(ProblemPtr _problem)
NodeBase::setProblem(_problem); NodeBase::setProblem(_problem);
if (this->isKey()) if (this->isKey())
this->registerNewStateBlocks(); registerNewStateBlocks(getProblem());
for (auto cap : capture_list_) for (auto cap : capture_list_)
cap->setProblem(_problem); cap->setProblem(_problem);
......
...@@ -32,7 +32,7 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State ...@@ -32,7 +32,7 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
LandmarkBase::~LandmarkBase() LandmarkBase::~LandmarkBase()
{ {
removeStateBlocks(); removeStateBlocks(getProblem());
} }
void LandmarkBase::remove(bool viral_remove_empty_parent) void LandmarkBase::remove(bool viral_remove_empty_parent)
...@@ -54,7 +54,7 @@ void LandmarkBase::remove(bool viral_remove_empty_parent) ...@@ -54,7 +54,7 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
} }
// Remove State Blocks // Remove State Blocks
removeStateBlocks(); removeStateBlocks(getProblem());
} }
} }
...@@ -78,37 +78,11 @@ std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec() const ...@@ -78,37 +78,11 @@ std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec() const
return used_state_block_vec; 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 bool LandmarkBase::getCovariance(Eigen::MatrixXd& _cov) const
{ {
return getProblem()->getLandmarkCovariance(shared_from_this(), _cov); 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 LandmarkBase::saveToYaml() const
{ {
YAML::Node node; YAML::Node node;
...@@ -150,7 +124,7 @@ void LandmarkBase::setProblem(ProblemPtr _problem) ...@@ -150,7 +124,7 @@ void LandmarkBase::setProblem(ProblemPtr _problem)
return; return;
NodeBase::setProblem(_problem); NodeBase::setProblem(_problem);
this->registerNewStateBlocks(); registerNewStateBlocks(_problem);
} }
FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr) FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
......
#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);
  • Shouldn't this line be in the scope of the if? If sbp is nullptr, it can't be removed.. would it crash?

    Edited by Joan Vallvé Navarro
  • Author Owner

    I just copy-pasted the code that was I think in CaptureBase. Did you look inside removeStateBlock(key)?

  • Author Owner

    It would work anyway. The second remove only affects the node, not problem. However, it makes more sense to have it inside the if, so I'll check if it works and change it.

  • Please register or sign in to reply
}
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment