Remove bias drift from FactorImu

Proposal:

• Use only a factor that takes into account IMU measurements linking POVB_1 and POV_2.

• the drift is taken care of by a second factor of the type FactorBlockRelative.

• The ProcessorImu takes care to use a single FactorImu (case of static bias) or a FactorImu plus a FactorBlockRelative (case of dynamic bias)

changed the description

Now that core:#474 is done, we can add both factors from the same ProcessorImu:

ProcessorImu::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own
    // 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
            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
        }
    }

created branch 14-remove-bias-drift-from-factorimu to address this issue

mentioned in merge request !53 (merged)

Classes to update:

• ProcessorImu
• FactorImu
• ProcessorImu2d
• FactorImu2d
• FactorImu2dWithGravity

mentioned in commit 1f13c7fe

closed with merge request !53 (merged)