diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index f314f961e2a22fa3fd80d5b92d65a01dfaa94b3d..df54c3468b1f455d9ff1032f16959d1c0d4ed58b 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -90,7 +90,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh private: HardwareBaseWPtr hardware_ptr_; ProcessorBasePtrList processor_list_; - SizeEigen calib_size_; static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory) @@ -234,8 +233,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh unsigned int _start_idx = 0, int _size = -1); - SizeEigen getCalibSize() const; - VectorComposite getCalibration() const; void getCalibration(VectorComposite& _calib) const; @@ -262,11 +259,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh template<typename classType, typename... T> static std::shared_ptr<classType> emplace(HardwareBasePtr _hwd_ptr, T&&... all); - protected: - SizeEigen computeCalibSize() const; - - private: - void updateCalibSize(); }; } @@ -354,16 +346,6 @@ inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXd& _x, const Eige addPriorParameter("I", _x, _cov); } -inline SizeEigen SensorBase::getCalibSize() const -{ - return calib_size_; -} - -inline void SensorBase::updateCalibSize() -{ - calib_size_ = computeCalibSize(); -} - } // namespace wolf #endif diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index cd5f8eeb33904a482bfb47a4c832721b98edcecf..0f48bb3fdb5cb4039946bc3d5a8712b7caa3e7ed 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -20,7 +20,6 @@ SensorBase::SensorBase(const std::string& _type, NodeBase("SENSOR", _type), HasStateBlocks(""), hardware_ptr_(), - calib_size_(0), sensor_id_(++sensor_id_count_), // simple ID factory noise_std_(_noise_size), noise_cov_(_noise_size, _noise_size) @@ -40,8 +39,6 @@ SensorBase::SensorBase(const std::string& _type, if (_intr_ptr) addStateBlock("I", _intr_ptr, _intr_dyn); - - updateCalibSize(); } SensorBase::SensorBase(const std::string& _type, @@ -55,7 +52,6 @@ SensorBase::SensorBase(const std::string& _type, NodeBase("SENSOR", _type), HasStateBlocks(""), hardware_ptr_(), - calib_size_(0), sensor_id_(++sensor_id_count_), // simple ID factory noise_std_(_noise_std), noise_cov_(_noise_std.size(), _noise_std.size()) @@ -70,8 +66,6 @@ SensorBase::SensorBase(const std::string& _type, if (_intr_ptr) addStateBlock("I", _intr_ptr, _intr_dyn); - - updateCalibSize(); } SensorBase::~SensorBase() @@ -105,7 +99,6 @@ void SensorBase::fixExtrinsics() if (sbp != nullptr) sbp->fix(); } - updateCalibSize(); } void SensorBase::unfixExtrinsics() @@ -116,7 +109,6 @@ void SensorBase::unfixExtrinsics() if (sbp != nullptr) sbp->unfix(); } - updateCalibSize(); } void SensorBase::fixIntrinsics() @@ -131,7 +123,6 @@ void SensorBase::fixIntrinsics() sbp->fix(); } } - updateCalibSize(); } void SensorBase::unfixIntrinsics() @@ -146,7 +137,6 @@ void SensorBase::unfixIntrinsics() sbp->unfix(); } } - updateCalibSize(); } void SensorBase::addPriorParameter(const std::string& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size) @@ -315,18 +305,6 @@ StateBlockPtr SensorBase::getIntrinsic() const return getStateBlockDynamic("I"); } -SizeEigen SensorBase::computeCalibSize() const -{ - SizeEigen sz = 0; - for (const auto& pair_key_sb : getStateBlockMap()) - { - auto sb = pair_key_sb.second; - if (sb && !sb->isFixed()) - sz += sb->getSize(); - } - return sz; -} - VectorComposite SensorBase::getCalibration() const { VectorComposite calib;