From 278fbdb75499f58e107a2570654222e73ffab02d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 5 Feb 2020 16:38:07 +0100
Subject: [PATCH] hotfix of hasStateBlocks
---
include/core/capture/capture_base.h | 4 +--
include/core/frame/frame_base.h | 1 -
include/core/sensor/sensor_base.h | 20 ++++++++++--
include/core/state_block/has_state_blocks.h | 24 ++++++--------
src/capture/capture_base.cpp | 16 ++++-----
src/frame/frame_base.cpp | 5 ---
src/sensor/sensor_base.cpp | 36 +++++++--------------
test/gtest_capture_base.cpp | 2 +-
8 files changed, 47 insertions(+), 61 deletions(-)
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 1e0006d70..172ccff4c 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 b5cfcc42f..eb4e6cd3a 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 43f82f100..ab90bb26d 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 f2c7cf227..c5938cef8 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 e971b00ab..dd4e84860 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 d293a8727..8fab1f81f 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 05755041d..3848f77c9 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 45d996770..f0c1cbfcb 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
--
GitLab