Add second factor with bias drift

include/imu/processor/processor_imu2d.h

@@ -23,6 +23,7 @@
 #define PROCESSOR_IMU2D_H
 
 // Wolf
+#include "imu/sensor/sensor_imu2d.h"
 #include "imu/capture/capture_imu.h"
 #include "imu/feature/feature_imu.h"
 #include <core/processor/processor_motion.h>
@@ -51,7 +52,7 @@ class ProcessorImu2d : public ProcessorMotion{
     public:
         ProcessorImu2d(ParamsProcessorImu2dPtr _params_motion_Imu);
         ~ProcessorImu2d() override;
-        void configure(SensorBasePtr _sensor) override { };
+        void configure(SensorBasePtr _sensor) override;
 
         WOLF_PROCESSOR_CREATE(ProcessorImu2d, ParamsProcessorImu2d);
         void preProcess() override;
@@ -97,6 +98,8 @@ class ProcessorImu2d : public ProcessorMotion{
 
     protected:
         ParamsProcessorImu2dPtr params_motion_Imu_;
+        SensorImu2dPtr          sensor_imu2d_;
+        Matrix3d                imu_drift_cov_;
 };
 
 }

src/processor/processor_imu2d.cpp

@@ -27,12 +27,14 @@
  */
 // imu
 #include "imu/processor/processor_imu2d.h"
+#include "imu/sensor/sensor_imu2d.h"
 #include "imu/factor/factor_imu2d.h"
 #include "imu/factor/factor_imu2d_with_gravity.h"
 #include "imu/math/imu2d_tools.h"
 
 // wolf
 #include <core/state_block/state_composite.h>
+#include <core/factor/factor_block_difference.h>
 
 namespace wolf {
 
@@ -113,6 +115,17 @@ namespace wolf {
   {
     _capture->getSensorIntrinsic()->setState(_calibration);
   }
+void ProcessorImu2d::configure(SensorBasePtr _sensor) 
+{
+    sensor_imu2d_ = std::static_pointer_cast<SensorImu2d>(_sensor);
+
+    auto acc_drift_std  = sensor_imu2d_->getAbRateStdev();
+    auto gyro_drift_std = sensor_imu2d_->getWbRateStdev();
+
+    Array3d imu_drift_sigmas(acc_drift_std, acc_drift_std, gyro_drift_std);
+    imu_drift_cov_  = imu_drift_sigmas.square().matrix().asDiagonal();
+
+}
 
   void ProcessorImu2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
   {
@@ -130,6 +143,35 @@ namespace wolf {
       else
           FactorBase::emplace<FactorImu2dWithGravity>(ftr_imu, ftr_imu, cap_imu, shared_from_this(),
                                                       params_->apply_loss_function);
+
+      if (getSensor()->isStateBlockDynamic('I'))
+      {
+          const auto& sb_imubias_own    = _capture_own->getStateBlock('I');
+          const auto& sb_imubias_origin = _capture_origin->getStateBlock('I');
+          if (sb_imubias_own != sb_imubias_origin)  // make sure it's two different state blocks! -- just in case
+          {
+              auto dt       = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp();
+              auto ftr_bias = FeatureBase::emplace<FeatureBase>(
+                  _capture_own, "FeatureBase",
+                  Vector3d::Zero(),      // mean IMU drift is zero
+                  imu_drift_cov_ * dt);  // IMU drift cov specified in continuous time
+              auto fac_bias =
+                  FactorBase::emplace<FactorBlockDifference>(ftr_bias, ftr_bias,
+                                                             sb_imubias_own,      // IMU bias block at t=own
+                                                             sb_imubias_origin,   // IMU bias block at t=origin
+                                                             nullptr,             // frame other
+                                                             _capture_origin,     // origin capture
+                                                             nullptr,             // feature other
+                                                             nullptr,             // landmark other
+                                                             0,                   // take all of first state block
+                                                             -1,                  // take all of first state block
+                                                             0,                   // take all of first second block
+                                                             -1,                  // take all of first second block
+                                                             shared_from_this(),  // this processor
+                                                             params_->apply_loss_function);  // loss function
+
+          }
+      }
   }
 
   void ProcessorImu2d::computeCurrentDelta(const Eigen::VectorXd& _data,