Draft: Resolve "Calibration drift factor creation in processorMotion"

added 1 commit

7c05241f - Calibration drift factor created in the processorMotion

Modify the prototpye of the pure virtual PM::emplaceCapture() to accept a set of state-blocks:

CaptureMotionPtr ProcessorMotion::emplaceCapture(const FrameBasePtr& _frame_own,
                                                 const SensorBasePtr& _sensor,
                                                 const TimeStamp& _ts,
                                                 const VectorXd& _data,
                                                 const MatrixXd& _data_cov,
                                                 const VectorXd& _calib,
                                                 const VectorXd& _calib_preint,
                                                 const CaptureBasePtr& _capture_origin,
                                                 const StateBlockComposite & _sbc)   = 0;

Create a new function in PM doing 3 things:

creating the set of state-blocks that are dynamic, that is, that are to be included in the Capture.
call emplaceCapture()
set the calibration parameters to the processor and to the capture (this was before done inside ProcDerived::emplaceCapture())

CaptureMotionPtr PM::newFcn                        (const FrameBasePtr& _frame_own,
                                                    const SensorBasePtr& _sensor,
                                                    const TimeStamp& _ts,
                                                    const VectorXd& _data,
                                                    const MatrixXd& _data_cov,
                                                    const VectorXd& _calib,
                                                    const VectorXd& _calib_preint,
                                                    const CaptureBasePtr& _capture_origin)
    auto sbc = // create state blocks dynamiques
    cap = emplaceCapture(_frame_own,
                         _sensor,
                         _ts,
                         _data,
                         _data_cov,
                         _calib,
                         _calib_preint,
                         _capture_origin
                         sbc);
    setCalibration                  (cap_motion, _calib); /// can be avoided if sbc are initialized
    cap_motion->setCalibrationPreint(_calib_preint);
    return cap_motion;
}

An example of the new ProcessorDerived::emplaceCapture():

CaptureMotionPtr ProcessorDerived::emplaceCapture(const FrameBasePtr& _frame_own,
                                                  const SensorBasePtr& _sensor,
                                                  const TimeStamp& _ts,
                                                  const VectorXd& _data,
                                                  const MatrixXd& _data_cov,
                                                  const VectorXd& _calib,
                                                  const VectorXd& _calib_preint,
                                                  const CaptureBasePtr& _capture_origin,
                                                  const StateBlockComposite & _sbc)
{

    // extract SBs
    auto sb1 = // from sbc
    auto sb2 = // from sbc
    auto sb3 = // from sbc
    auto cap_motion = CaptureBase::emplace<CaptureDerived>  (_frame_own,
                                                             _ts,
                                                             _sensor,
                                                             _data,
                                                             _data_cov,
                                                             _capture_origin,
                                                             sb1,
                                                             sb2,
                                                             sb3);

    return cap_motion;
}

Note: for the Sensor/Capture stateblocks, we need potentially to create 3 types of factors:

Motion factor
Calibration drift factor (FactorBlockDifference)
Calibration prior/fix (FactorAbsolute or fix())

These 3 cases should be handled nicely for each case if sensor stateblocks are dynamic/static.

This is implemented in SensorBase::isStateBlockInCapture()

This is no longer necessary

In SensorBase we should add the last_capture_ptr_ atribute (and the getter getLastCapture()). Now, getLastCapture() is looking for the last capture of the sensor, this method should be renamed to updateLastCapture().

Also, in CaptureBase::remove(), it should check if shared_from_this == sensor_ptr_->getLastCapture() and if its the case, call sensor_ptr_->updateLastCapture().

Also, somehow we should update the last capture whenever a new KF is created...