From de9b3458f35c73bd0b4672b025ee7092ffe1fa95 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Tue, 22 Dec 2020 17:58:28 +0100
Subject: [PATCH] Remoce sensor calibration API
---
include/core/processor/processor_diff_drive.h | 7 +++-
include/core/processor/processor_motion.h | 2 +-
include/core/sensor/sensor_base.h | 20 ----------
src/processor/processor_motion.cpp | 8 ++--
src/sensor/sensor_base.cpp | 38 -------------------
5 files changed, 10 insertions(+), 65 deletions(-)
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index 427e74dc7..7203ff3fd 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 c621f61c3..75053ab0f 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 4c27ecb6e..f4cae62c3 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 c6fda0b64..406137e26 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 6ead58cd5..1126708fa 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());
--
GitLab