diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 427e74dc7c259d3abd3147fe5cf13608740d3472..7203ff3fd48833c5be92de7445dc174193e6a3e1 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -60,7 +60,7 @@ class ProcessorDiffDrive : public ProcessorOdom2d
FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) override;
- VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const override;
void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
protected:
@@ -71,7 +71,10 @@ class ProcessorDiffDrive : public ProcessorOdom2d
inline Eigen::VectorXd ProcessorDiffDrive::getCalibration (const CaptureBasePtr _capture) const
{
- return _capture->getStateBlock('I')->getState();
+ if (_capture)
+ return _capture->getStateBlock('I')->getState();
+ else
+ return getSensor()->getStateBlockDynamic('I')->getState();
}
inline void ProcessorDiffDrive::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index c621f61c3ac0c09943a7f58789763c4f14b8e115..75053ab0f35d1c338092c723573d5cfbf85755b6 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -463,7 +463,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
public:
- virtual VectorXd getCalibration (const CaptureBasePtr _capture) const = 0;
+ virtual VectorXd getCalibration (const CaptureBasePtr _capture = nullptr) const = 0;
bool hasCalibration() const {return calib_size_ > 0;}
//getters
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 4c27ecb6e45bcd2ff44ea5a20d22705d6c024b6d..f4cae62c31f2c8389107388bdebedbec72a68b5b 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,9 +233,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
unsigned int _start_idx = 0,
int _size = -1);
- SizeEigen getCalibSize() const;
- Eigen::VectorXd getCalibration() const;
-
void setNoiseStd(const Eigen::VectorXd & _noise_std);
void setNoiseCov(const Eigen::MatrixXd & _noise_std);
Eigen::VectorXd getNoiseStd() const;
@@ -261,12 +257,6 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
void link(HardwareBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
-
- protected:
- SizeEigen computeCalibSize() const;
-
- private:
- void updateCalibSize();
};
}
@@ -356,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/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index c6fda0b6468a76644dfdf165240fdfdcf07949ba..406137e26a1a12897041f79067cb8862f6d65876 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -686,8 +686,8 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
origin_ts,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
- getSensor()->getCalibration(),
- getSensor()->getCalibration(),
+ getCalibration(),
+ getCalibration(),
nullptr);
// ---------- LAST ----------
@@ -702,8 +702,8 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
origin_ts,
Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
- getSensor()->getCalibration(),
- getSensor()->getCalibration(),
+ getCalibration(),
+ getCalibration(),
origin_ptr_);
// clear and reset buffer
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 6ead58cd5f20f34b37e0ed4ca2023286fb75ef20..1126708fad699f766da7dc2d2c300dc55df72d2b 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),
@@ -42,7 +41,6 @@ SensorBase::SensorBase(const std::string& _type,
if (_intr_ptr)
addStateBlock('I', _intr_ptr, _intr_dyn);
- updateCalibSize();
}
SensorBase::SensorBase(const std::string& _type,
@@ -56,7 +54,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()),
@@ -72,8 +69,6 @@ SensorBase::SensorBase(const std::string& _type,
if (_intr_ptr)
addStateBlock('I', _intr_ptr, _intr_dyn);
-
- updateCalibSize();
}
SensorBase::~SensorBase()
@@ -106,7 +101,6 @@ void SensorBase::fixExtrinsics()
if (sbp != nullptr)
sbp->fix();
}
- updateCalibSize();
}
void SensorBase::unfixExtrinsics()
@@ -117,7 +111,6 @@ void SensorBase::unfixExtrinsics()
if (sbp != nullptr)
sbp->unfix();
}
- updateCalibSize();
}
void SensorBase::fixIntrinsics()
@@ -131,7 +124,6 @@ void SensorBase::fixIntrinsics()
sbp->fix();
}
}
- updateCalibSize();
}
void SensorBase::unfixIntrinsics()
@@ -145,7 +137,6 @@ void SensorBase::unfixIntrinsics()
sbp->unfix();
}
}
- updateCalibSize();
}
void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
@@ -306,35 +297,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;
-}
-
-Eigen::VectorXd SensorBase::getCalibration() const
-{
- SizeEigen index = 0;
- SizeEigen sz = getCalibSize();
- Eigen::VectorXd calib(sz);
- for (const auto& key : getStructure())
- {
- auto sb = getStateBlockDynamic(key);
- if (sb && !sb->isFixed())
- {
- calib.segment(index, sb->getSize()) = sb->getState();
- index += sb->getSize();
- }
- }
- return calib;
-}
-
bool SensorBase::process(const CaptureBasePtr capture_ptr)
{
capture_ptr->setSensor(shared_from_this());