diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h index d78cd5169f3dfc523c9435c11dc65b41ef6b3591..5b59b02421ad1050c97a4a188957c6274d1be946 100644 --- a/include/core/capture/capture_motion.h +++ b/include/core/capture/capture_motion.h @@ -82,19 +82,19 @@ class CaptureMotion : public CaptureBase const MotionBuffer& getBuffer() const; // Buffer's initial conditions for pre-integration - VectorXd getCalibrationPreint() const; - void setCalibrationPreint(const VectorXd& _calib_preint); - MatrixXd getJacobianCalib() const; - MatrixXd getJacobianCalib(const TimeStamp& _ts) const; + VectorComposite getCalibrationPreint() const; + void setCalibrationPreint(const VectorComposite& _calib_preint); + MatrixComposite getJacobianCalib() const; + MatrixComposite getJacobianCalib(const TimeStamp& _ts) const; // Get delta preintegrated, and corrected for changes on calibration params - VectorXd getDeltaCorrected(const VectorXd& _calib_current) const; - VectorXd getDeltaCorrected(const VectorXd& _calib_current, const TimeStamp& _ts) const; - VectorXd getDeltaPreint() const; - VectorXd getDeltaPreint(const TimeStamp& _ts) const; - MatrixXd getDeltaPreintCov() const; - MatrixXd getDeltaPreintCov(const TimeStamp& _ts) const; - virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const; + VectorComposite getDeltaCorrected(const VectorComposite& _calib_current) const; + VectorComposite getDeltaCorrected(const VectorComposite& _calib_current, const TimeStamp& _ts) const; + VectorComposite getDeltaPreint() const; + VectorComposite getDeltaPreint(const TimeStamp& _ts) const; + MatrixComposite getDeltaPreintCov() const; + MatrixComposite getDeltaPreintCov(const TimeStamp& _ts) const; + virtual VectorComposite correctDelta(const VectorComposite& _delta, const VectorComposite& _delta_error) const; // Origin frame and capture CaptureBasePtr getOriginCapture(); @@ -104,7 +104,7 @@ class CaptureMotion : public CaptureBase private: Eigen::VectorXd data_; ///< Motion data in form of vector mandatory Eigen::MatrixXd data_cov_; ///< Motion data covariance - Eigen::VectorXd calib_preint_; ///< Calibration parameters used during pre-integration + VectorComposite calib_preint_; ///< Calibration parameters used during pre-integration MotionBuffer buffer_; ///< Buffer of motions between this Capture and the next one. CaptureBaseWPtr capture_origin_ptr_; ///< Pointer to the origin capture of the motion }; @@ -142,17 +142,17 @@ inline MotionBuffer& CaptureMotion::getBuffer() return buffer_; } -inline Eigen::MatrixXd CaptureMotion::getJacobianCalib() const +inline MatrixComposite CaptureMotion::getJacobianCalib() const { return getBuffer().get().back().jacobian_calib_; } -inline Eigen::MatrixXd CaptureMotion::getJacobianCalib(const TimeStamp& _ts) const +inline MatrixComposite CaptureMotion::getJacobianCalib(const TimeStamp& _ts) const { return getBuffer().getMotion(_ts).jacobian_calib_; } -inline Eigen::VectorXd CaptureMotion::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const +inline VectorComposite CaptureMotion::correctDelta(const VectorComposite& _delta, const VectorComposite& _delta_error) const { WOLF_DEBUG("WARNING: using Cartesian sum for delta correction. \nIf your deltas lie on a manifold, derive this function and implement the proper delta correction!") return _delta + _delta_error; @@ -168,32 +168,32 @@ inline void CaptureMotion::setOriginCapture(CaptureBasePtr _capture_origin_ptr) capture_origin_ptr_ = _capture_origin_ptr; } -inline VectorXd CaptureMotion::getCalibrationPreint() const +inline VectorComposite CaptureMotion::getCalibrationPreint() const { return calib_preint_; } -inline void CaptureMotion::setCalibrationPreint(const VectorXd& _calib_preint) +inline void CaptureMotion::setCalibrationPreint(const VectorComposite& _calib_preint) { calib_preint_ = _calib_preint; } -inline VectorXd CaptureMotion::getDeltaPreint() const +inline VectorComposite CaptureMotion::getDeltaPreint() const { return getBuffer().get().back().delta_integr_; } -inline VectorXd CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const +inline VectorComposite CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const { return getBuffer().getMotion(_ts).delta_integr_; } -inline MatrixXd CaptureMotion::getDeltaPreintCov() const +inline MatrixComposite CaptureMotion::getDeltaPreintCov() const { return getBuffer().get().back().delta_integr_cov_; } -inline MatrixXd CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const +inline MatrixComposite CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const { return getBuffer().getMotion(_ts).delta_integr_cov_; } diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h index a8e06e0e112bfb5f07910ad56353bb71b2517ca8..bb37eeb34801900c0e65cde582fddf982f5a6da4 100644 --- a/include/core/processor/motion_buffer.h +++ b/include/core/processor/motion_buffer.h @@ -10,6 +10,7 @@ #include "core/common/wolf.h" #include "core/common/time_stamp.h" +#include "core/state_block/state_composite.h" #include <list> #include <algorithm> @@ -24,26 +25,26 @@ struct Motion TimeStamp ts_; ///< Time stamp Eigen::VectorXd data_; ///< instantaneous motion data Eigen::MatrixXd data_cov_; ///< covariance of the instantaneous data - Eigen::VectorXd delta_; ///< instantaneous motion delta - Eigen::MatrixXd delta_cov_; ///< covariance of the instantaneous delta - Eigen::VectorXd delta_integr_; ///< the integrated motion or delta-integral - Eigen::MatrixXd delta_integr_cov_; ///< covariance of the integrated delta - Eigen::MatrixXd jacobian_delta_; ///< Jacobian of the integration wrt delta_ - Eigen::MatrixXd jacobian_delta_integr_; ///< Jacobian of the integration wrt delta_integr_ - Eigen::MatrixXd jacobian_calib_; ///< Jacobian of delta_integr wrt extra states (TBD by the derived processors) + VectorComposite delta_; ///< instantaneous motion delta + MatrixComposite delta_cov_; ///< covariance of the instantaneous delta + VectorComposite delta_integr_; ///< the integrated motion or delta-integral + MatrixComposite delta_integr_cov_; ///< covariance of the integrated delta + MatrixComposite jacobian_delta_; ///< Jacobian of the integration wrt delta_ + MatrixComposite jacobian_delta_integr_; ///< Jacobian of the integration wrt delta_integr_ + MatrixComposite jacobian_calib_; ///< Jacobian of delta_integr wrt extra states (TBD by the derived processors) public: Motion() = delete; // completely delete unpredictable stuff like this - Motion(const TimeStamp& _ts, SizeEigen _data_size, SizeEigen _delta_size, SizeEigen _cov_size, SizeEigen _calib_size); + Motion(const TimeStamp& _ts, SizeEigen _data_size, SizeEigen _delta_size, SizeEigen _cov_size, SizeEigen _calib_size); // TODO remove Motion(const TimeStamp& _ts, const VectorXd& _data, const MatrixXd& _data_cov, - const VectorXd& _delta, - const MatrixXd& _delta_cov, - const VectorXd& _delta_int, - const MatrixXd& _delta_integr_cov, - const MatrixXd& _jac_delta, - const MatrixXd& _jac_delta_int, - const MatrixXd& _jacobian_calib); + const VectorComposite& _delta, + const MatrixComposite& _delta_cov, + const VectorComposite& _delta_int, + const MatrixComposite& _delta_integr_cov, + const MatrixComposite& _jac_delta, + const MatrixComposite& _jac_delta_int, + const MatrixComposite& _jacobian_calib); ~Motion(); private: diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h index 31d621a48855d8c774820b578fd4e0294d423484..0ca665a661a83409d17540f8caa94a5dfd1b5603 100644 --- a/include/core/processor/processor_motion.h +++ b/include/core/processor/processor_motion.h @@ -194,7 +194,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion /** \brief Gets the delta preintegrated covariance from all integrations so far * \return the delta preintegrated covariance matrix */ - const Eigen::MatrixXd getCurrentDeltaPreintCov() const; + const MatrixComposite getCurrentDeltaPreintCov() const; /** \brief Provide the motion integrated so far * \return the integrated motion @@ -358,13 +358,13 @@ class ProcessorMotion : public ProcessorBase, public IsMotion * * where `F_data = d_f/d_data` is the Jacobian of `f()`. */ - virtual void computeCurrentDelta(const Eigen::VectorXd& _data, - const Eigen::MatrixXd& _data_cov, - const Eigen::VectorXd& _calib, - const double _dt, - Eigen::VectorXd& _delta, - Eigen::MatrixXd& _delta_cov, - Eigen::MatrixXd& _jacobian_calib) const = 0; +// virtual void computeCurrentDelta(const Eigen::VectorXd& _data, +// const Eigen::MatrixXd& _data_cov, +// const Eigen::VectorXd& _calib, +// const double _dt, +// Eigen::VectorXd& _delta, +// Eigen::MatrixXd& _delta_cov, +// Eigen::MatrixXd& _jacobian_calib) const = 0; virtual void computeCUrrentDelta(const VectorXd& _data, const MatrixXd& _data_cov, @@ -382,10 +382,10 @@ class ProcessorMotion : public ProcessorBase, public IsMotion * * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2. */ - virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1, - const Eigen::VectorXd& _delta2, - const double _dt2, - Eigen::VectorXd& _delta1_plus_delta2) const = 0; +// virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1, +// 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, @@ -401,12 +401,12 @@ class ProcessorMotion : public ProcessorBase, public IsMotion * * This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2 and its jacobians. */ - virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1, - const Eigen::VectorXd& _delta2, - const double _dt2, - Eigen::VectorXd& _delta1_plus_delta2, - Eigen::MatrixXd& _jacobian1, - Eigen::MatrixXd& _jacobian2) const = 0; +// virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1, +// const Eigen::VectorXd& _delta2, +// const double _dt2, +// 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, @@ -422,10 +422,10 @@ class ProcessorMotion : public ProcessorBase, public IsMotion * * This function implements the composition (+) so that _x2 = _x1 (+) _delta. */ - virtual void statePlusDelta(const Eigen::VectorXd& _x, - const Eigen::VectorXd& _delta, - const double _dt, - Eigen::VectorXd& _x_plus_delta) const = 0; +// virtual void statePlusDelta(const Eigen::VectorXd& _x, +// 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, @@ -439,7 +439,7 @@ class ProcessorMotion : public ProcessorBase, public IsMotion * - 3d odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1] * - 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 Eigen::VectorXd deltaZero() const = 0; virtual VectorComposite deltaZero() const = 0; @@ -514,15 +514,15 @@ class ProcessorMotion : public ProcessorBase, public IsMotion // helpers to avoid allocation double dt_; ///< Time step Eigen::VectorXd x_; ///< current state - Eigen::VectorXd delta_; ///< current delta - Eigen::MatrixXd delta_cov_; ///< current delta covariance - Eigen::VectorXd delta_integrated_; ///< integrated delta - Eigen::MatrixXd delta_integrated_cov_; ///< integrated delta covariance - Eigen::VectorXd calib_preint_; ///< calibration vector used during pre-integration - Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated - Eigen::MatrixXd jacobian_delta_; ///< jacobian of delta composition w.r.t current delta - Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params - Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params + VectorComposite delta_; ///< current delta + MatrixComposite delta_cov_; ///< current delta covariance + VectorComposite delta_integrated_; ///< integrated delta + MatrixComposite delta_integrated_cov_; ///< integrated delta covariance + VectorComposite calib_preint_; ///< calibration vector used during pre-integration + MatrixComposite jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated + MatrixComposite jacobian_delta_; ///< jacobian of delta composition w.r.t current delta + MatrixComposite jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params + MatrixComposite jacobian_delta_calib_; ///< jacobian of delta wrt calib params Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity }; @@ -556,7 +556,7 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const statePlusDelta(curr_x, last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x); } -inline const Eigen::MatrixXd ProcessorMotion::getCurrentDeltaPreintCov() const +inline const MatrixComposite ProcessorMotion::getCurrentDeltaPreintCov() const { return getBuffer().get().back().delta_integr_cov_; } diff --git a/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h index 6a2816a5c44db5777531b0eb88e4ba6329dc48f9..5c947f386efcd4270ed14e02cf1886d8e6acd3ee 100644 --- a/include/core/state_block/state_composite.h +++ b/include/core/state_block/state_composite.h @@ -32,6 +32,8 @@ typedef std::unordered_map < std::string, Eigen::VectorXd > VectorComposite; namespace Eigen { std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x); + +wolf::VectorComposite& operator + (wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2); } namespace wolf{ diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp index a736c242e7fb7047498d061d9121ce8fb56f817c..13b11dda7909e80d1c81687eb4e7313e79994a50 100644 --- a/src/capture/capture_motion.cpp +++ b/src/capture/capture_motion.cpp @@ -51,7 +51,7 @@ CaptureMotion::~CaptureMotion() // } -Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current) const +VectorComposite CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current) const { VectorXd calib_preint = getCalibrationPreint(); VectorXd delta_preint = getBuffer().get().back().delta_integr_; @@ -61,7 +61,7 @@ Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current) return delta_corrected; } -Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current, const TimeStamp& _ts) const +VectorComposite CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current, const TimeStamp& _ts) const { Motion motion = getBuffer().getMotion(_ts); VectorXd delta_preint = motion.delta_integr_; diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp index e7e349e9196b3b3b298fc1d5142d8a38f348e1a6..a12de2212f53992b7bb2bb2e74b5ee98ccfa2ce4 100644 --- a/src/processor/motion_buffer.cpp +++ b/src/processor/motion_buffer.cpp @@ -5,13 +5,13 @@ namespace wolf Motion::Motion(const TimeStamp& _ts, const VectorXd& _data, const MatrixXd& _data_cov, - const VectorXd& _delta, - const MatrixXd& _delta_cov, - const VectorXd& _delta_integr, - const MatrixXd& _delta_integr_cov, - const MatrixXd& _jac_delta, - const MatrixXd& _jac_delta_int, - const MatrixXd& _jac_calib) : + const VectorComposite& _delta, + const MatrixComposite& _delta_cov, + const VectorComposite& _delta_integr, + const MatrixComposite& _delta_integr_cov, + const MatrixComposite& _jac_delta, + const MatrixComposite& _jac_delta_int, + const MatrixComposite& _jac_calib) : ts_(_ts), data_(_data), data_cov_(_data_cov), @@ -126,12 +126,12 @@ void MotionBuffer::print(bool show_data, bool show_delta, bool show_delta_int, b } if (show_delta) { - cout << " delta: " << mot.delta_.transpose() << endl; + cout << " delta: " << mot.delta_ << endl; cout << " delta cov: \n" << mot.delta_cov_ << endl; } if (show_delta_int) { - cout << " delta integrated: " << mot.delta_integr_.transpose() << endl; + cout << " delta integrated: " << mot.delta_integr_ << endl; cout << " delta integrated cov: \n" << mot.delta_integr_cov_ << endl; } if (show_jacs) diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp index 29154c33afe3cd97d24b34c41afa03a977a78ece..80ed5d181c609f0e9eee82ff98e26c1d147a8b3f 100644 --- a/src/state_block/state_composite.cpp +++ b/src/state_block/state_composite.cpp @@ -15,6 +15,20 @@ std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x) } return _os; } + +wolf::VectorComposite& operator + (const wolf::VectorComposite& _v1, const wolf::VectorComposite& _v2) +{ + wolf::VectorComposite v; + for (const auto& pair_key_vec1 : _v1) + { + const auto &key = pair_key_vec1.first; + const auto &vec1 = pair_key_vec1.second; + const auto& vec2 = _v2.at(key); + v.at(key) = vec1 + vec2; + } + return v; +} + } namespace wolf