diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 2340cd8fc16c7da8db8767298b2337ba46a666ac..31d621a48855d8c774820b578fd4e0294d423484 100644 --- a/include/core/processor/processor_motion.h +++ b/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