Skip to content
Snippets Groups Projects
Commit 278fbdb7 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

hotfix of hasStateBlocks

parent 527ceb18
No related branches found
No related tags found
No related merge requests found
Pipeline #4823 passed
......@@ -90,8 +90,8 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s
void removeStateBlocks();
virtual void registerNewStateBlocks() const;
void fix();
void unfix();
virtual void fix() override;
virtual void unfix() override;
bool hasCalibration() {return calib_size_ > 0;}
SizeEigen getCalibSize() const;
......
......@@ -105,7 +105,6 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
// States
public:
void setState(const Eigen::VectorXd& _state);
bool getCovariance(Eigen::MatrixXd& _cov) const;
// Wolf tree access ---------------------------------------------------
......
......@@ -173,8 +173,10 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
bool process(const CaptureBasePtr capture_ptr);
// State blocks
void addStateBlock(const std::string& _key, const StateBlockPtr _sb_ptr);
void addStateBlock(const char _key, const StateBlockPtr _sb_ptr) { addStateBlock(std::string(1, _key), _sb_ptr); }
virtual StateBlockPtr addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr) override;
virtual StateBlockPtr addStateBlock(const char _key, const StateBlockPtr& _sb_ptr) override { return this->addStateBlock(std::string(1, _key), _sb_ptr); }
StateBlockPtr addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic);
StateBlockPtr addStateBlock(const char _key, const StateBlockPtr& _sb_ptr, const bool _dynamic) { return this->addStateBlock(std::string(1, _key), _sb_ptr, _dynamic); }
StateBlockPtr getStateBlockDynamic(const std::string& _key) const;
StateBlockPtr getStateBlockDynamic(const std::string& _key, const TimeStamp& _ts) const;
StateBlockPtr getStateBlockDynamic(const char _key) const { return getStateBlockDynamic(std::string(1,_key)); }
......@@ -277,19 +279,31 @@ inline const ProcessorBasePtrList& SensorBase::getProcessorList() const
return processor_list_;
}
inline void SensorBase::addStateBlock(const std::string& _key, const StateBlockPtr _sb_ptr)
inline StateBlockPtr SensorBase::addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr)
{
assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
HasStateBlocks::addStateBlock(_key, _sb_ptr);
setStateBlockDynamic(_key, false);
return _sb_ptr;
}
inline StateBlockPtr SensorBase::addStateBlock(const std::string& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic)
{
assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
HasStateBlocks::addStateBlock(_key, _sb_ptr);
setStateBlockDynamic(_key, _dynamic);
return _sb_ptr;
}
inline void SensorBase::setStateBlockDynamic(const std::string& _key, bool _dynamic)
{
assert(hasStateBlock(_key) and "setting dynamic an state block of a key that doesn't exist in the sensor. It is required anyway.");
state_block_dynamic_[_key] = _dynamic;
}
inline bool SensorBase::isStateBlockDynamic(const std::string& _key) const
{
assert(state_block_dynamic_.count(_key) != 0);
return state_block_dynamic_.at(_key);
}
......
......@@ -22,33 +22,28 @@ class HasStateBlocks
HasStateBlocks(const std::string& _structure) : structure_(_structure), state_block_map_() {}
virtual ~HasStateBlocks();
public:
const std::string& getStructure() const { return structure_; }
void appendToStructure(const std::string& _frame_type){ structure_ += _frame_type; }
bool isInStructure(const std::string& _sb_type) { return structure_.find(_sb_type) != std::string::npos; }
public:
const std::map<std::string, StateBlockPtr>& getStateBlockMap() const;
std::vector<StateBlockPtr> getStateBlockVec() const;
public:
// Some typical shortcuts -- not all should be coded here, see notes below.
StateBlockPtr getP() const;
StateBlockPtr getO() const;
void setP(const StateBlockPtr _p_ptr);
void setO(const StateBlockPtr _o_ptr);
public:
// These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API in derived classes of this.
void fix();
void unfix();
bool isFixed() const;
public:
StateBlockPtr addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb);
StateBlockPtr addStateBlock(const char _sb_type, const StateBlockPtr& _sb) { return addStateBlock(std::string(1,_sb_type), _sb); }
unsigned int removeStateBlock(const std::string& _sb_type);
unsigned int removeStateBlock(const char _sb_type);
virtual void fix();
virtual void unfix();
virtual bool isFixed() const;
virtual StateBlockPtr addStateBlock(const std::string& _sb_type, const StateBlockPtr& _sb);
virtual StateBlockPtr addStateBlock(const char _sb_type, const StateBlockPtr& _sb) { return addStateBlock(std::string(1,_sb_type), _sb); }
virtual unsigned int removeStateBlock(const std::string& _sb_type);
virtual unsigned int removeStateBlock(const char _sb_type);
bool hasStateBlock(const std::string& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
bool hasStateBlock(const char _sb_type) const { return hasStateBlock(std::string(1, _sb_type)); }
StateBlockPtr getStateBlock(const std::string& _sb_type) const;
......@@ -65,8 +60,7 @@ class HasStateBlocks
inline StateBlockPtr emplaceStateBlock(const std::string& _sb_type, Args&&... _args_of_base_state_block_constructor);
// States
public:
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;
void getState(Eigen::VectorXd& _state) const;
unsigned int getSize() const;
......
......@@ -23,31 +23,29 @@ CaptureBase::CaptureBase(const std::string& _type,
{
if (_sensor_ptr)
{
if (_sensor_ptr->isStateBlockDynamic("P"))
if (_sensor_ptr->getP() != nullptr and _sensor_ptr->isStateBlockDynamic("P"))
{
assert(_p_ptr && "Pointer to dynamic position params is null!");
addStateBlock("P", _p_ptr);
}
else
assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static");
assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static or doesn't exist");
if (_sensor_ptr->isStateBlockDynamic("O"))
if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic("O"))
{
assert(_o_ptr && "Pointer to dynamic orientation params is null!");
addStateBlock("O", _o_ptr);
}
else
assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static");
assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static or doesn't exist");
if (_sensor_ptr->isStateBlockDynamic("I"))
if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic("I"))
{
assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
addStateBlock("I", _intr_ptr);
}
else
assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static");
assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static or don't exist");
}
else if (_p_ptr || _o_ptr || _intr_ptr)
......@@ -144,7 +142,7 @@ StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
{
if (getSensor())
{
if (getSensor()->isStateBlockDynamic(_key))
if (getSensor()->getO() and getSensor()->isStateBlockDynamic(_key))
return HasStateBlocks::getStateBlock(_key);
else
return getSensor()->getStateBlock(_key);
......
......@@ -205,11 +205,6 @@ void FrameBase::setAux()
}
}
void FrameBase::setState(const Eigen::VectorXd& _state)
{
HasStateBlocks::setState(_state, isKeyOrAux());
}
bool FrameBase::getCovariance(Eigen::MatrixXd& _cov) const
{
return getProblem()->getFrameCovariance(shared_from_this(), _cov);
......
......@@ -25,26 +25,21 @@ SensorBase::SensorBase(const std::string& _type,
noise_std_(_noise_size),
noise_cov_(_noise_size, _noise_size)
{
assert((_p_ptr or not _p_dyn) and "Trying to set dynamic position state block without providing a position state block. It is required anyway.");
assert((_o_ptr or not _o_dyn) and "Trying to set dynamic orientation state block without providing an orientation state block. It is required anyway.");
assert((_intr_ptr or not _intr_dyn) and "Trying to set dynamic intrinsics state block without providing an intrinsics state block. It is required anyway.");
noise_std_.setZero();
noise_cov_.setZero();
if (_p_ptr)
{
addStateBlock("P", _p_ptr);
}
setStateBlockDynamic("P", _p_dyn);
addStateBlock("P", _p_ptr, _p_dyn);
if (_o_ptr)
{
addStateBlock("O", _o_ptr);
}
setStateBlockDynamic("O", _o_dyn);
addStateBlock("O", _o_ptr, _o_dyn);
if (_intr_ptr)
{
addStateBlock("I", _intr_ptr);
}
setStateBlockDynamic("I", _intr_dyn);
addStateBlock("I", _intr_ptr, _intr_dyn);
updateCalibSize();
}
......@@ -68,22 +63,13 @@ SensorBase::SensorBase(const std::string& _type,
setNoiseStd(_noise_std);
if (_p_ptr)
{
addStateBlock("P", _p_ptr);
}
setStateBlockDynamic("P", _p_dyn);
addStateBlock("P", _p_ptr, _p_dyn);
if (_o_ptr)
{
addStateBlock("O", _o_ptr);
}
setStateBlockDynamic("O", _o_dyn);
addStateBlock("O", _o_ptr, _o_dyn);
if (_intr_ptr)
{
addStateBlock("I", _intr_ptr);
}
setStateBlockDynamic("I", _intr_dyn);
addStateBlock("I", _intr_ptr, _intr_dyn);
updateCalibSize();
}
......@@ -403,7 +389,7 @@ StateBlockPtr SensorBase::getStateBlockDynamic(const std::string& _key, const Ti
bool SensorBase::isStateBlockInCapture(const std::string& _key, CaptureBasePtr& _cap) const
{
if (isStateBlockDynamic(_key))
if (state_block_dynamic_.count(_key) != 0 and isStateBlockDynamic(_key))
{
_cap = lastCapture();
// cap = lastKeyCapture();
......
......@@ -59,7 +59,7 @@ TEST(CaptureBase, Dynamic_sensor_params)
StateBlockPtr p(std::make_shared<StateBlock>(2));
StateBlockPtr o(std::make_shared<StateAngle>() );
StateBlockPtr i(std::make_shared<StateBlock>(2));
SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2, true, true, true)); // last 3 'true' mark dynamic
SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", std::make_shared<StateBlock>(2), std::make_shared<StateAngle>(), std::make_shared<StateBlock>(2), 2, true, true, true)); // last 3 'true' mark dynamic
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5
// query sensor blocks -- need KFs to find some Capture with the params
......
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