diff --git a/src/processor_imu.h b/src/processor_imu.h index c20f141b1b42676daf6e35bc38fff4c87219a3f5..11dbe77767d5acb3c240ea6b72d13e628e7c4e62 100644 --- a/src/processor_imu.h +++ b/src/processor_imu.h @@ -79,6 +79,8 @@ class ProcessorIMU : public ProcessorMotion{ virtual void remapPQV(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, Eigen::VectorXs& _delta_out); virtual void remapDelta(Eigen::VectorXs& _delta_out); virtual void remapData(const Eigen::VectorXs& _data); + void getJacobians(Eigen::Matrix3s& _dDp_dab, Eigen::Matrix3s& _dDv_dab, Eigen::Matrix3s& _dDp_dwb, Eigen::Matrix3s& _dDv_dwb, Eigen::Matrix3s& _dDq_dwb); + public: @@ -405,7 +407,16 @@ inline void ProcessorIMU::remapData(const Eigen::VectorXs& _data) new (&gyro_measured_) Eigen::Map<const Eigen::Vector3s>(_data.data() + 3); } -/*void ProcessorIMU::getJacobians(Eigen::Matrix3s& _dDp_dab, Eigen::Matrix3s& _dDv_dab, Eigen::Matrix3s& _dDp_dwb, Eigen::Matrix3s& _dDv_dwb, Eigen::Matrix3s& _dDq_dwb) +inline void ProcessorIMU::getJacobians(Eigen::Matrix3s& _dDp_dab, Eigen::Matrix3s& _dDv_dab, Eigen::Matrix3s& _dDp_dwb, Eigen::Matrix3s& _dDv_dwb, Eigen::Matrix3s& _dDq_dwb) +{ + _dDp_dab = dDp_dab_; + _dDv_dab = dDv_dab_; + _dDp_dwb = dDp_dwb_; + _dDv_dwb = dDv_dwb_; + _dDq_dwb = dDq_dwb_; +} + +/*inline void ProcessorIMU::getJacobians(Eigen::Matrix9s& _dD_db) { _dDp_dab = dDp_dab_; _dDv_dab = dDv_dab_;