diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h index 1e0006d70660ae35a665bb921ca35eeba366b2e3..172ccff4c9986851ca7760afde526b04bbc1e3f7 100644 --- a/include/core/capture/capture_base.h +++ b/include/core/capture/capture_base.h @@ -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; diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h index b5cfcc42fab8f1da75b18f26fbacb8dff4968055..eb4e6cd3a038a9f9d3ee934aa4cc9fd3d61ad3f5 100644 --- a/include/core/frame/frame_base.h +++ b/include/core/frame/frame_base.h @@ -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 --------------------------------------------------- diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 43f82f10036a364061c681e79e38c598e88dceb4..ab90bb26d654e76905e47a9b57effd6be4b69e54 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -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); } diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h index f2c7cf227c3ca2b308731a5c2d2a257cb2de6910..c5938cef8777b47daea6f3205fde75554915288b 100644 --- a/include/core/state_block/has_state_blocks.h +++ b/include/core/state_block/has_state_blocks.h @@ -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; diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index e971b00ab87f5dbd2aa31efae66c434e61d6dbde..dd4e848601004573fabd94b328be05d728826cd9 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -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); diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index d293a8727cda2b24875b41bacadde96112f722c6..8fab1f81f8cebc9832bad1ad1d5987a34747ee4c 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -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); diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 05755041df8b92c7e53895b8ed2732d0739b37ce..3848f77c96d2fdbd3db5197dd35195342dd95474 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -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(); diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp index 45d9967706d9eca9d04d9b7f15b806f8fa0cf4bd..f0c1cbfcb2cee9f676700846dbb93fe0cd9e3521 100644 --- a/test/gtest_capture_base.cpp +++ b/test/gtest_capture_base.cpp @@ -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