diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 1f457b1838b4475fccbf796d99ebf6daedae636d..e74e4a76b350455044b718d7d9084b727bc7f48e 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 5e35be115a93cdcf8543fcfcc59cc5c80ce552d1..6eaeb9aa6a1fcff66458ad634dd0f3660f2e360c 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")