Skip to content
Snippets Groups Projects
Commit 43384f8f authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

delta operators are const

parent 1d8a6436
No related branches found
No related tags found
3 merge requests!39release after RAL,!38After 2nd RAL submission,!11Add const
...@@ -45,21 +45,21 @@ class ProcessorIMU : public ProcessorMotion{ ...@@ -45,21 +45,21 @@ class ProcessorIMU : public ProcessorMotion{
const Scalar _dt, const Scalar _dt,
Eigen::VectorXs& _delta, Eigen::VectorXs& _delta,
Eigen::MatrixXs& _delta_cov, Eigen::MatrixXs& _delta_cov,
Eigen::MatrixXs& _jacobian_calib) override; Eigen::MatrixXs& _jacobian_calib) const override;
virtual void deltaPlusDelta(const Eigen::VectorXs& _delta_preint, virtual void deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
const Eigen::VectorXs& _delta, const Eigen::VectorXs& _delta,
const Scalar _dt, 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, virtual void deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
const Eigen::VectorXs& _delta, const Eigen::VectorXs& _delta,
const Scalar _dt, const Scalar _dt,
Eigen::VectorXs& _delta_preint_plus_delta, Eigen::VectorXs& _delta_preint_plus_delta,
Eigen::MatrixXs& _jacobian_delta_preint, Eigen::MatrixXs& _jacobian_delta_preint,
Eigen::MatrixXs& _jacobian_delta) override; Eigen::MatrixXs& _jacobian_delta) const override;
virtual void statePlusDelta(const Eigen::VectorXs& _x, virtual void statePlusDelta(const Eigen::VectorXs& _x,
const Eigen::VectorXs& _delta, const Eigen::VectorXs& _delta,
const Scalar _dt, const Scalar _dt,
Eigen::VectorXs& _x_plus_delta ) override; Eigen::VectorXs& _x_plus_delta ) const override;
virtual Eigen::VectorXs deltaZero() const override; virtual Eigen::VectorXs deltaZero() const override;
virtual bool voteForKeyFrame() override; virtual bool voteForKeyFrame() override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own, virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
......
...@@ -105,7 +105,7 @@ void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data, ...@@ -105,7 +105,7 @@ void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data,
const Scalar _dt, const Scalar _dt,
Eigen::VectorXs& _delta, Eigen::VectorXs& _delta,
Eigen::MatrixXs& _delta_cov, Eigen::MatrixXs& _delta_cov,
Eigen::MatrixXs& _jac_delta_calib) Eigen::MatrixXs& _jac_delta_calib) const
{ {
assert(_data.size() == data_size_ && "Wrong data size!"); assert(_data.size() == data_size_ && "Wrong data size!");
...@@ -144,7 +144,7 @@ void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data, ...@@ -144,7 +144,7 @@ void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data,
void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint, void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
const Eigen::VectorXs& _delta, const Eigen::VectorXs& _delta,
const Scalar _dt, const Scalar _dt,
Eigen::VectorXs& _delta_preint_plus_delta) Eigen::VectorXs& _delta_preint_plus_delta) const
{ {
/* MATHS according to Sola-16 /* 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 * 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, ...@@ -159,7 +159,7 @@ void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
void ProcessorIMU::statePlusDelta(const Eigen::VectorXs& _x, void ProcessorIMU::statePlusDelta(const Eigen::VectorXs& _x,
const Eigen::VectorXs& _delta, const Eigen::VectorXs& _delta,
const Scalar _dt, const Scalar _dt,
Eigen::VectorXs& _x_plus_delta) Eigen::VectorXs& _x_plus_delta) const
{ {
assert(_x.size() == 10 && "Wrong _x vector size"); assert(_x.size() == 10 && "Wrong _x vector size");
assert(_delta.size() == 10 && "Wrong _delta vector size"); assert(_delta.size() == 10 && "Wrong _delta vector size");
...@@ -174,7 +174,7 @@ void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint, ...@@ -174,7 +174,7 @@ void ProcessorIMU::deltaPlusDelta(const Eigen::VectorXs& _delta_preint,
const Scalar _dt, const Scalar _dt,
Eigen::VectorXs& _delta_preint_plus_delta, Eigen::VectorXs& _delta_preint_plus_delta,
Eigen::MatrixXs& _jacobian_delta_preint, Eigen::MatrixXs& _jacobian_delta_preint,
Eigen::MatrixXs& _jacobian_delta) Eigen::MatrixXs& _jacobian_delta) const
{ {
/* /*
* Expression of the delta integration step, D' = D (+) d: * Expression of the delta integration step, D' = D (+) d:
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment