From 8107c98b0e7bff05920af3babbfc8356ff107e2e Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Tue, 1 Dec 2020 16:08:39 +0100
Subject: [PATCH] WIP
---
include/core/sensor/sensor_base.h | 31 +++++++++++++++++++++++++-----
src/processor/processor_motion.cpp | 3 ++-
2 files changed, 28 insertions(+), 6 deletions(-)
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 1f457b183..e74e4a76b 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -64,9 +64,6 @@ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _ex
return sensor; \
} \
-
-
-
/** \brief base struct for intrinsic sensor parameters
*
* Derive from this struct to create structs of sensor intrinsic parameters.
@@ -74,11 +71,30 @@ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _ex
struct ParamsSensorBase: public ParamsBase
{
std::string prefix = "sensor/";
+
+ // Intrinsic dynamic and drift
+ bool intrinsic_dynamic;
+ bool intrinsic_drift;
+ Eigen::MatrixXd intrinsic_drift_cov;
+
+ ParamsSensorBase(std::string _unique_name, const wolf::ParamsServer& _server)
+ : ParamsBase(_unique_name, _server)
+ {
+ // Intrinsics dynamic
+ intrinsic_dynamic = _server.getParam<bool>(prefix + _unique_name + "/intrinsic_dynamic");
+ intrinsic_drift = _server.getParam<bool>(prefix + _unique_name + "/intrinsic_drift");
+ if (intrinsic_drift)
+ intrinsic_drift_cov = _server.getParam<Eigen::VectorXd>(prefix + _unique_name + "/intrinsic_drift_sigma").square().asDiagonal();
+ }
+
~ParamsSensorBase() override = default;
- using ParamsBase::ParamsBase;
+
std::string print() const
{
- return "";
+ return "\n" + ParamsBase::print() + "\n"
+ + "intrinsic_dynamic: " + std::to_string(intrinsic_dynamic) + "\n"
+ + "intrinsic_drift: " + std::to_string(intrinsic_drift) + "\n"
+ + (intrinsic_drift ? "intrinsic_drift_cov: " + std::to_string(intrinsic_drift_cov) : "");
}
};
@@ -195,6 +211,11 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
StateBlockPtr getO() const;
StateBlockPtr getIntrinsic() const;
+ virtual const MatrixXd& getIntrinsicDriftCov() const final
+ {
+ return params_->intrinsic_drift_cov;
+ }
+
protected:
void removeStateBlocks();
virtual void registerNewStateBlocks() const;
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 5e35be115..6eaeb9aa6 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -962,7 +962,8 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
return nullptr;
}
-void ProcessorMotion::emplaceCalibrationDriftFactor(CaptureMotionPtr _cap_motion){
+void ProcessorMotion::emplaceCalibrationDriftFactor(CaptureMotionPtr _cap_motion)
+{
MatrixXd cov_continuous = getSensor()->getCalibrationDriftCov();
if (cov_continuous.size() == 0){
WOLF_TRACE("SensorBase calib_drift_cov_ not initialized")
--
GitLab