diff --git a/include/IMU/processor/processor_IMU.h b/include/IMU/processor/processor_IMU.h index 41c85c2e5997f3d196437b1c9a226b354d0c3dc5..4204b264a915e07a384720e6716139c9cf5bba9b 100644 --- a/include/IMU/processor/processor_IMU.h +++ b/include/IMU/processor/processor_IMU.h @@ -45,21 +45,21 @@ class ProcessorIMU : public ProcessorMotion{ const Scalar _dt, Eigen::VectorXs& _delta, Eigen::MatrixXs& _delta_cov, - Eigen::MatrixXs& _jacobian_calib) override; + Eigen::MatrixXs& _jacobian_calib) const override; virtual void deltaPlusDelta(const Eigen::VectorXs& _delta_preint, const Eigen::VectorXs& _delta, const Scalar _dt, - Eigen::VectorXs& _delta_preint_plus_delta) override; + Eigen::VectorXs& _delta_preint_plus_delta) const override; virtual void deltaPlusDelta(const Eigen::VectorXs& _delta_preint, const Eigen::VectorXs& _delta, const Scalar _dt, Eigen::VectorXs& _delta_preint_plus_delta, Eigen::MatrixXs& _jacobian_delta_preint, - Eigen::MatrixXs& _jacobian_delta) override; + Eigen::MatrixXs& _jacobian_delta) const override; virtual void statePlusDelta(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, const Scalar _dt, - Eigen::VectorXs& _x_plus_delta ) override; + Eigen::VectorXs& _x_plus_delta ) const override; virtual Eigen::VectorXs deltaZero() const override; virtual bool voteForKeyFrame() override; virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp index 2d83320e714ff9c8d37768cb5946941de4807776..c8342b1266ee0172e5ace5b2c68771bb7c7e4e99 100644 --- a/src/processor/processor_IMU.cpp +++ b/src/processor/processor_IMU.cpp @@ -105,7 +105,7 @@ void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data, const Scalar _dt, Eigen::VectorXs& _delta, Eigen::MatrixXs& _delta_cov, - Eigen::MatrixXs& _jac_delta_calib) + Eigen::MatrixXs& _jac_delta_calib) const { assert(_data.size() == data_size_ && "Wrong data size!"); @@ -144,7 +144,7 @@ void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data, void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint, const Eigen::VectorXs& _delta, const Scalar _dt, - Eigen::VectorXs& _delta_preint_plus_delta) + Eigen::VectorXs& _delta_preint_plus_delta) const { /* MATHS according to Sola-16 * Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2 = Dp + Dv*dt + Dq*dp if dp = 1/2*(a-a_b)*dt^2 @@ -159,7 +159,7 @@ void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint, void ProcessorIMU::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, const Scalar _dt, - Eigen::VectorXs& _x_plus_delta) + Eigen::VectorXs& _x_plus_delta) const { assert(_x.size() == 10 && "Wrong _x vector size"); assert(_delta.size() == 10 && "Wrong _delta vector size"); @@ -174,7 +174,7 @@ void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint, const Scalar _dt, Eigen::VectorXs& _delta_preint_plus_delta, Eigen::MatrixXs& _jacobian_delta_preint, - Eigen::MatrixXs& _jacobian_delta) + Eigen::MatrixXs& _jacobian_delta) const { /* * Expression of the delta integration step, D' = D (+) d: