WIP composite delta operators

include/core/processor/processor_motion.h

@@ -366,6 +366,14 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
                                          Eigen::MatrixXd& _delta_cov,
                                          Eigen::MatrixXd& _jacobian_calib) const = 0;
 
+        virtual void computeCUrrentDelta(const VectorXd& _data,
+                                         const MatrixXd& _data_cov,
+                                         const VectorComposite& _calib,
+                                         const double _dt,
+                                         VectorComposite& _delta,
+                                         MatrixComposite& _delta_cov,
+                                         MatrixComposite& _jacobian_calib) const = 0;
+
         /** \brief composes a delta-state on top of another delta-state
          * \param _delta1 the first delta-state
          * \param _delta2 the second delta-state
@@ -378,6 +386,10 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
                                     const Eigen::VectorXd& _delta2,
                                     const double _dt2,
                                     Eigen::VectorXd& _delta1_plus_delta2) const = 0;
+        virtual void deltaPlusDelta(const VectorComposite& _delta1,
+                                    const VectorComposite& _delta2,
+                                    const double _dt2,
+                                    VectorComposite& _delta1_plus_delta2) const = 0;
 
         /** \brief composes a delta-state on top of another delta-state, and computes the Jacobians
          * \param _delta1 the first delta-state
@@ -395,6 +407,12 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
                                     Eigen::VectorXd& _delta1_plus_delta2,
                                     Eigen::MatrixXd& _jacobian1,
                                     Eigen::MatrixXd& _jacobian2) const = 0;
+        virtual void deltaPlusDelta(const VectorComposite& _delta1,
+                                    const VectorComposite& _delta2,
+                                    const double _dt2,
+                                    VectorComposite& _delta1_plus_delta2,
+                                    MatrixComposite& _jacobian1,
+                                    MatrixComposite& _jacobian2) const = 0;
 
         /** \brief composes a delta-state on top of a state
          * \param _x the initial state
@@ -408,6 +426,10 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
                                     const Eigen::VectorXd& _delta,
                                     const double _dt,
                                     Eigen::VectorXd& _x_plus_delta) const = 0;
+        virtual void statePlusDelta(const VectorComposite& _x,
+                                    const VectorComposite& _delta,
+                                    const double _dt,
+                                    VectorComposite& _x_plus_delta) const = 0;
 
         /** \brief Delta zero
          * \return a delta state equivalent to the null motion.
@@ -418,6 +440,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
          *   - IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !!
          */
         virtual Eigen::VectorXd deltaZero() const = 0;
+        virtual VectorComposite deltaZero() const = 0;
 
 
         /** \brief emplace a CaptureMotion
@@ -436,6 +459,14 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
                                                 const VectorXd& _calib,
                                                 const VectorXd& _calib_preint,
                                                 const CaptureBasePtr& _capture_origin_ptr) = 0;
+        virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+                                                const SensorBasePtr& _sensor,
+                                                const TimeStamp& _ts,
+                                                const VectorXd& _data,
+                                                const MatrixXd& _data_cov,
+                                                const VectorComposite& _calib,
+                                                const VectorComposite& _calib_preint,
+                                                const CaptureBasePtr& _capture_origin_ptr) = 0;
 
         /** \brief emplace a feature corresponding to given capture and add the feature to this capture
          * \param _capture_motion: the parent capture