Remove bias drift factors from FactorImu. Fix PrcImu accordingly.

include/imu/factor/factor_imu.h

@@ -36,7 +36,7 @@ namespace wolf {
 WOLF_PTR_TYPEDEFS(FactorImu);
 
 //class
-class FactorImu : public FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>
+class FactorImu : public FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>
 {
     public:
         FactorImu(const FeatureImuPtr& _ftr_ptr,
@@ -61,7 +61,6 @@ class FactorImu : public FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>
                          const T* const _p2,
                          const T* const _o2,
                          const T* const _v2,
-                         const T* const _b2,
                          T* _res) const;
 
         /** \brief Estimation error given the states in the wolf tree
@@ -95,8 +94,6 @@ class FactorImu : public FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>
                       const Eigen::MatrixBase<D1> &     _p2,
                       const Eigen::QuaternionBase<D2> & _q2,
                       const Eigen::MatrixBase<D1> &     _v2,
-                      const Eigen::MatrixBase<D1> &     _ab2,
-                      const Eigen::MatrixBase<D1> &     _wb2,
                       Eigen::MatrixBase<D3> &           _res) const;
 
         /** Function expectation(...)
@@ -150,23 +147,7 @@ class FactorImu : public FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>
 
         /// Metrics
         const double dt_; ///< delta-time and delta-time-squared between keyframes
-        const double ab_rate_stdev_, wb_rate_stdev_; //stdev for standard_deviation (= sqrt(variance))
         
-        /** bias covariance and bias residuals
-         *
-         * continuous bias covariance matrix for accelerometer would then be A_r = diag(ab_stdev_^2, ab_stdev_^2, ab_stdev_^2)
-         * To compute bias residuals, we will need to do (sqrt(A_r)).inverse() * ab_error
-         *
-         * In our case, we introduce time 'distance' in the computation of this residual (SEE FORSTER17), thus we have to use the discret covariance matrix
-         * discrete bias covariance matrix for accelerometer : A_r_dt = dt_ * A_r
-         * taking the square root for bias residuals : sqrt_A_r_dt = sqrt(dt_ * A_r) = sqrt(dt_) * sqrt(A_r)
-         * then with the inverse : sqrt_A_r_dt_inv = (sqrt(dt_ * A_r)).inverse() = (1/sqrt(dt_)) * sqrt(A_r).inverse()
-         *
-         * same logic for gyroscope bias
-         */ 
-        const Eigen::Matrix3d sqrt_A_r_dt_inv;
-        const Eigen::Matrix3d sqrt_W_r_dt_inv;
-
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
 };
@@ -177,8 +158,8 @@ inline FactorImu::FactorImu(const FeatureImuPtr&    _ftr_ptr,
                             const CaptureImuPtr&    _cap_origin_ptr,
                             const ProcessorBasePtr& _processor_ptr,
                             bool                    _apply_loss_function,
-                            FactorStatus        _status) :
-                FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>( // ...
+                            FactorStatus            _status) :
+                FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>( // ...
                         "FactorImu",
                         TOP_MOTION,
                         _ftr_ptr,
@@ -195,8 +176,7 @@ inline FactorImu::FactorImu(const FeatureImuPtr&    _ftr_ptr,
                         _cap_origin_ptr->getSensorIntrinsic(),
                         _ftr_ptr->getFrame()->getP(),
                         _ftr_ptr->getFrame()->getO(),
-                        _ftr_ptr->getFrame()->getV(),
-                        _ftr_ptr->getCapture()->getSensorIntrinsic()),
+                        _ftr_ptr->getFrame()->getV()),
         processor_imu_(std::static_pointer_cast<ProcessorImu>(_processor_ptr)),
         dp_preint_(_ftr_ptr->getMeasurement().head<3>()), // dp, dv, dq at preintegration time
         dq_preint_(_ftr_ptr->getMeasurement().data()+3),
@@ -208,11 +188,7 @@ inline FactorImu::FactorImu(const FeatureImuPtr&    _ftr_ptr,
         dDp_dwb_(_ftr_ptr->getJacobianBias().block(0,3,3,3)),
         dDv_dwb_(_ftr_ptr->getJacobianBias().block(6,3,3,3)),
         dDq_dwb_(_ftr_ptr->getJacobianBias().block(3,3,3,3)),
-        dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
-        ab_rate_stdev_(std::static_pointer_cast<SensorImu>(_ftr_ptr->getCapture()->getSensor())->getAbRateStdev()),
-        wb_rate_stdev_(std::static_pointer_cast<SensorImu>(_ftr_ptr->getCapture()->getSensor())->getWbRateStdev()),
-        sqrt_A_r_dt_inv((Eigen::Matrix3d::Identity() * ab_rate_stdev_ * sqrt(dt_)).inverse()),
-        sqrt_W_r_dt_inv((Eigen::Matrix3d::Identity() * wb_rate_stdev_ * sqrt(dt_)).inverse())
+        dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp())
 {
     //
 }
@@ -225,7 +201,6 @@ inline bool FactorImu::operator ()(const T* const _p1,
                                    const T* const _p2,
                                    const T* const _q2,
                                    const T* const _v2,
-                                   const T* const _b2,
                                    T* _res) const
 {
     using namespace Eigen;
@@ -240,12 +215,10 @@ inline bool FactorImu::operator ()(const T* const _p1,
     Map<const Matrix<T,3,1> > p2(_p2);
     Map<const Quaternion<T> > q2(_q2);
     Map<const Matrix<T,3,1> > v2(_v2);
-    Map<const Matrix<T,3,1> > ab2(_b2);
-    Map<const Matrix<T,3,1> > wb2(_b2 + 3);
 
     Map<Matrix<T,15,1> > res(_res);
 
-    residual(p1, q1, v1, ab1, wb1, p2, q2, v2, ab2, wb2, res);
+    residual(p1, q1, v1, ab1, wb1, p2, q2, v2, res);
 
     return true;
 }
@@ -264,7 +237,7 @@ Eigen::Vector9d FactorImu::error()
     Eigen::Vector9d delta_step;
 
     delta_step.head<3>()     = dDp_dab_ * (acc_bias - acc_bias_preint_ ) + dDp_dwb_ * (gyro_bias - gyro_bias_preint_);
-    delta_step.segment<3>(3) =                                       dDq_dwb_ * (gyro_bias - gyro_bias_preint_);
+    delta_step.segment<3>(3) =                                             dDq_dwb_ * (gyro_bias - gyro_bias_preint_);
     delta_step.tail<3>()     = dDv_dab_ * (acc_bias - acc_bias_preint_ ) + dDv_dwb_ * (gyro_bias - gyro_bias_preint_);
 
     Eigen::VectorXd delta_corr = imu::plus(delta_preint, delta_step);
@@ -283,8 +256,6 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> &       _p1,
                                 const Eigen::MatrixBase<D1> &       _p2,
                                 const Eigen::QuaternionBase<D2> &   _q2,
                                 const Eigen::MatrixBase<D1> &       _v2,
-                                const Eigen::MatrixBase<D1> &       _ab2,
-                                const Eigen::MatrixBase<D1> &       _wb2,
                                 Eigen::MatrixBase<D3> &             _res) const
 {
 
@@ -405,13 +376,6 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> &       _p1,
 
 #endif
 
-    // Errors between biases
-    Eigen::Matrix<T,3,1> ab_error(_ab1 - _ab2); // KF1.bias - KF2.bias
-    Eigen::Matrix<T,3,1> wb_error(_wb1 - _wb2);
-
-    // 4. Residuals are the weighted errors
-    _res.segment(9,3)  = sqrt_A_r_dt_inv.cast<T>() * ab_error;
-    _res.tail(3)       = sqrt_W_r_dt_inv.cast<T>() * wb_error;
 
     //////////////////////////////////////////////////////////////////////////////////////////////
     /////////////////////////////////       PRINT VALUES       ///////////////////////////////////

include/imu/processor/processor_imu.h

@@ -22,9 +22,12 @@
 #ifndef PROCESSOR_IMU_H
 #define PROCESSOR_IMU_H
 
-// Wolf
+// Wolf Imu
+#include "imu/sensor/sensor_imu.h"
 #include "imu/capture/capture_imu.h"
 #include "imu/feature/feature_imu.h"
+// Wolf core
+
 #include <core/processor/processor_motion.h>
 
 namespace wolf {
@@ -82,7 +85,7 @@ class ProcessorImu : public ProcessorMotion{
         ProcessorImu(ParamsProcessorImuPtr _params_motion_Imu);
         ~ProcessorImu() override;
         WOLF_PROCESSOR_CREATE(ProcessorImu, ParamsProcessorImu);
-        void configure(SensorBasePtr _sensor) override { };
+        void configure(SensorBasePtr _sensor) override;
 
         void preProcess() override;
 
@@ -147,8 +150,10 @@ class ProcessorImu : public ProcessorMotion{
         bool recomputeStates() const;
 
       protected:
-        ParamsProcessorImuPtr    params_motion_Imu_;
-        std::list<FactorBasePtr> bootstrap_factor_list_; ///< List of all IMU factors created while IMU is bootstrapping
+        ParamsProcessorImuPtr       params_motion_Imu_;
+        std::list<FactorBasePtr>    bootstrap_factor_list_; ///< List of all IMU factors created while IMU is bootstrapping
+        SensorImuPtr                sensor_imu_;
+        Matrix6d                    imu_drift_cov_;
 };
 }
 

src/processor/processor_imu.cpp

@@ -26,6 +26,7 @@
 
 // wolf
 #include <core/state_block/state_composite.h>
+#include <core/factor/factor_block_difference.h>
 
 namespace wolf {
 
@@ -106,25 +107,77 @@ void ProcessorImu::setCalibration (const CaptureBasePtr _capture, const VectorXd
     _capture->getSensorIntrinsic()->setState(_calibration);
 }
 
+void ProcessorImu::configure(SensorBasePtr _sensor) 
+{
+    sensor_imu_ = std::static_pointer_cast<SensorImu>(_sensor);
+
+    auto acc_drift_std  = sensor_imu_->getAbRateStdev();
+    auto gyro_drift_std = sensor_imu_->getAbRateStdev();
+
+    ArrayXd imu_drift_sigmas(6);
+    imu_drift_sigmas << acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std;
+    imu_drift_cov_  = imu_drift_sigmas.square().matrix().asDiagonal();
+
+}
 void ProcessorImu::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
 {
-    auto feature = FeatureBase::emplace<FeatureImu>(_capture_own,
-                                                    _capture_own->getBuffer().back().delta_integr_,
-                                                    _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
-                                                    _capture_own->getCalibrationPreint(),
-                                                    _capture_own->getBuffer().back().jacobian_calib_);
+    // 1. Feature and factor Imu
+    //---------------------------
+
+    auto ftr_imu = FeatureBase::emplace<FeatureImu>(
+        _capture_own, _capture_own->getBuffer().back().delta_integr_,
+        _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
+        _capture_own->getCalibrationPreint(), _capture_own->getBuffer().back().jacobian_calib_);
 
     CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
-    FeatureImuPtr ftr_imu = std::static_pointer_cast<FeatureImu>(feature);
 
-    auto fac_imu = FactorBase::emplace<FactorImu>(feature, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+    auto fac_imu =
+        FactorBase::emplace<FactorImu>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
 
-    if (bootstrapping_) 
+    if (bootstrapping_)
     {
         fac_imu->setStatus(FAC_INACTIVE);
         bootstrap_factor_list_.push_back(fac_imu);
     }
 
+    // 2. Feature and factor bias -- IMU bias drift for acc and gyro
+    //---------------------------------------------------------------
+    // - This factor only if IMU biases are Dynamic (check sensor->isStateBlockDynamic('I'))
+    // - feature has zero measurement size 6, with the cov of the bias drift size 6x6
+    // - factor relates bias(last capture) and bias(origin capture)
+    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",
+                                                  Vector6d::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
+
+            if (bootstrapping_)
+            {
+                fac_bias->setStatus(FAC_INACTIVE);
+                bootstrap_factor_list_.push_back(fac_bias);
+            }
+        }
+    }
 }
 
 void ProcessorImu::computeCurrentDelta(const Eigen::VectorXd& _data,