diff --git a/CMakeLists.txt b/CMakeLists.txt
index 455a6a61b102f09a3794f71aa25a9cd94eee7174..ea892d9b6b3b197e573e295313227fcb62286c09 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -115,6 +115,7 @@ SET(HDRS_FACTOR
include/imu/factor/factor_compass_3d.h
include/imu/factor/factor_imu.h
include/imu/factor/factor_imu2d.h
+ include/imu/factor/factor_imu2d_with_gravity.h
)
SET(HDRS_FEATURE
include/imu/feature/feature_imu.h
diff --git a/demos/sensor_imu2d.yaml b/demos/sensor_imu2d.yaml
index ce810d4f5f397a7bcb8647e086c026b125fbc936..da862c15361d86daccd5185465655a42c8c74c5a 100644
--- a/demos/sensor_imu2d.yaml
+++ b/demos/sensor_imu2d.yaml
@@ -7,3 +7,4 @@ motion_variances:
wb_initial_stdev: 0.350 # rad/sec - initial bias
ab_rate_stdev: 0.1 # m/s2/sqrt(s)
wb_rate_stdev: 0.0400 # rad/s/sqrt(s)
+ orthogonal_gravity: true
diff --git a/demos/sensor_imu2d_with_gravity.yaml b/demos/sensor_imu2d_with_gravity.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..eb30116dd303e9fc0d497ff62e2102d66adeca7d
--- /dev/null
+++ b/demos/sensor_imu2d_with_gravity.yaml
@@ -0,0 +1,10 @@
+type: "SensorImu2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+motion_variances:
+ a_noise: 0.053 # standard deviation of Acceleration noise (same for all the axis) in m/s2
+ w_noise: 0.0011 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec
+ ab_initial_stdev: 0.800 # m/s2 - initial bias
+ wb_initial_stdev: 0.350 # rad/sec - initial bias
+ ab_rate_stdev: 0.1 # m/s2/sqrt(s)
+ wb_rate_stdev: 0.0400 # rad/s/sqrt(s)
+ orthogonal_gravity: false
diff --git a/include/imu/factor/factor_imu2d.h b/include/imu/factor/factor_imu2d.h
index 71129e3ead4e1840cbd6d11bad14231a7e35cefe..5158c590fb2a6ecaf570cf6e0830fbc697437ded 100644
--- a/include/imu/factor/factor_imu2d.h
+++ b/include/imu/factor/factor_imu2d.h
@@ -186,7 +186,7 @@ inline bool FactorImu2d::operator ()(const T* const _p1,
Map<Matrix<T,2,1> > dp_predicted( &delta_predicted(0) + 0);
T& dth_predicted = delta_predicted(2);
Map<Matrix<T,2,1> > dv_predicted(&delta_predicted(0) + 3);
- imu2d::between(p1, th1, v1, p2, th2, v2, dt_, dp_predicted, dth_predicted, dv_predicted);
+ imu2d::betweenStates(p1, th1, v1, p2, th2, v2, dt_, dp_predicted, dth_predicted, dv_predicted);
Matrix<T, 5, 1> delta_error = delta_corr - delta_predicted;
delta_error(2) = pi2pi(delta_error(2));
diff --git a/include/imu/factor/factor_imu2d_with_gravity.h b/include/imu/factor/factor_imu2d_with_gravity.h
new file mode 100644
index 0000000000000000000000000000000000000000..6356a659e06a4a347607cff564cc8e03d581210b
--- /dev/null
+++ b/include/imu/factor/factor_imu2d_with_gravity.h
@@ -0,0 +1,209 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#ifndef FACTOR_IMU2D_WITH_GRAVITY_H_
+#define FACTOR_IMU2D_WITH_GRAVITY_H_
+
+//Wolf includes
+#include "imu/feature/feature_imu2d.h"
+#include "imu/processor/processor_imu2d.h"
+#include "imu/sensor/sensor_imu2d.h"
+#include "core/factor/factor_autodiff.h"
+#include "core/math/rotations.h"
+#include "imu/math/imu2d_tools.h"
+
+//Eigen include
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorImu2dWithGravity);
+
+//class
+class FactorImu2dWithGravity : public FactorAutodiff<FactorImu2dWithGravity, 8, 2, 1, 2, 3, 2, 1, 2, 3, 2>
+{
+ public:
+ FactorImu2dWithGravity(const FeatureImu2dPtr& _ftr_ptr,
+ const CaptureImuPtr& _capture_origin_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorImu2dWithGravity() override = default;
+
+ /** \brief : compute the residual from the state blocks being iterated by the solver.
+ -> computes the expected measurement
+ -> corrects actual measurement with new bias
+ -> compares the corrected measurement with the expected one
+ -> weights the result with the covariance of the noise (residual = sqrt_info_matrix * err;)
+ */
+ template<typename T>
+ bool operator ()(const T* const _p1,
+ const T* const _o1,
+ const T* const _v1,
+ const T* const _b1,
+ const T* const _p2,
+ const T* const _o2,
+ const T* const _v2,
+ const T* const _b2,
+ const T* const _g,
+ T* _res) const;
+ Eigen::Matrix3d getBiasDriftSquareRootInformationUpper(const FeatureImu2dPtr& _ftr_ptr)
+ {
+ Eigen::Matrix3d res = Eigen::Matrix3d::Identity();
+ if(_ftr_ptr->getCapture()->getSensor()){
+ res *= 1./(std::static_pointer_cast<SensorImu2d>(_ftr_ptr->getCapture()->getSensor())->getAbRateStdev() * dt_);
+ res(2,2) = 1./(std::static_pointer_cast<SensorImu2d>(_ftr_ptr->getCapture()->getSensor())->getWbRateStdev() * dt_);
+ }
+ WOLF_WARN_COND(!_ftr_ptr->getCapture()->getSensor(), "Null Sensor Pointer");
+ return res;
+ }
+
+ private:
+ // Imu processor
+ ProcessorImu2dPtr processor_imu2d_;
+
+ /// Preintegrated delta
+ Eigen::Vector5d delta_preint_;
+
+ // Biases used during preintegration
+ Eigen::Vector3d bias_preint_;
+
+ // Jacobians of preintegrated deltas wrt biases
+ Eigen::Matrix<double, 5, 3> jacobian_bias_;
+
+ /// Metrics
+ const double dt_; ///< delta-time and delta-time-squared between keyframes
+
+ /** bias covariance and bias residuals
+ *
+ * continuous bias covariance matrix for accelerometer would then be A_r = diag(ab_stdev_^2, ab_stdev_^2, ab_stdev_^2)
+ * To compute bias residuals, we will need to do (sqrt(A_r)).inverse() * ab_error
+ *
+ * In our case, we introduce time 'distance' in the computation of this residual (SEE FORSTER17), thus we have to use the discret covariance matrix
+ * discrete bias covariance matrix for accelerometer : A_r_dt = dt_ * A_r
+ * taking the square root for bias residuals : sqrt_A_r_dt = sqrt(dt_ * A_r) = sqrt(dt_) * sqrt(A_r)
+ * then with the inverse : sqrt_A_r_dt_inv = (sqrt(dt_ * A_r)).inverse() = (1/sqrt(dt_)) * sqrt(A_r).inverse()
+ *
+ * same logic for gyroscope bias
+ */
+ const Eigen::Matrix5d sqrt_delta_preint_inv_;
+ const Eigen::Matrix3d sqrt_bias_drift_dt_inv_;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+};
+
+///////////////////// IMPLEMENTAITON ////////////////////////////
+
+inline FactorImu2dWithGravity::FactorImu2dWithGravity(const FeatureImu2dPtr& _ftr_ptr,
+ const CaptureImuPtr& _cap_origin_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorAutodiff<FactorImu2dWithGravity, 8, 2, 1, 2, 3, 2, 1, 2, 3, 2>( // ...
+ "FactorImu2dWithGravity",
+ TOP_MOTION,
+ _ftr_ptr,
+ _cap_origin_ptr->getFrame(),
+ _cap_origin_ptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _cap_origin_ptr->getFrame()->getP(),
+ _cap_origin_ptr->getFrame()->getO(),
+ _cap_origin_ptr->getFrame()->getV(),
+ _cap_origin_ptr->getSensorIntrinsic(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getFrame()->getV(),
+ _ftr_ptr->getCapture()->getSensorIntrinsic(),
+ _cap_origin_ptr->getSensor()->getStateBlock('G')),
+ processor_imu2d_(std::static_pointer_cast<ProcessorImu2d>(_processor_ptr)),
+ delta_preint_(_ftr_ptr->getMeasurement()), // dp, dth, dv at preintegration time
+ bias_preint_(_ftr_ptr->getBiasPreint()), // state biases at preintegration time
+ jacobian_bias_(_ftr_ptr->getJacobianBias()), // Jacs of dp dv dq wrt biases
+ dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
+ sqrt_delta_preint_inv_(_ftr_ptr->getMeasurementSquareRootInformationUpper()),
+ sqrt_bias_drift_dt_inv_(getBiasDriftSquareRootInformationUpper(_ftr_ptr))
+{
+ //
+}
+
+template<typename T>
+inline bool FactorImu2dWithGravity::operator ()(const T* const _p1,
+ const T* const _th1,
+ const T* const _v1,
+ const T* const _b1,
+ const T* const _p2,
+ const T* const _th2,
+ const T* const _v2,
+ const T* const _b2,
+ const T* const _g,
+ T* _res) const
+{
+ using namespace Eigen;
+
+ // MAPS
+ Map<const Matrix<T,2,1> > p1(_p1);
+ const T& th1 = *_th1;
+ Map<const Matrix<T,2,1> > v1(_v1);
+ Map<const Matrix<T,3,1> > b1(_b1);
+
+ Map<const Matrix<T,2,1> > p2(_p2);
+ const T& th2 = *_th2;
+ Map<const Matrix<T,2,1> > v2(_v2);
+ Map<const Matrix<T,3,1> > b2(_b2);
+ Map<const Matrix<T,2,1> > g(_g);
+
+ Map<Matrix<T,8,1> > res(_res);
+
+ //residual
+ /*
+ * MATH:
+ * res_delta = (Covariancia delta)^(T/2) * ((delta_preint (+) Jacob^delta_bias*(b1-b1_preint))- (x2 <-> x1))
+ */
+ Matrix<T, 5, 1> delta_step = jacobian_bias_*(b1 - bias_preint_);
+ Matrix<T, 5, 1> delta_preint = delta_preint_.cast<T>();
+ Matrix<T, 5, 1> delta_corr = imu2d::plus(delta_preint, delta_step);
+
+ Matrix<T, 5, 1> delta_predicted;
+ Map<Matrix<T,2,1> > dp_predicted( &delta_predicted(0) + 0);
+ T& dth_predicted = delta_predicted(2);
+ Map<Matrix<T,2,1> > dv_predicted(&delta_predicted(0) + 3);
+ imu2d::betweenStatesWithGravity(p1, th1, v1, p2, th2, v2, dt_, g, dp_predicted, dth_predicted, dv_predicted);
+
+ Matrix<T, 5, 1> delta_error = imu2d::diff(delta_predicted, delta_corr);
+ delta_error(2) = pi2pi(delta_error(2));
+
+ res.template head<5>() = sqrt_delta_preint_inv_*delta_error;
+
+
+ //bias drift
+ res.template tail<3>() = sqrt_bias_drift_dt_inv_*(b2 -b1);
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/imu/math/imu2d_tools.h b/include/imu/math/imu2d_tools.h
index 018c34f27e4b7910ab777c842098dec503ff7cf0..4f2a1ef535d9a538c03228a81d6940e67c7439a2 100644
--- a/include/imu/math/imu2d_tools.h
+++ b/include/imu/math/imu2d_tools.h
@@ -53,12 +53,14 @@
* - inverse: so that D (+) D.inv = I
* - compose: Dc = D1 (+) D2
* - between: Db = D2 (-) D1, so that D2 = D1 (+) Db
- * - composeOverState: x2 = x1 (+) D
- * - betweenStates: D = x2 (-) x1, so that x2 = x1 (+) D
- * - log: go from Delta manifold to tangent space (equivalent to log() in rotations)
+ * - composeOverState: x2 = x1 <+> D
+ * - composeOverStateWithGravity: same as composeOverState but used in the slanted case with an arbitrary gravity vector
+ * - betweenStates: D = x2 <-> x1, so that x2 = x1 (+) D
+ * - betweenStatesWithGravity: same as betweenStatesWithGravity but used in the slanted case with an arbitrary gravity vector
+ * - exp_Log: go from Delta manifold to tangent space (equivalent to log() in rotations)
* - exp_Imu: go from tangent space to delta manifold (equivalent to exp() in rotations)
* - plus: D2 = D1 (+) exp_Imu(d)
- * - diff: d = log_Imu( D2 (-) D1 )
+ * - diff: d = exp_Log_Imu( D2 (-) D1 )
* - body2delta: construct a delta from body magnitudes of linAcc and angVel
*/
@@ -283,7 +285,7 @@ inline void compose(const MatrixBase<D1>& d1,
// // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
// compose(d1, d2, dt, sum);
//}
-//
+
template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, class T1, class T2, class T3, class T4>
inline void between(const MatrixBase<D1>& dp1, const T1& dth1, const MatrixBase<D2>& dv1,
const MatrixBase<D3>& dp2, const T2& dth2, const MatrixBase<D4>& dv2,
@@ -333,164 +335,281 @@ inline Matrix<typename D1::Scalar, 5, 1> between(const MatrixBase<D1>& d1,
return diff;
}
-//template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T>
-//inline void composeOverState(const MatrixBase<D1>& p, const QuaternionBase<D2>& q, const MatrixBase<D3>& v,
-// const MatrixBase<D4>& dp,const QuaternionBase<D5>& dq,const MatrixBase<D6>& dv,
-// T dt,
-// MatrixBase<D7>& p_plus_dp, QuaternionBase<D8>& q_plus_dq, MatrixBase<D9>& v_plus_dv)
-//{
-// MatrixSizeCheck<3, 1>::check(p);
-// MatrixSizeCheck<3, 1>::check(v);
-// MatrixSizeCheck<3, 1>::check(dp);
-// MatrixSizeCheck<3, 1>::check(dv);
-// MatrixSizeCheck<3, 1>::check(p_plus_dp);
-// MatrixSizeCheck<3, 1>::check(v_plus_dv);
-//
-// p_plus_dp = p + v*dt + 0.5*gravity()*dt*dt + q*dp;
-// v_plus_dv = v + gravity()*dt + q*dv;
-// q_plus_dq = q*dq; // dq here to avoid possible aliasing between x and x_plus_d
-//}
-//
-//template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T>
-//inline void composeOverState(const MatrixBase<D1>& p , const MatrixBase<D2>& q , const MatrixBase<D3>& v,
-// const MatrixBase<D4>& dp, const MatrixBase<D5>& dq, const MatrixBase<D6>& dv,
-// T dt,
-// MatrixBase<D7>& p_plus_dp, MatrixBase<D8>& q_plus_dq, MatrixBase<D9>& v_plus_dv)
-//{
-//
-// MatrixSizeCheck<3, 1>::check(p);
-// MatrixSizeCheck<4, 1>::check(q);
-// MatrixSizeCheck<3, 1>::check(v);
-// MatrixSizeCheck<3, 1>::check(dp);
-// MatrixSizeCheck<4, 1>::check(dq);
-// MatrixSizeCheck<3, 1>::check(dv);
-// MatrixSizeCheck<3, 1>::check(p_plus_dp);
-// MatrixSizeCheck<4, 1>::check(q_plus_dq);
-// MatrixSizeCheck<3, 1>::check(v_plus_dv);
-//
-// Map<const Quaternion<typename D2::Scalar> > qq ( & q (0) );
-// Map<const Quaternion<typename D5::Scalar> > dqq ( & dq (0) );
-// Map< Quaternion<typename D8::Scalar> > qq_plus_dqq ( & q_plus_dq (0) );
-//
-// composeOverState(p, qq, v,
-// dp, dqq, dv,
-// dt,
-// p_plus_dp, qq_plus_dqq, v_plus_dv);
-//}
-//
-//template<typename D1, typename D2, typename D3, class T>
-//inline void composeOverState(const MatrixBase<D1>& x,
-// const MatrixBase<D2>& d,
-// T dt,
-// MatrixBase<D3>& x_plus_d)
-//{
-// MatrixSizeCheck<10, 1>::check(x);
-// MatrixSizeCheck<10, 1>::check(d);
-// MatrixSizeCheck<10, 1>::check(x_plus_d);
-//
-// Map<const Matrix<typename D1::Scalar, 3, 1> > p ( & x( 0 ) );
-// Map<const Quaternion<typename D1::Scalar> > q ( & x( 3 ) );
-// Map<const Matrix<typename D1::Scalar, 3, 1> > v ( & x( 7 ) );
-// Map<const Matrix<typename D2::Scalar, 3, 1> > dp ( & d( 0 ) );
-// Map<const Quaternion<typename D2::Scalar> > dq ( & d( 3 ) );
-// Map<const Matrix<typename D2::Scalar, 3, 1> > dv ( & d( 7 ) );
-// Map<Matrix<typename D3::Scalar, 3, 1> > p_plus_d ( & x_plus_d( 0 ) );
-// Map<Quaternion<typename D3::Scalar> > q_plus_d ( & x_plus_d( 3 ) );
-// Map<Matrix<typename D3::Scalar, 3, 1> > v_plus_d ( & x_plus_d( 7 ) );
-//
-// composeOverState( p, q, v,
-// dp, dq, dv,
-// dt,
-// p_plus_d, q_plus_d, v_plus_d);
-//}
-//
-//template<typename D1, typename D2, class T>
-//inline Matrix<typename D1::Scalar, 10, 1> composeOverState(const MatrixBase<D1>& x,
-// const MatrixBase<D2>& d,
-// T dt)
-//{
-// Matrix<typename D1::Scalar, 10, 1> ret;
-// composeOverState(x, d, dt, ret);
-// return ret;
-//}
-//
-//template<class T>
-//inline void composeOverState(const VectorComposite& x,
-// const VectorComposite& d,
-// T dt,
-// VectorComposite& x_plus_d)
-//{
-// assert(x_plus_d.count('P') && "provided reference does not have key 'P'");
-// assert(x_plus_d.count('O') && "provided reference does not have key 'O'");
-// assert(x_plus_d.count('V') && "provided reference does not have key 'V'");
-//
-// composeOverState(x.at('P'), x.at('O'), x.at('V'),
-// d.at('P'), d.at('O'), d.at('V'),
-// dt,
-// x_plus_d['P'], x_plus_d['O'], x_plus_d['V']);
-//}
-//
-//template<class T>
-//inline VectorComposite composeOverState(const VectorComposite& x,
-// const VectorComposite& d,
-// T dt)
-//{
-// VectorComposite ret("POV", {3,4,3});
-//
-// composeOverState(x, d, dt, ret);
-// return ret;
-//}
-//
-//template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, class T>
-//inline void betweenStates(const MatrixBase<D1>& p1, const QuaternionBase<D2>& q1, const MatrixBase<D3>& v1,
-// const MatrixBase<D4>& p2, const QuaternionBase<D5>& q2, const MatrixBase<D6>& v2,
-// const T dt,
-// MatrixBase<D7>& dp, QuaternionBase<D8>& dq, MatrixBase<D9>& dv )
-//{
-// MatrixSizeCheck<3, 1>::check(p1);
-// MatrixSizeCheck<3, 1>::check(v1);
-// MatrixSizeCheck<3, 1>::check(p2);
-// MatrixSizeCheck<3, 1>::check(v2);
-// MatrixSizeCheck<3, 1>::check(dp);
-// MatrixSizeCheck<3, 1>::check(dv);
-//
-// dp = q1.conjugate() * ( p2 - p1 - v1*dt - (T)0.5*gravity().cast<T>()*(T)dt*(T)dt );
-// dv = q1.conjugate() * ( v2 - v1 - gravity().cast<T>()*(T)dt );
-// dq = q1.conjugate() * q2;
-//}
-//
-//template<typename D1, typename D2, typename D3, class T>
-//inline void betweenStates(const MatrixBase<D1>& x1,
-// const MatrixBase<D2>& x2,
-// T dt,
-// MatrixBase<D3>& x2_minus_x1)
-//{
-// MatrixSizeCheck<10, 1>::check(x1);
-// MatrixSizeCheck<10, 1>::check(x2);
-// MatrixSizeCheck<10, 1>::check(x2_minus_x1);
-//
-// Map<const Matrix<typename D1::Scalar, 3, 1> > p1 ( & x1(0) );
-// Map<const Quaternion<typename D1::Scalar> > q1 ( & x1(3) );
-// Map<const Matrix<typename D1::Scalar, 3, 1> > v1 ( & x1(7) );
-// Map<const Matrix<typename D2::Scalar, 3, 1> > p2 ( & x2(0) );
-// Map<const Quaternion<typename D2::Scalar> > q2 ( & x2(3) );
-// Map<const Matrix<typename D2::Scalar, 3, 1> > v2 ( & x2(7) );
-// Map<Matrix<typename D3::Scalar, 3, 1> > dp ( & x2_minus_x1(0) );
-// Map<Quaternion<typename D3::Scalar> > dq ( & x2_minus_x1(3) );
-// Map<Matrix<typename D3::Scalar, 3, 1> > dv ( & x2_minus_x1(7) );
-//
-// betweenStates(p1, q1, v1, p2, q2, v2, dt, dp, dq, dv);
-//}
-//
-//template<typename D1, typename D2, class T>
-//inline Matrix<typename D1::Scalar, 10, 1> betweenStates(const MatrixBase<D1>& x1,
-// const MatrixBase<D2>& x2,
-// T dt)
-//{
-// Matrix<typename D1::Scalar, 10, 1> ret;
-// betweenStates(x1, x2, dt, ret);
-// return ret;
-//}
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, class T1, class T2, class T3, class T4>
+inline void composeOverState(const MatrixBase<D1>& p, const T1& th, const MatrixBase<D2>& v,
+ const MatrixBase<D3>& dp,const T2& dth, const MatrixBase<D4>& dv,
+ T3& dt,
+ MatrixBase<D5>& p_plus_dp, T4& th_plus_dth, MatrixBase<D6>& v_plus_dv)
+{
+ MatrixSizeCheck<2, 1>::check(p);
+ MatrixSizeCheck<2, 1>::check(v);
+ MatrixSizeCheck<2, 1>::check(dp);
+ MatrixSizeCheck<2, 1>::check(dv);
+ MatrixSizeCheck<2, 1>::check(p_plus_dp);
+ MatrixSizeCheck<2, 1>::check(v_plus_dv);
+ const auto& R = Rotation2D<T1>(th).matrix();
+
+ p_plus_dp = p + v*dt + R*dp;
+ v_plus_dv = v + R*dv;
+ th_plus_dth = th + dth; //Angles simply sum
+}
+
+//Versions of composeOverState with parameter g are used in the slanted plane case
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, class T1, class T2, class T3, class T4>
+inline void composeOverStateWithGravity(const MatrixBase<D1>& p, const T1& th, const MatrixBase<D2>& v,
+ const MatrixBase<D3>& dp,const T2& dth, const MatrixBase<D4>& dv,
+ T3& dt, const MatrixBase<D5>& g,
+ MatrixBase<D6>& p_plus_dp, T4& th_plus_dth, MatrixBase<D7>& v_plus_dv)
+{
+ MatrixSizeCheck<2, 1>::check(p);
+ MatrixSizeCheck<2, 1>::check(v);
+ MatrixSizeCheck<2, 1>::check(dp);
+ MatrixSizeCheck<2, 1>::check(dv);
+ MatrixSizeCheck<2, 1>::check(g);
+ MatrixSizeCheck<2, 1>::check(p_plus_dp);
+ MatrixSizeCheck<2, 1>::check(v_plus_dv);
+ const auto& R = Rotation2D<T1>(th).matrix();
+
+ p_plus_dp = p + v*dt + R*dp + 0.5*g*dt*dt;
+ v_plus_dv = v + R*dv + g*dt;
+ th_plus_dth = th + dth; //Angles simply sum
+}
+
+template<typename D1, typename D2, typename D3, class T>
+inline void composeOverState(const MatrixBase<D1>& x,
+ const MatrixBase<D2>& d,
+ T dt,
+ MatrixBase<D3>& x_plus_d)
+{
+ MatrixSizeCheck<5, 1>::check(x);
+ MatrixSizeCheck<5, 1>::check(d);
+ MatrixSizeCheck<5, 1>::check(x_plus_d);
+
+ Map<const Matrix<typename D1::Scalar, 2, 1> > p ( & x( 0 ) );
+ Map<const Matrix<typename D1::Scalar, 2, 1> > v ( & x( 3 ) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > dp ( & d( 0 ) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > dv ( & d( 3 ) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > p_plus_d ( & x_plus_d( 0 ) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > v_plus_d ( & x_plus_d( 3 ) );
+
+ composeOverState( p, x(2), v,
+ dp, d(2), dv,
+ dt,
+ p_plus_d, x_plus_d(2), v_plus_d);
+}
+
+//Versions of composeOverState with parameter g are used in the slanted plane case
+template<typename D1, typename D2, typename D3, typename D4, class T>
+inline void composeOverStateWithGravity(const MatrixBase<D1>& x,
+ const MatrixBase<D2>& d,
+ T dt,
+ const MatrixBase<D3>& g,
+ MatrixBase<D4>& x_plus_d)
+{
+ MatrixSizeCheck<5, 1>::check(x);
+ MatrixSizeCheck<5, 1>::check(d);
+ MatrixSizeCheck<2, 1>::check(g);
+ MatrixSizeCheck<5, 1>::check(x_plus_d);
+
+ Map<const Matrix<typename D1::Scalar, 2, 1> > p ( & x( 0 ) );
+ Map<const Matrix<typename D1::Scalar, 2, 1> > v ( & x( 3 ) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > dp ( & d( 0 ) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > dv ( & d( 3 ) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > p_plus_d ( & x_plus_d( 0 ) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > v_plus_d ( & x_plus_d( 3 ) );
+
+ composeOverStateWithGravity( p, x(2), v,
+ dp, d(2), dv,
+ dt, g,
+ p_plus_d, x_plus_d(2), v_plus_d);
+}
+
+template<typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 5, 1> composeOverState(const MatrixBase<D1>& x,
+ const MatrixBase<D2>& d,
+ T dt)
+{
+ Matrix<typename D1::Scalar, 5, 1> ret;
+ composeOverState(x, d, dt, ret);
+ return ret;
+}
+
+//Versions of composeOverState with parameter g are used in the slanted plane case
+template<typename D1, typename D2, typename D3, class T>
+inline Matrix<typename D1::Scalar, 5, 1> composeOverStateWithGravity(const MatrixBase<D1>& x,
+ const MatrixBase<D2>& d,
+ T dt,
+ MatrixBase<D3>& g)
+{
+ Matrix<typename D1::Scalar, 5, 1> ret;
+ MatrixSizeCheck<5, 1>::check(x);
+ MatrixSizeCheck<5, 1>::check(d);
+ MatrixSizeCheck<2, 1>::check(g);
+
+ composeOverStateWithGravity(x, d, dt, g, ret);
+ return ret;
+}
+
+template<class T>
+inline void composeOverState(const VectorComposite& x,
+ const VectorComposite& d,
+ T dt,
+ VectorComposite& x_plus_d)
+{
+ assert(x_plus_d.count('P') && "provided reference does not have key 'P'");
+ assert(x_plus_d.count('O') && "provided reference does not have key 'O'");
+ assert(x_plus_d.count('V') && "provided reference does not have key 'V'");
+
+ composeOverState(x.at('P'), x.at('O')(0), x.at('V'),
+ d.at('P'), d.at('O')(0), d.at('V'),
+ dt,
+ x_plus_d['P'], x_plus_d['O'](0), x_plus_d['V']);
+}
+
+template<class T>
+inline void composeOverStateWithGravity(const VectorComposite& x,
+ const VectorComposite& d,
+ T dt,
+ const VectorComposite& g,
+ VectorComposite& x_plus_d)
+{
+ assert(x_plus_d.count('P') && "provided reference does not have key 'P'");
+ assert(x_plus_d.count('O') && "provided reference does not have key 'O'");
+ assert(x_plus_d.count('V') && "provided reference does not have key 'V'");
+
+ composeOverStateWithGravity( x.at('P'), x.at('O')(0), x.at('V'),
+ d.at('P'), d.at('O')(0), d.at('V'),
+ dt,
+ g.at('G'),
+ x_plus_d['P'], x_plus_d['O'](0), x_plus_d['V']);
+}
+
+template<class T>
+inline VectorComposite composeOverState(const VectorComposite& x,
+ const VectorComposite& d,
+ T dt)
+{
+ VectorComposite ret("POV", {2,1,2});
+
+ composeOverState(x, d, dt, ret);
+ return ret;
+}
+
+template<class T>
+inline VectorComposite composeOverStateWithGravity(const VectorComposite& x,
+ const VectorComposite& d,
+ T dt,
+ const VectorComposite& g)
+{
+ VectorComposite ret("POV", {2,1,2});
+
+ composeOverStateWithGravity(x, d, dt, g, ret);
+ return ret;
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, class T1, class T2, class T3, class T4>
+inline void betweenStates(const MatrixBase<D1>& p1, const T1& th1, const MatrixBase<D2>& v1,
+ const MatrixBase<D3>& p2, const T2& th2, const MatrixBase<D4>& v2,
+ const T3 dt,
+ MatrixBase<D5>& diff_p, T4& diff_th, MatrixBase<D6>& diff_v )
+{
+ MatrixSizeCheck<2, 1>::check(p1);
+ MatrixSizeCheck<2, 1>::check(v1);
+ MatrixSizeCheck<2, 1>::check(p2);
+ MatrixSizeCheck<2, 1>::check(v2);
+ MatrixSizeCheck<2, 1>::check(diff_p);
+ MatrixSizeCheck<2, 1>::check(diff_v);
+
+ const auto& dR1_tr = Rotation2D<T1>(-th1).matrix();
+ diff_p = dR1_tr * ( p2 - p1 - v1*dt );
+ diff_v = dR1_tr * ( v2 - v1 );
+ diff_th = pi2pi(th2 - th1);
+}
+
+//Versions of betweenStates with parameter g are used in the slanted plane case
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, class T1, class T2, class T3, class T4>
+inline void betweenStatesWithGravity(const MatrixBase<D1>& p1, const T1& th1, const MatrixBase<D2>& v1,
+ const MatrixBase<D3>& p2, const T2& th2, const MatrixBase<D4>& v2,
+ const T3 dt, MatrixBase<D5>& g,
+ MatrixBase<D6>& diff_p, T4& diff_th, MatrixBase<D7>& diff_v )
+{
+ MatrixSizeCheck<2, 1>::check(p1);
+ MatrixSizeCheck<2, 1>::check(v1);
+ MatrixSizeCheck<2, 1>::check(p2);
+ MatrixSizeCheck<2, 1>::check(v2);
+ MatrixSizeCheck<2, 1>::check(g);
+ MatrixSizeCheck<2, 1>::check(diff_p);
+ MatrixSizeCheck<2, 1>::check(diff_v);
+
+ const auto& dR1_tr = Rotation2D<T1>(-th1).matrix();
+ diff_p = dR1_tr * ( p2 - p1 - v1*dt - 0.5*g*dt*dt );
+ diff_v = dR1_tr * ( v2 - v1 - g*dt);
+ diff_th = pi2pi(th2 - th1);
+}
+
+template<typename D1, typename D2, typename D3, class T>
+inline void betweenStates(const MatrixBase<D1>& x1,
+ const MatrixBase<D2>& x2,
+ T dt,
+ MatrixBase<D3>& x2_minus_x1)
+{
+ MatrixSizeCheck<5, 1>::check(x1);
+ MatrixSizeCheck<5, 1>::check(x2);
+ MatrixSizeCheck<5, 1>::check(x2_minus_x1);
+
+ Map<const Matrix<typename D1::Scalar, 2, 1> > p1 ( & x1(0) );
+ Map<const Matrix<typename D1::Scalar, 2, 1> > v1 ( & x1(3) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > p2 ( & x2(0) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > v2 ( & x2(3) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > dp ( & x2_minus_x1(0) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > dv ( & x2_minus_x1(3) );
+
+ betweenStates(p1, x1(2), v1, p2, x2(2), v2, dt, dp, x2_minus_x1(2), dv);
+}
+
+//Versions of betweenStates with parameter g are used in the slanted plane case
+template<typename D1, typename D2, typename D3, typename D4, class T>
+inline void betweenStatesWithGravity(const MatrixBase<D1>& x1,
+ const MatrixBase<D2>& x2,
+ T dt, MatrixBase<D3>& g,
+ MatrixBase<D4>& x2_minus_x1)
+{
+ MatrixSizeCheck<5, 1>::check(x1);
+ MatrixSizeCheck<5, 1>::check(x2);
+ MatrixSizeCheck<2, 1>::check(g);
+ MatrixSizeCheck<5, 1>::check(x2_minus_x1);
+
+ Map<const Matrix<typename D1::Scalar, 2, 1> > p1 ( & x1(0) );
+ Map<const Matrix<typename D1::Scalar, 2, 1> > v1 ( & x1(3) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > p2 ( & x2(0) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > v2 ( & x2(3) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > dp ( & x2_minus_x1(0) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > dv ( & x2_minus_x1(3) );
+
+ betweenStatesWithGravity(p1, x1(2), v1, p2, x2(2), v2, dt, g, dp, x2_minus_x1(2), dv);
+}
+
+template<typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 5, 1> betweenStates(const MatrixBase<D1>& x1,
+ const MatrixBase<D2>& x2,
+ T dt)
+{
+ Matrix<typename D1::Scalar, 5, 1> ret;
+ betweenStates(x1, x2, dt, ret);
+ return ret;
+}
+
+//Versions of betweenStates with parameter g are used in the slanted plane case
+template<typename D1, typename D2, typename D3, class T>
+inline Matrix<typename D1::Scalar, 5, 1> betweenStatesWithGravity(const MatrixBase<D1>& x1,
+ const MatrixBase<D2>& x2,
+ T dt,
+ MatrixBase<D3>& g)
+{
+ Matrix<typename D1::Scalar, 5, 1> ret;
+ betweenStatesWithGravity(x1, x2, dt, g, ret);
+ return ret;
+}
//
//template<typename Derived>
//Matrix<typename Derived::Scalar, 9, 1> log_Imu(const MatrixBase<Derived>& delta_in)
diff --git a/include/imu/sensor/sensor_imu2d.h b/include/imu/sensor/sensor_imu2d.h
index 880c9be07043a5f30da2df995e9e5d42ef8d9092..fb9bc31fab1b4e37322cbd6547dfe59700af5bb8 100644
--- a/include/imu/sensor/sensor_imu2d.h
+++ b/include/imu/sensor/sensor_imu2d.h
@@ -46,30 +46,51 @@ struct ParamsSensorImu2d : public ParamsSensorBase
double ab_rate_stdev = 0.00001;
double wb_rate_stdev = 0.00001;
+ // Is the plane flat?
+ bool orthogonal_gravity = true;
+
~ParamsSensorImu2d() override = default;
ParamsSensorImu2d()
{
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
}
+
+ ParamsSensorImu2d(bool gravity)
+ {
+ //ONLY FOR TESTING
+ w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
+ a_noise = 0.04; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
+
+ ab_initial_stdev = 0.01; //accelerometer micro_g/sec
+ wb_initial_stdev = 0.01; //gyroscope rad/sec
+
+ ab_rate_stdev = 0.00001;
+ wb_rate_stdev = 0.00001;
+
+ orthogonal_gravity = gravity;
+ }
+
ParamsSensorImu2d(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
- w_noise = _server.getParam<double>(prefix + _unique_name + "/w_noise");
- a_noise = _server.getParam<double>(prefix + _unique_name + "/a_noise");
- ab_initial_stdev = _server.getParam<double>(prefix + _unique_name + "/ab_initial_stdev");
- wb_initial_stdev = _server.getParam<double>(prefix + _unique_name + "/wb_initial_stdev");
- ab_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/ab_rate_stdev");
- wb_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/wb_rate_stdev");
+ w_noise = _server.getParam<double>(prefix + _unique_name + "/w_noise");
+ a_noise = _server.getParam<double>(prefix + _unique_name + "/a_noise");
+ ab_initial_stdev = _server.getParam<double>(prefix + _unique_name + "/ab_initial_stdev");
+ wb_initial_stdev = _server.getParam<double>(prefix + _unique_name + "/wb_initial_stdev");
+ ab_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/ab_rate_stdev");
+ wb_rate_stdev = _server.getParam<double>(prefix + _unique_name + "/wb_rate_stdev");
+ orthogonal_gravity = _server.getParam<bool>(prefix + _unique_name + "/orthogonal_gravity");
}
std::string print() const override
{
return ParamsSensorBase::print() + "\n"
- + "w_noise: " + std::to_string(w_noise) + "\n"
- + "a_noise: " + std::to_string(a_noise) + "\n"
- + "ab_initial_stdev: " + std::to_string(ab_initial_stdev) + "\n"
- + "wb_initial_stdev: " + std::to_string(wb_initial_stdev) + "\n"
- + "ab_rate_stdev: " + std::to_string(ab_rate_stdev) + "\n"
- + "wb_rate_stdev: " + std::to_string(wb_rate_stdev) + "\n";
+ + "w_noise: " + std::to_string(w_noise) + "\n"
+ + "a_noise: " + std::to_string(a_noise) + "\n"
+ + "ab_initial_stdev: " + std::to_string(ab_initial_stdev) + "\n"
+ + "wb_initial_stdev: " + std::to_string(wb_initial_stdev) + "\n"
+ + "ab_rate_stdev: " + std::to_string(ab_rate_stdev) + "\n"
+ + "wb_rate_stdev: " + std::to_string(wb_rate_stdev) + "\n"
+ + "orthogonal_gravity: " + std::to_string(orthogonal_gravity) + "\n";
}
};
@@ -88,6 +109,8 @@ class SensorImu2d : public SensorBase
double ab_rate_stdev; //accelerometer m/sec^2 / sqrt(sec)
double wb_rate_stdev; //gyroscope rad/sec / sqrt(sec)
+ bool orthogonal_gravity; // Is the 2D plane orthogonal to gravity?
+
public:
SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params);
@@ -138,7 +161,7 @@ inline double SensorImu2d::getAbRateStdev() const
inline bool SensorImu2d::isGravityOrthogonal() const
{
- return true;
+ return orthogonal_gravity;
}
} // namespace wolf
diff --git a/src/processor/processor_imu2d.cpp b/src/processor/processor_imu2d.cpp
index a1efbcf5861a232703aed6ab4a63afdf1dbd9c99..036d488e88371d3f4a9628452f1ecd7e7f0e172a 100644
--- a/src/processor/processor_imu2d.cpp
+++ b/src/processor/processor_imu2d.cpp
@@ -19,9 +19,16 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
+/*
+ * processor_imu2d.cpp
+ *
+ * Created on: Nov 16, 2020
+ * Author: igeer
+ */
// imu
#include "imu/processor/processor_imu2d.h"
#include "imu/factor/factor_imu2d.h"
+#include "imu/factor/factor_imu2d_with_gravity.h"
#include "imu/math/imu2d_tools.h"
// wolf
@@ -122,9 +129,10 @@ namespace wolf {
CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
FeatureImu2dPtr ftr_imu = std::static_pointer_cast<FeatureImu2d>(_feature_motion);
- auto fac_imu = FactorBase::emplace<FactorImu2d>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
-
- return fac_imu;
+ if( std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal() )
+ return FactorBase::emplace<FactorImu2d>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ else
+ return FactorBase::emplace<FactorImu2dWithGravity>(_feature_motion, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
}
void ProcessorImu2d::computeCurrentDelta(const Eigen::VectorXd& _data,
@@ -206,7 +214,10 @@ namespace wolf {
*
* In the absence of gravity (2D case) it's the same as deltaPlusDelta(), so the same compose() function is used
*/
- _x_plus_delta = imu2d::compose(_x, delta, _dt);
+ if( std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal() )
+ _x_plus_delta = imu2d::composeOverState(_x, delta, _dt);
+ else
+ _x_plus_delta = imu2d::composeOverStateWithGravity(_x, delta, _dt, getSensor()->getState("G"));
}
diff --git a/src/sensor/sensor_imu2d.cpp b/src/sensor/sensor_imu2d.cpp
index e307e6ec245d12e82ec6bb8b5b5bc7d7f94708fa..f74b3baa23383ea5265d7d288abc8d97d225a0ff 100644
--- a/src/sensor/sensor_imu2d.cpp
+++ b/src/sensor/sensor_imu2d.cpp
@@ -38,9 +38,14 @@ SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorI
ab_initial_stdev(_params.ab_initial_stdev),
wb_initial_stdev(_params.wb_initial_stdev),
ab_rate_stdev(_params.ab_rate_stdev),
- wb_rate_stdev(_params.wb_rate_stdev)
+ wb_rate_stdev(_params.wb_rate_stdev),
+ orthogonal_gravity(_params.orthogonal_gravity)
{
assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2d.");
+ if(!orthogonal_gravity)
+ {
+ addStateBlock('G', std::make_shared<StateBlock>(2, false), false);
+ }
}
SensorImu2d::~SensorImu2d()
diff --git a/src/yaml/sensor_imu2d_yaml.cpp b/src/yaml/sensor_imu2d_yaml.cpp
index 23aee8a13778464ad2ef877494e376e43f4b814e..a138b56bf11564a794893c872eecb297d10a1634 100644
--- a/src/yaml/sensor_imu2d_yaml.cpp
+++ b/src/yaml/sensor_imu2d_yaml.cpp
@@ -59,7 +59,13 @@ static ParamsSensorBasePtr createParamsSensorImu2d(const std::string & _filename
params->wb_initial_stdev = variances["wb_initial_stdev"] .as<double>();
params->ab_rate_stdev = variances["ab_rate_stdev"] .as<double>();
params->wb_rate_stdev = variances["wb_rate_stdev"] .as<double>();
-
+ try
+ {
+ params->orthogonal_gravity = variances["orthogonal_gravity"].as<bool>();
+ }
+ catch(...){
+ WOLF_INFO("orthogonal_gravity parameter not found, using true");
+ }
return params;
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 893aaadb8400712867b4f3df2e223b2c3fb0e4a9..bc10d2bf34366c9dbd624d62a8fffa92b1fb2598 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -19,6 +19,9 @@ target_link_libraries(gtest_processor_imu ${PLUGIN_NAME})
wolf_add_gtest(gtest_processor_imu2d gtest_processor_imu2d.cpp)
target_link_libraries(gtest_processor_imu2d ${PLUGIN_NAME})
+wolf_add_gtest(gtest_processor_imu2d_with_gravity gtest_processor_imu2d_with_gravity.cpp)
+target_link_libraries(gtest_processor_imu2d_with_gravity ${PLUGIN_NAME} ${wolf_LIBRARY})
+
wolf_add_gtest(gtest_imu gtest_imu.cpp)
target_link_libraries(gtest_imu ${PLUGIN_NAME})
@@ -43,6 +46,9 @@ target_link_libraries(gtest_imu_static_init ${PLUGIN_NAME})
wolf_add_gtest(gtest_imu2d_static_init gtest_imu2d_static_init.cpp)
target_link_libraries(gtest_imu2d_static_init ${PLUGIN_NAME})
+wolf_add_gtest(gtest_factor_imu2d_with_gravity gtest_factor_imu2d_with_gravity.cpp)
+target_link_libraries(gtest_factor_imu2d_with_gravity ${PLUGIN_NAME} ${wolf_LIBRARY})
+
wolf_add_gtest(gtest_imu_mocap_fusion gtest_imu_mocap_fusion.cpp)
target_link_libraries(gtest_imu_mocap_fusion ${PLUGIN_NAME})
diff --git a/test/gtest_factor_imu2d_with_gravity.cpp b/test/gtest_factor_imu2d_with_gravity.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..931fcd3db298b8aa27e4f0a9edf189fbc98f9bb5
--- /dev/null
+++ b/test/gtest_factor_imu2d_with_gravity.cpp
@@ -0,0 +1,605 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+
+#include "imu/factor/factor_imu2d_with_gravity.h"
+#include "imu/math/imu2d_tools.h"
+#include "imu/sensor/sensor_imu2d.h"
+
+using namespace Eigen;
+using namespace wolf;
+
+// Input odometry data and covariance
+Matrix6d data_cov = 0.1*Matrix6d::Identity();
+Matrix5d delta_cov = 0.1*Matrix5d::Identity();
+
+// Jacobian of the bias
+MatrixXd jacobian_bias((MatrixXd(5,3) << 1, 0, 0,
+ 0, 1, 0,
+ 0, 0, 1,
+ 1, 0, 0,
+ 0, 1, 0 ).finished());
+//preintegration bias
+Vector3d b0_preint = Vector3d::Zero();
+
+// Problem and solver
+ProblemPtr problem_ptr = Problem::create("POV", 2);
+SolverCeres solver(problem_ptr);
+
+// Two frames
+FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector5d::Zero());
+FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector5d::Zero());
+
+// Imu2d sensor
+ParamsSensorImu2dPtr sensorparams = std::make_shared<ParamsSensorImu2d>(false);
+SensorBasePtr sensor = SensorBase::emplace<SensorImu2d>(problem_ptr->getHardware(), Vector3d::Zero(), *sensorparams );
+
+//capture from frm1 to frm0
+auto cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
+auto cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
+
+TEST(FactorImu2dWithGravity, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorImu2dWithGravity, bias_zero_solve_f1)
+{
+ for(int i = 0; i < 50; i++){
+ //WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ //Random gravity
+ Vector2d g = Vector2d::Random()*5;
+ //Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and biases, perturb frm1
+ frm0->fix();
+ cap0->fix();
+ frm1->unfix();
+ cap1->fix();
+ sensor->getStateBlock('G')->fix();
+ frm1->perturb(0.01);
+
+ // solve
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+ //std::cout << report << std::endl;
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+ //WOLF_INFO(frm1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+}
+}
+
+TEST(FactorImu2dWithGravity, bias_zero_solve_f0)
+{
+ for(int i = 0; i < 50; i++){
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ //Random gravity
+ Vector2d g = Vector2d::Random()*5;
+ //Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1 and biases, perturb frm0
+ frm0->unfix();
+ cap0->fix();
+ frm1->fix();
+ cap1->fix();
+ sensor->getStateBlock('G')->fix();
+ frm0->perturb(0.01);
+
+ // solve
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+ //WOLF_INFO(frm1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+}
+}
+
+TEST(FactorImu2dWithGravity, bias_zero_solve_b1)
+{
+ for(int i = 0; i < 50; i++){
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ //Random gravity
+ Vector2d g = Vector2d::Random()*5;
+ //Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ //0 Initial bias
+ Vector3d b0 = Vector3d::Zero();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
+ frm0->fix();
+ cap0->fix();
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+ cap1->unfix();
+ cap1->perturb(0.01);
+
+ // solve to estimate bias at cap1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
+ //WOLF_INFO(cap1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorImu2dWithGravity, solve_b0)
+{
+ for(int i = 0; i < 50; i++){
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ //Random gravity
+ Vector2d g = Vector2d::Random()*5;
+ //Vector2d g = Vector2d::Zero();
+
+ //Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ //Corrected delta
+ Vector5d delta_step = jacobian_bias*(b0-b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ frm0->fix();
+ cap0->unfix();
+ frm1->fix();
+ cap1->fix();
+ sensor->getStateBlock('G')->fix();
+ cap0->perturb(0.1);
+
+ // solve
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(cap0->getStateVector(), b0, 1e-6);
+ //WOLF_INFO(cap0->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorImu2dWithGravity, solve_b1)
+{
+ for(int i = 0; i < 50; i++){
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ //Random gravity
+ Vector2d g = Vector2d::Random()*5;
+ //Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ //0 Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and frm1, unfix bias, perturb cap1
+ frm0->fix();
+ cap0->fix();
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+ cap1->unfix();
+ cap1->perturb(0.01);
+
+ // solve to estimate bias at cap1
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
+ //WOLF_INFO(cap1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+}
+}
+
+TEST(FactorImu2dWithGravity, solve_f0)
+{
+ for(int i = 0; i < 50; i++){
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ //Random gravity
+ Vector2d g = Vector2d::Random()*5;
+ //Vector2d g = Vector2d::Zero();
+
+ //Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ //Corrected delta
+ Vector5d delta_step = jacobian_bias*(b0-b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ frm0->unfix();
+ cap0->fix();
+ frm1->fix();
+ cap1->fix();
+ sensor->getStateBlock('G')->fix();
+ frm0->perturb(0.1);
+
+ // solve
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+ //WOLF_INFO(frm0->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+}
+}
+
+TEST(FactorImu2dWithGravity, solve_f1)
+{
+ for(int i = 0; i < 50; i++){
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ //Random gravity
+ Vector2d g = Vector2d::Random()*5;
+ //Vector2d g = Vector2d::Zero();
+
+ //Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ //Corrected delta
+ Vector5d delta_step = jacobian_bias*(b0-b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ frm0->fix();
+ cap0->fix();
+ frm1->unfix();
+ cap1->fix();
+ sensor->getStateBlock('G')->fix();
+ frm1->perturb(0.1);
+
+ // solve
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+ //WOLF_INFO(frm0->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorImu2dWithGravity, solve_f1_b1)
+{
+ for(int i = 0; i < 50; i++){
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ //Random gravity
+ Vector2d g = Vector2d::Random()*5;
+ //Vector2d g = Vector2d::Zero();
+
+ //Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ //Corrected delta
+ Vector5d delta_step = jacobian_bias*(b0-b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ frm0->fix();
+ cap0->fix();
+ sensor->getStateBlock('G')->fix();
+ frm1->unfix();
+ cap1->unfix();
+ frm1->perturb(0.1);
+ cap1->perturb(0.1);
+
+ // solve
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+ ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
+ //WOLF_INFO(frm0->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+TEST(FactorImu2dWithGravity, bias_zero_solve_G)
+{
+ for(int i = 0; i < 50; i++){
+ //WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ //Random gravity
+ //Vector2d g = Vector2d::Random()*5;
+ Vector2d g = Vector2d::Zero();
+
+ //Zero bias
+ Vector3d b0 = Vector3d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frames and biases, perturb g
+ frm0->fix();
+ cap0->fix();
+ frm1->fix();
+ cap1->fix();
+ sensor->getStateBlock('G')->unfix();
+ sensor->getStateBlock('G')->perturb(1);
+
+ // solve
+ std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
+ //WOLF_INFO(frm1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+TEST(FactorImu2dWithGravity, solve_G)
+{
+ for(int i = 0; i < 50; i++){
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ //Random gravity
+ Vector2d g = Vector2d::Random()*5;
+ //Vector2d g = Vector2d::Zero();
+
+ //Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ //Corrected delta
+ Vector5d delta_step = jacobian_bias*(b0-b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frames and captures, perturb g
+ frm0->fix();
+ cap0->fix();
+ frm1->fix();
+ cap1->fix();
+ sensor->getStateBlock('G')->unfix();
+ sensor->getStateBlock('G')->perturb(1);
+
+ // solve
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
+ //WOLF_INFO(cap0->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ // ::testing::GTEST_FLAG(filter) = "FactorImu2dWithGravity.no_bias_fixed"; // Test only this one
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_imu2d_tools.cpp b/test/gtest_imu2d_tools.cpp
index 2386b7c945a9fba5782dea5cd6fc09da7d89468c..964cdbee899e1e83760b062a2caf531614c545e4 100644
--- a/test/gtest_imu2d_tools.cpp
+++ b/test/gtest_imu2d_tools.cpp
@@ -140,34 +140,83 @@ TEST(Imu2d_tools, compose_between)
ASSERT_MATRIX_APPROX(diffx, inverse(dx2, dt), 1e-10);
}
-//TEST(Imu2d_tools, compose_between_with_state)
-//{
-// VectorXd x(10), d(10), x2(10), x3(10), d2(10), d3(10);
-// Vector4d qv;
-// double dt = 0.1;
-//
-// qv = (Vector4d() << 3, 4, 5, 6).finished().normalized();
-// x << 0, 1, 2, qv, 7, 8, 9;
-// qv = (Vector4d() << 6, 5, 4, 3).finished().normalized();
-// d << 9, 8, 7, qv, 2, 1, 0;
-//
-// composeOverState(x, d, dt, x2);
-// x3 = composeOverState(x, d, dt);
-// ASSERT_MATRIX_APPROX(x3, x2, 1e-10);
-//
-// // betweenStates(x, x2) should recover d
-// betweenStates(x, x2, dt, d2);
-// d3 = betweenStates(x, x2, dt);
-// ASSERT_MATRIX_APPROX(d2, d, 1e-10);
-// ASSERT_MATRIX_APPROX(d3, d, 1e-10);
-// ASSERT_MATRIX_APPROX(betweenStates(x, x2, dt), d, 1e-10);
-//
-// // x + (x2 - x) = x2
-// ASSERT_MATRIX_APPROX(composeOverState(x, betweenStates(x, x2, dt), dt), x2, 1e-10);
-//
-// // (x + d) - x = d
-// ASSERT_MATRIX_APPROX(betweenStates(x, composeOverState(x, d, dt), dt), d, 1e-10);
-//}
+TEST(Imu2d_tools, compose_between_with_state)
+{
+ VectorXd x1(5), d(5), x2(5), x3(5), d2(5), d3(5);
+ double dt = 0.1;
+
+ x1 << 0, 1, pi2pi(2), 3, 4;
+ d << 4, 3, pi2pi(2), 1, 0;
+
+ composeOverState(x1, d, dt, x2);
+ x3 = composeOverState(x1, d, dt);
+ ASSERT_MATRIX_APPROX(x3, x2, 1e-10);
+
+ // betweenStates(x, x2) should recover d
+ betweenStates(x1, x2, dt, d2);
+ d3 = betweenStates(x1, x2, dt);
+ ASSERT_MATRIX_APPROX(d2, d, 1e-10);
+ ASSERT_MATRIX_APPROX(d3, d, 1e-10);
+ ASSERT_MATRIX_APPROX(betweenStates(x1, x2, dt), d, 1e-10);
+
+ // x1 <+> (x2 <-> x1) = x2
+ ASSERT_MATRIX_APPROX(composeOverState(x1, betweenStates(x1, x2, dt), dt), x2, 1e-10);
+
+ // (x1 <+> d) <-> x1 = d
+ ASSERT_MATRIX_APPROX(betweenStates(x1, composeOverState(x1, d, dt), dt), d, 1e-10);
+}
+
+TEST(imu2d_tools, betweencomposeOverStateWithGravity_D000)
+{
+ VectorXd x0(5), x1(5), d(5), g(2);
+ double dt = 0.1;
+ d.setZero();
+
+ x0 << 1, 2, pi2pi(3), 0, 0;
+ g << 1, -3;
+ x1.head(2) = x0.head(2) + x0.tail(2)*dt + 0.5*g*dt*dt;
+ x1(2) = x0(2) + d(2);
+ x1.tail(2) = x0.tail(2) + g*dt;
+
+ ASSERT_MATRIX_APPROX(x1, composeOverStateWithGravity(x0, d, dt, g), 1e-10);
+ ASSERT_MATRIX_APPROX(d, betweenStatesWithGravity(x0, x1, dt, g), 1e-10);
+
+}
+
+TEST(imu2d_tools, betweencomposeOverStateWithGravity_D0X0)
+{
+ VectorXd x0(5), x1(5), d(5), g(2);
+ double dt = 0.1;
+ d.setZero();
+ d(2) = 0.5;
+
+ x0 << 1, 2, pi2pi(3), 0, 0;
+ g << 1, -3;
+ x1.head(2) = x0.head(2) + x0.tail(2)*dt + 0.5*g*dt*dt;
+ x1(2) = x0(2) + d(2);
+ x1.tail(2) = x0.tail(2) + g*dt;
+
+ ASSERT_MATRIX_APPROX(x1, composeOverStateWithGravity(x0, d, dt, g), 1e-10);
+ ASSERT_MATRIX_APPROX(d, betweenStatesWithGravity(x0, x1, dt, g), 1e-10);
+
+}
+
+TEST(imu2d_tools, betweencomposeOverStateWithGravity_DXXX)
+{
+ VectorXd x0(5), x1(5), d(5), g(2);
+ double dt = 0.1;
+
+ x0 << 1, 2, pi2pi(3), 4, 5;
+ d << 6, 7, pi2pi(8), 9, 10;
+ g << 11, -12;
+ x1 = composeOverStateWithGravity(x0, d, dt, g);
+
+ ASSERT_MATRIX_APPROX(d, betweenStatesWithGravity(x0, x1, dt, g), 1e-10);
+
+ d = betweenStatesWithGravity(x0, x1, dt, g);
+ ASSERT_MATRIX_APPROX(x1, composeOverStateWithGravity(x0, d, dt, g), 1e-10);
+
+}
//
//TEST(Imu2d_tools, lift_retract)
//{
diff --git a/test/gtest_processor_imu2d_with_gravity.cpp b/test/gtest_processor_imu2d_with_gravity.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e62db80ef6c9bcd213eaf516ede25c482c35bb9b
--- /dev/null
+++ b/test/gtest_processor_imu2d_with_gravity.cpp
@@ -0,0 +1,228 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+/**
+ * \file gtest_processor_imu2d.cpp
+ *
+ * Created on: Nov 24, 2020
+ * \author: igeer
+ */
+
+#include "imu/internal/config.h"
+#include "imu/capture/capture_imu.h"
+#include "imu/processor/processor_imu2d.h"
+#include "imu/sensor/sensor_imu2d.h"
+
+// #include "core/common/wolf.h"
+
+#include <core/utils/utils_gtest.h>
+#include "core/utils/logging.h"
+
+#include "core/math/rotations.h"
+#include <cmath>
+#include <iostream>
+
+using namespace Eigen;
+using namespace wolf;
+
+class ProcessorImu2dTest : public testing::Test
+{
+
+ public: //These can be accessed in fixtures
+ wolf::ProblemPtr problem;
+ wolf::SensorBasePtr sensor_ptr;
+ wolf::ProcessorMotionPtr processor_motion;
+ wolf::TimeStamp t;
+ wolf::TimeStamp t0;
+ double mti_clock, tmp;
+ double dt;
+ Vector6d data;
+ Matrix6d data_cov;
+ Vector2d g;
+ VectorComposite x0c; // initial state composite
+ VectorComposite s0c; // initial sigmas composite
+ std::shared_ptr<wolf::CaptureImu> C;
+
+ //a new of this will be created for each new test
+ void SetUp() override
+ {
+ using namespace Eigen;
+ using std::shared_ptr;
+ using std::make_shared;
+ using std::static_pointer_cast;
+ using namespace wolf::Constants;
+
+ std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+
+ // Wolf problem
+ problem = Problem::create("POV", 2);
+ Vector3d extrinsics = (Vector3d() << 0,0, 0).finished();
+ sensor_ptr = problem->installSensor("SensorImu2d", "Main Imu", extrinsics, wolf_root + "/demos/sensor_imu2d_with_gravity.yaml");
+ ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu2d.yaml");
+ processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr);
+
+ // Time and data variables
+ dt = 0.01;
+ t0.set(0);
+ t = t0;
+ data = Vector6d::Zero();
+ data_cov = Matrix6d::Identity();
+
+ // Create one capture to store the Imu data arriving from (sensor / callback / file / etc.)
+ C = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector3d::Zero());
+ }
+
+};
+
+
+TEST_F(ProcessorImu2dTest, MUA_Only_G)
+{
+ data << 0, 0, 0, 0, 0, 0;
+ data_cov *= 1e-3;
+ // Set the origin
+ x0c['P'] = Vector2d(1,2);
+ x0c['O'] = Vector1d(0);
+ x0c['V'] = Vector2d(4,5);
+ auto KF0 = problem->setPriorFix(x0c, t0);
+ processor_motion->setOrigin(KF0);
+
+ // Set the gravity
+ g << 6, 7;
+ sensor_ptr->getStateBlock('G')->setState(g);
+
+ //WOLF_DEBUG("Current State: ", problem->getState());
+ for(t = t0+dt; t <= t0 + 1.0 + dt/2; t+=dt){
+ C->setTimeStamp(t);
+ C->setData(data);
+ C->setDataCovariance(data_cov);
+ C->process();
+ //WOLF_INFO("Current State: ", problem->getState()['P'].transpose());
+ //WOLF_INFO((x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2)).transpose());
+ ASSERT_MATRIX_APPROX(x0c['P'] + x0c['V']*(t-t0) + 0.5*g*std::pow(t-t0, 2), problem->getState()['P'], 1e-9);
+ EXPECT_MATRIX_APPROX(x0c['V'] + g*(t-t0), problem->getState()['V'], 1e-9);
+ }
+}
+
+TEST_F(ProcessorImu2dTest, MUA)
+{
+ data << 1, 2, 0, 0, 0, 0;
+ data_cov *= 1e-3;
+ // Set the origin
+ x0c['P'] = Vector2d(1,2);
+ x0c['O'] = Vector1d(0);
+ x0c['V'] = Vector2d(4,5);
+ auto KF0 = problem->setPriorFix(x0c, t0);
+ processor_motion->setOrigin(KF0);
+
+ // Set the gravity
+ g << 6, 7;
+ sensor_ptr->getStateBlock('G')->setState(g);
+
+ //WOLF_DEBUG("Current State: ", problem->getState());
+ for(t = t0+dt; t <= t0 + 1.0 + dt/2; t+=dt){
+ C->setTimeStamp(t);
+ C->setData(data);
+ C->setDataCovariance(data_cov);
+ C->process();
+ //WOLF_INFO("Current State: ", problem->getState()['P'].transpose());
+ //WOLF_INFO((x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2)).transpose());
+ ASSERT_MATRIX_APPROX(x0c['P'] + x0c['V']*(t-t0) + 0.5*(data.head(2)+g)*std::pow(t-t0, 2), problem->getState()['P'], 1e-9);
+ EXPECT_MATRIX_APPROX(x0c['V'] + (data.head(2)+g)*(t-t0), problem->getState()['V'], 1e-9);
+ }
+}
+
+TEST_F(ProcessorImu2dTest, Spinning)
+{
+ double w = 0.5;
+ data << 0, 0, 0, 0, 0, w;
+ data_cov *= 1e-3;
+ // Set the origin
+ x0c['P'] = Vector2d(1,2);
+ x0c['O'] = Vector1d(0);
+ x0c['V'] = Vector2d(4,5);
+ auto KF0 = problem->setPriorFix(x0c, t0);
+ processor_motion->setOrigin(KF0);
+
+ // Set the gravity
+ g << 6, 7;
+ sensor_ptr->getStateBlock('G')->setState(g);
+
+ //WOLF_DEBUG("Current State: ", problem->getState());
+ for(t = t0+dt; t <= t0 + 1.0 + dt/2; t+=dt){
+ C->setTimeStamp(t);
+ C->setData(data);
+ C->setDataCovariance(data_cov);
+ C->process();
+ //WOLF_INFO("Current State: ", problem->getState()['P'].transpose());
+ //WOLF_INFO((x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2)).transpose());
+ ASSERT_MATRIX_APPROX(x0c['P'] + x0c['V']*(t-t0) + 0.5*g*std::pow(t-t0, 2), problem->getState()['P'], 1e-9);
+ EXPECT_MATRIX_APPROX(x0c['V'] + g*(t-t0), problem->getState()['V'], 1e-9);
+ EXPECT_NEAR(x0c['O'](0) + w*(t-t0), problem->getState()['O'](0), 1e-9);
+ }
+}
+
+
+//TEST_F(ProcessorImu2dTest, Circular_move)
+//{
+//// double pi = 3.14159265358993;
+// double r = 1;
+// double w = 0.5;
+// double alpha = 0;
+// Vector2d pos;
+// // Set the origin
+// x0c['P'] = Vector2d(r, 0);
+// pos = x0c['P'];
+// x0c['O'] = Vector1d(alpha);
+// x0c['V'] = Vector2d(0, w*r);
+//
+// data_cov *= 1e-3;
+// //dt = 0.0001;
+// auto KF0 = problem->setPriorFix(x0c, t0, dt/2);
+// processor_motion->setOrigin(KF0);
+//
+// // Set the gravity
+// g << 0.1, 0;
+// sensor_ptr->getStateBlock('G')->setState(g);
+//
+// WOLF_DEBUG("Current State: ", problem->getState());
+// dt = 0.001;
+// for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){
+// C->setTimeStamp(t);
+// data << -std::cos(alpha)*w*w*r, std::sin(alpha)*w*w*r, 0, 0, 0, w;
+// data.head(2) -= Eigen::Rotation2Dd(alpha + w*(t-t0))*g;
+// C->setData(data);
+// C->setDataCovariance(data_cov);
+// C->process();
+// WOLF_INFO("Current State: ", problem->getState());
+// pos << r*std::cos( w*(t-t0) ),
+// r*std::sin( w*(t-t0) );
+// WOLF_INFO("Calculated State: ", pos.transpose());
+// EXPECT_MATRIX_APPROX(Eigen::Rotation2Dd(w*(t-t0))*x0c['V'] , problem->getState()['V'], 1e-3);
+// }
+//}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ //::testing::GTEST_FLAG(filter) = "ProcessorImu2dt.check_Covariance";
+ return RUN_ALL_TESTS();
+}
+