WIP

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;

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")