diff --git a/.gitignore b/.gitignore
index c237d0402877332e46b0dbaeb3a4a12c6283bfe7..5861b1e56adb7cc3c87bb3c10d66c1736ea8885c 100644
--- a/.gitignore
+++ b/.gitignore
@@ -33,5 +33,11 @@ src/examples/map_apriltag_save.yaml
build_release/
IMU.found
+imu.found
+.ccls
+.ccls-cache
+.ccls-root
+compile_commands.json
+.vimspector.json
est.csv
/imu.found
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 16e11c7d14548787af08849a911d5821df4c3e06..31887a2e83ced1be3c875fcca9d9d90bd99355be 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -24,7 +24,7 @@ ENDIF (NOT CMAKE_BUILD_TYPE)
message(STATUS "Configured to compile in ${CMAKE_BUILD_TYPE} mode.")
#Set Flags
-SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
+SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -O0 -D_REENTRANT")
SET(CMAKE_CXX_FLAGS_RELEASE "-O3 -D_REENTRANT")
#Set compiler according C++11 support
@@ -138,19 +138,23 @@ SET(HDRS_CAPTURE
SET(HDRS_FACTOR
include/imu/factor/factor_compass_3d.h
include/imu/factor/factor_imu.h
+ include/imu/factor/factor_imu2d.h
)
SET(HDRS_FEATURE
include/imu/feature/feature_imu.h
+ include/imu/feature/feature_imu2d.h
)
SET(HDRS_LANDMARK
)
SET(HDRS_PROCESSOR
include/imu/processor/processor_compass.h
include/imu/processor/processor_imu.h
+ include/imu/processor/processor_imu2d.h
)
SET(HDRS_SENSOR
include/imu/sensor/sensor_compass.h
include/imu/sensor/sensor_imu.h
+ include/imu/sensor/sensor_imu2d.h
)
SET(HDRS_SOLVER
)
@@ -163,6 +167,7 @@ SET(SRCS_COMMON
)
SET(SRCS_MATH
include/imu/math/imu_tools.h
+ include/imu/math/imu2d_tools.h
)
SET(SRCS_UTILS
)
@@ -174,17 +179,21 @@ SET(SRCS_FACTOR
)
SET(SRCS_FEATURE
src/feature/feature_imu.cpp
+ src/feature/feature_imu2d.cpp
)
SET(SRCS_LANDMARK
)
SET(SRCS_PROCESSOR
src/processor/processor_compass.cpp
src/processor/processor_imu.cpp
+ src/processor/processor_imu2d.cpp
test/processor_imu_UnitTester.cpp
+ test/processor_imu2d_UnitTester.cpp
)
SET(SRCS_SENSOR
src/sensor/sensor_compass.cpp
src/sensor/sensor_imu.cpp
+ src/sensor/sensor_imu2d.cpp
)
SET(SRCS_DTASSC
)
@@ -192,7 +201,9 @@ SET(SRCS_SOLVER
)
SET(SRCS_YAML
src/yaml/processor_imu_yaml.cpp
+ src/yaml/processor_imu2d_yaml.cpp
src/yaml/sensor_imu_yaml.cpp
+ src/yaml/sensor_imu2d_yaml.cpp
)
#OPTIONALS
#optional HDRS and SRCS
diff --git a/demos/processor_imu2d.yaml b/demos/processor_imu2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..6d8d756c415d5d9db4b32ab80cf1ea06f72736fd
--- /dev/null
+++ b/demos/processor_imu2d.yaml
@@ -0,0 +1,12 @@
+type: "ProcessorImu2d" # This must match the KEY used in the SensorFactory. Otherwise it is an error.
+
+time_tolerance: 0.0025 # Time tolerance for joining KFs
+unmeasured_perturbation_std: 0.00001
+
+keyframe_vote:
+ voting_active: false
+ voting_aux_active: false
+ max_time_span: 2.0 # seconds
+ max_buff_length: 20000 # motion deltas
+ dist_traveled: 2.0 # meters
+ angle_turned: 0.2 # radians (1 rad approx 57 deg, approx 60 deg)
diff --git a/demos/sensor_imu2d.yaml b/demos/sensor_imu2d.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ce810d4f5f397a7bcb8647e086c026b125fbc936
--- /dev/null
+++ b/demos/sensor_imu2d.yaml
@@ -0,0 +1,9 @@
+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)
diff --git a/include/imu/capture/capture_imu.h b/include/imu/capture/capture_imu.h
index 861af516e10a8c48afc1be766800cf8a7721cbc6..274d74d494ec4897e11360b4a25bd497098aa6bd 100644
--- a/include/imu/capture/capture_imu.h
+++ b/include/imu/capture/capture_imu.h
@@ -23,7 +23,7 @@ class CaptureImu : public CaptureMotion
SensorBasePtr _sensor_ptr,
const Eigen::Vector6d& _data,
const Eigen::MatrixXd& _data_cov,
- const Vector6d& _bias,
+ const VectorXd& _bias,
CaptureBasePtr _capture_origin_ptr = nullptr);
~CaptureImu() override;
diff --git a/include/imu/factor/factor_imu2d.h b/include/imu/factor/factor_imu2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..4e9be3689c49fbfedda19f69b3c1bf0139e1604c
--- /dev/null
+++ b/include/imu/factor/factor_imu2d.h
@@ -0,0 +1,184 @@
+#ifndef FACTOR_IMU2D_THETA_H_
+#define FACTOR_IMU2D_THETA_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(FactorImu2d);
+
+//class
+class FactorImu2d : public FactorAutodiff<FactorImu2d, 8, 2, 1, 2, 3, 2, 1, 2, 3>
+{
+ public:
+ FactorImu2d(const FeatureImu2dPtr& _ftr_ptr,
+ const CaptureImuPtr& _capture_origin_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+ ~FactorImu2d() 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,
+ 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 FactorImu2d::FactorImu2d(const FeatureImu2dPtr& _ftr_ptr,
+ const CaptureImuPtr& _cap_origin_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorAutodiff<FactorImu2d, 8, 2, 1, 2, 3, 2, 1, 2, 3>( // ...
+ "FactorImu2d",
+ 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()),
+ 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 FactorImu2d::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,
+ 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<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::between(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));
+
+ 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/feature/feature_imu2d.h b/include/imu/feature/feature_imu2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..713df8f8489fad8be3fe7d6fbf3988e61a44cb60
--- /dev/null
+++ b/include/imu/feature/feature_imu2d.h
@@ -0,0 +1,68 @@
+#ifndef FEATURE_IMU2D_H_
+#define FEATURE_IMU2D_H_
+
+//Wolf includes
+#include "imu/capture/capture_imu.h"
+#include "core/feature/feature_base.h"
+#include "core/common/wolf.h"
+
+//std includes
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FeatureImu2d);
+
+class FeatureImu2d : public FeatureBase
+{
+ public:
+
+ /** \brief Constructor from and measures
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ * \param _dD_db_jacobians Jacobians of preintegrated delta wrt Imu2d biases
+ * \param acc_bias accelerometer bias of origin frame
+ * \param gyro_bias gyroscope bias of origin frame
+ * \param _cap_imu_ptr pointer to parent CaptureMotion
+ */
+ FeatureImu2d(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::Vector3d& _bias,
+ const Eigen::Matrix<double,5,3>& _dD_db_jacobians,
+ CaptureMotionPtr _cap_imu_ptr = nullptr);
+
+ /** \brief Constructor from capture pointer
+ *
+ * \param _cap_imu_ptr pointer to parent CaptureMotion
+ */
+ FeatureImu2d(CaptureMotionPtr _cap_imu_ptr);
+
+ ~FeatureImu2d() override;
+
+ const Eigen::Vector3d& getBiasPreint() const;
+ const Eigen::Matrix<double, 5, 3>& getJacobianBias() const;
+
+ private:
+
+ // Used biases
+ Eigen::Vector3d bias_preint_;
+
+ Eigen::Matrix<double, 5, 3> jacobian_bias_;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+};
+
+inline const Eigen::Vector3d& FeatureImu2d::getBiasPreint() const
+{
+ return bias_preint_;
+}
+
+inline const Eigen::Matrix<double, 5, 3>& FeatureImu2d::getJacobianBias() const
+{
+ return jacobian_bias_;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/imu/math/imu2d_tools.h b/include/imu/math/imu2d_tools.h
new file mode 100644
index 0000000000000000000000000000000000000000..1abcd588f1354c7a7e0181dea8f602aba92ec390
--- /dev/null
+++ b/include/imu/math/imu2d_tools.h
@@ -0,0 +1,843 @@
+/*
+ * imu2d_tools.h
+ *
+ * Created on: Nov 16, 2020
+ * Author: igeer
+ */
+
+#ifndef IMU2D_TOOLS_H_
+#define IMU2D_TOOLS_H_
+
+#include "core/common/wolf.h"
+#include "core/math/rotations.h"
+#include "core/math/SE2.h"
+#include "core/state_block/state_composite.h"
+
+#include <cstdio>
+
+/*
+ * Most functions in this file are the 2d versions of the functions in imu_tools.h
+ * They relate manipulations of Delta motion magnitudes used for Imu pre-integration.
+ *
+ * The Delta is defined as
+ * Delta = [Dp, Dth, Dv]
+ * with
+ * Dp : position delta
+ * Dth : angle delta
+ * Dv : velocity delta
+ *
+ * They are listed below:
+ *
+ * - identity: I = Delta at the origin, with Dp = [0,0]; Dth = [0], Dv = [0,0]
+ * - 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)
+ * - 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 )
+ * - body2delta: construct a delta from body magnitudes of linAcc and angVel
+ */
+
+namespace wolf
+{
+namespace imu2d {
+using namespace Eigen;
+using namespace SE2;
+
+template<typename D1, typename D2, typename D3>
+inline void identity(MatrixBase<D1>& p, double& th, MatrixBase<D3>& v)
+{
+ p = MatrixBase<D1>::Zero(2,1);
+ th = 0;
+ v = MatrixBase<D3>::Zero(2,1);
+}
+
+template<typename D1, typename D2, typename D3>
+inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& th, MatrixBase<D3>& v)
+{
+ typedef typename D1::Scalar T1;
+ typedef typename D2::Scalar T2;
+ typedef typename D3::Scalar T3;
+ p << T1(0), T1(0);
+ th << T2(0);
+ v << T3(0), T3(0);
+}
+
+template<typename T = double>
+inline Matrix<T, 5, 1> identity()
+{
+ Matrix<T, 5, 1> ret;
+ ret<< T(0), T(0),
+ T(0),
+ T(0), T(0);
+ return ret;
+}
+
+inline VectorComposite identityComposite()
+{
+ VectorComposite D;
+ D.emplace('P', Vector2d::Zero());
+ D.emplace('O', Vector1d::Zero());
+ D.emplace('V', Vector2d::Zero());
+ return D;
+}
+
+template<typename D1, typename D2, typename D3, typename D4, class T1, class T2, class T3>
+inline void inverse(const MatrixBase<D1>& dp, const T1& dth, const MatrixBase<D2>& dv,
+ const T2& dt,
+ MatrixBase<D3>& idp, T3& idth, MatrixBase<D4>& idv )
+{
+ MatrixSizeCheck<2, 1>::check(dp);
+ MatrixSizeCheck<2, 1>::check(dv);
+ MatrixSizeCheck<2, 1>::check(idp);
+ MatrixSizeCheck<2, 1>::check(idv);
+ const auto& dR1T = Eigen::Rotation2D<T1>(-dth).matrix();
+
+ idp = - dR1T*(dp - dv*dt );
+ idth = - dth;
+ idv = - dR1T*dv;
+}
+
+template<typename D1, typename D2, class T>
+inline void inverse(const MatrixBase<D1>& d,
+ T dt,
+ MatrixBase<D2>& id)
+{
+ MatrixSizeCheck<5, 1>::check(d);
+ MatrixSizeCheck<5, 1>::check(id);
+
+ Map<const Matrix<typename D1::Scalar, 2, 1> > dp ( & d( 0 ) );
+ Map<const Matrix<typename D1::Scalar, 2, 1> > dv ( & d( 3 ) );
+ Map<Matrix<typename D2::Scalar, 2, 1> > idp ( & id( 0 ) );
+ Map<Matrix<typename D2::Scalar, 2, 1> > idv ( & id( 3 ) );
+
+ inverse(dp, d(2), dv, dt, idp, id(2), idv);
+}
+
+template<typename D, class T>
+inline Matrix<typename D::Scalar, 5, 1> inverse(const MatrixBase<D>& d,
+ T& dt)
+{
+ Matrix<typename D::Scalar, 5, 1> id;
+ inverse(d, dt, id);
+ return id;
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, class T1, class T2, class T3, class T4>
+inline void compose(const MatrixBase<D1>& dp1, const T1& dth1, const MatrixBase<D2>& dv1,
+ const MatrixBase<D3>& dp2, const T2& dth2, const MatrixBase<D4>& dv2,
+ const T3& dt,
+ MatrixBase<D5>& sum_p, T4& sum_th, MatrixBase<D6>& sum_v )
+{
+ MatrixSizeCheck<2, 1>::check(dp1);
+ MatrixSizeCheck<2, 1>::check(dv1);
+ MatrixSizeCheck<2, 1>::check(dp2);
+ MatrixSizeCheck<2, 1>::check(dv2);
+ MatrixSizeCheck<2, 1>::check(sum_p);
+ MatrixSizeCheck<2, 1>::check(sum_v);
+ const auto& dR1 = Eigen::Rotation2D<T1>(dth1).matrix();
+
+ sum_p = dp1 + dv1*dt + dR1*dp2; //Rotation matrix here and below because we are rotating a vector
+ sum_v = dv1 + dR1*dv2;
+ sum_th = pi2pi(dth1+dth2); //Sum here because angles compose by sum
+}
+
+template<typename D1, typename D2, typename D3, class T>
+inline void compose(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ T dt,
+ MatrixBase<D3>& sum)
+{
+ MatrixSizeCheck<5, 1>::check(d1);
+ MatrixSizeCheck<5, 1>::check(d2);
+ MatrixSizeCheck<5, 1>::check(sum);
+
+ Map<const Matrix<typename D1::Scalar, 2, 1> > dp1 ( & d1( 0 ) );
+ Map<const Matrix<typename D1::Scalar, 2, 1> > dv1 ( & d1( 3 ) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > dp2 ( & d2( 0 ) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > dv2 ( & d2( 3 ) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > sum_p ( & sum( 0 ) );
+ sum(2) = d1(2) + d2(2);
+ Map<Matrix<typename D3::Scalar, 2, 1> > sum_v ( & sum( 3 ) );
+
+ compose(dp1, d1(2), dv1, dp2, d2(2), dv2, dt, sum_p, sum(2), sum_v);
+}
+
+inline void compose(const VectorComposite& d1, const VectorComposite& d2, double dt, VectorComposite& dc)
+{
+ compose(d1.at('P'), d1.at('O')(0), d1.at('V'), d2.at('P'), d2.at('O')(0), d2.at('V'), dt, dc['P'], dc['O'](0), dc['V']);
+}
+
+inline VectorComposite compose(const VectorComposite& d1, const VectorComposite& d2, double dt)
+{
+ VectorComposite dc("POV", {2,1,2});
+ compose(d1.at('P'), d1.at('O')(0), d1.at('V'), d2.at('P'), d2.at('O')(0), d2.at('V'), dt, dc['P'], dc['O'](0), dc['V']);
+ return dc;
+}
+
+template<typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 5, 1> compose(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ T dt)
+{
+ Matrix<typename D1::Scalar, 5, 1> ret;
+ compose(d1, d2, dt, ret);
+ return ret;
+}
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5, class T>
+inline void compose(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ T dt,
+ MatrixBase<D3>& sum,
+ MatrixBase<D4>& J_sum_d1,
+ MatrixBase<D5>& J_sum_d2)
+{
+ MatrixSizeCheck<5, 1>::check(d1);
+ MatrixSizeCheck<5, 1>::check(d2);
+ MatrixSizeCheck<5, 1>::check(sum);
+ MatrixSizeCheck< 5, 5>::check(J_sum_d1);
+ MatrixSizeCheck< 5, 5>::check(J_sum_d2);
+
+ //Useful auxiliaries
+ const auto& dR1 = Rotation2D<typename D1::Scalar>(d1(2)).matrix();
+
+ // Jac wrt first delta
+ J_sum_d1.setIdentity(); // dDp'/dDp = dDv'/dDv = I
+ J_sum_d1.block(0,2,2,1).noalias() = dR1 * skewed(d2.head(2)) ; // dDp'/dDo
+ J_sum_d1.block(0,3,2,2).noalias() = Matrix2d::Identity() * dt; // dDp'/dDv = I*dt
+ J_sum_d1.block(3,2,2,1).noalias() = dR1 * skewed(d2.tail(2)) ; // dDv'/dDo
+ // J_sum_d1.block(2,2,1,1) = 1; // dDo'/dDo = 1
+
+ // Jac wrt second delta
+ J_sum_d2.setIdentity(); //
+ J_sum_d2.block(0,0,2,2) = dR1; // dDp'/ddp
+ J_sum_d2.block(3,3,2,2) = dR1; // dDv'/ddv
+ // J_sum_d2.block(2,2,1,1) = 1; // dDo'/ddo = 1
+
+ // compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
+ compose(d1, d2, dt, sum);
+}
+
+//inline void compose(const VectorComposite& d1,
+// const VectorComposite& d2,
+// double dt,
+// VectorComposite& sum,
+// MatrixComposite& J_sum_d1,
+// MatrixComposite& J_sum_d2)
+//{
+//
+// // Some useful temporaries
+// Matrix3d dR1 = q2R(d1.at('O')); //dq1.matrix(); // First Delta, DR
+// Matrix3d dR2 = q2R(d2.at('O')); //dq2.matrix(); // Second delta, dR
+//
+// // Jac wrt first delta // TODO find optimal way to re-use memory allocation!!!
+// J_sum_d1.clear();
+// J_sum_d1.emplace('P','P', Matrix3d::Identity()); // dDp'/dDp = I
+// J_sum_d1.emplace('P','O', - dR1 * skew(d2.at('P'))) ; // dDp'/dDo
+// J_sum_d1.emplace('P','V', Matrix3d::Identity() * dt); // dDp'/dDv = I*dt
+// J_sum_d1.emplace('O','P', Matrix3d::Zero());
+// J_sum_d1.emplace('O','O', dR2.transpose()); // dDo'/dDo
+// J_sum_d1.emplace('O','V', Matrix3d::Zero());
+// J_sum_d1.emplace('V','P', Matrix3d::Zero());
+// J_sum_d1.emplace('V','O', - dR1 * skew(d2.at('V'))) ; // dDv'/dDo
+// J_sum_d1.emplace('V','V', Matrix3d::Identity()); // dDv'/dDv = I
+//
+//
+// // Jac wrt second delta
+// J_sum_d2.clear();
+// J_sum_d2.emplace('P','P', dR1); // dDp'/ddp
+// J_sum_d2.emplace('P','O', Matrix3d::Zero()) ; // dDp'/ddo
+// J_sum_d2.emplace('P','V', Matrix3d::Zero()); // dDp'/ddv
+// J_sum_d2.emplace('O','P', Matrix3d::Zero());
+// J_sum_d2.emplace('O','O', Matrix3d::Identity());// dDo'/ddo
+// J_sum_d2.emplace('O','V', Matrix3d::Zero());
+// J_sum_d2.emplace('V','P', Matrix3d::Zero());
+// J_sum_d2.emplace('V','O', Matrix3d::Zero()) ; // dDv'/ddo
+// J_sum_d2.emplace('V','V', dR1); // dDv'/ddv
+//
+// // 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,
+ const T3 dt,
+ MatrixBase<D5>& diff_p, T4& diff_th, MatrixBase<D6>& diff_v )
+{
+ MatrixSizeCheck<2, 1>::check(dp1);
+ MatrixSizeCheck<2, 1>::check(dv1);
+ MatrixSizeCheck<2, 1>::check(dp2);
+ MatrixSizeCheck<2, 1>::check(dv2);
+ MatrixSizeCheck<2, 1>::check(diff_p);
+ MatrixSizeCheck<2, 1>::check(diff_v);
+
+ const auto& dR1_tr = Rotation2D<T1>(-dth1).matrix();
+ diff_p = dR1_tr * ( dp2 - dp1 - dv1*dt );
+ diff_v = dR1_tr * ( dv2 - dv1 );
+ diff_th = pi2pi(dth2 - dth1);
+}
+
+template<typename D1, typename D2, typename D3, class T>
+inline void between(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ T dt,
+ MatrixBase<D3>& d2_minus_d1)
+{
+ MatrixSizeCheck<5, 1>::check(d1);
+ MatrixSizeCheck<5, 1>::check(d2);
+ MatrixSizeCheck<5, 1>::check(d2_minus_d1);
+
+ Map<const Matrix<typename D1::Scalar, 2, 1> > dp1 ( & d1(0) );
+ Map<const Matrix<typename D1::Scalar, 2, 1> > dv1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > dp2 ( & d2(0) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > dv2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > diff_p ( & d2_minus_d1(0) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > diff_v ( & d2_minus_d1(3) );
+
+ between(dp1, d1(2), dv1, dp2, d2(2), dv2, dt, diff_p, d2_minus_d1(2), diff_v);
+}
+
+template<typename D1, typename D2, class T>
+inline Matrix<typename D1::Scalar, 5, 1> between(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ T dt)
+{
+ Matrix<typename D1::Scalar, 5, 1> diff;
+ between(d1, d2, dt, diff);
+ 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 Derived>
+//Matrix<typename Derived::Scalar, 9, 1> log_Imu(const MatrixBase<Derived>& delta_in)
+//{
+// MatrixSizeCheck<10, 1>::check(delta_in);
+//
+// Matrix<typename Derived::Scalar, 9, 1> ret;
+//
+// Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & delta_in(0) );
+// Map<const Quaternion<typename Derived::Scalar> > dq_in ( & delta_in(3) );
+// Map<const Matrix<typename Derived::Scalar, 3, 1> > dv_in ( & delta_in(7) );
+// Map<Matrix<typename Derived::Scalar, 3, 1> > dp_ret ( & ret(0) );
+// Map<Matrix<typename Derived::Scalar, 3, 1> > do_ret ( & ret(3) );
+// Map<Matrix<typename Derived::Scalar, 3, 1> > dv_ret ( & ret(6) );
+//
+// dp_ret = dp_in;
+// dv_ret = dv_in;
+// do_ret = log_q(dq_in);
+//
+// return ret;
+//}
+//
+//template<typename Derived>
+//Matrix<typename Derived::Scalar, 10, 1> exp_Imu(const MatrixBase<Derived>& d_in)
+//{
+// MatrixSizeCheck<9, 1>::check(d_in);
+//
+// Matrix<typename Derived::Scalar, 10, 1> ret;
+//
+// Map<const Matrix<typename Derived::Scalar, 3, 1> > dp_in ( & d_in(0) );
+// Map<const Matrix<typename Derived::Scalar, 3, 1> > do_in ( & d_in(3) );
+// Map<const Matrix<typename Derived::Scalar, 3, 1> > dv_in ( & d_in(6) );
+// Map<Matrix<typename Derived::Scalar, 3, 1> > dp ( & ret(0) );
+// Map<Quaternion<typename Derived::Scalar> > dq ( & ret(3) );
+// Map<Matrix<typename Derived::Scalar, 3, 1> > dv ( & ret(7) );
+//
+// dp = dp_in;
+// dv = dv_in;
+// dq = exp_q(do_in);
+//
+// return ret;
+//}
+//
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, class T1, class T2, class T3>
+inline void plus(const MatrixBase<D1>& dp1, const T1& dth1, const MatrixBase<D2>& dv1,
+ const MatrixBase<D3>& dp2, const T2& dth2, const MatrixBase<D4>& dv2,
+ MatrixBase<D5>& plus_p, T3& plus_th, MatrixBase<D6>& plus_v )
+{
+ plus_p = dp1 + dp2;
+ plus_v = dv1 + dv2;
+ plus_th = dth1 + dth2;
+}
+
+template<typename D1, typename D2, typename D3>
+inline void plus(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& d_pert)
+{
+ Map<const Matrix<typename D1::Scalar, 2, 1> > dp1 ( & d1(0) );
+ Map<const Matrix<typename D1::Scalar, 2, 1> > dv1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > dp2 ( & d2(0) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > dv2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > dp_p ( & d_pert(0) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > dv_p ( & d_pert(3) );
+
+ plus(dp1, d1(2), dv1, dp2, d2(2), dv2, dp_p, d_pert(2), dv_p);
+}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 5, 1> plus(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2)
+{
+ Matrix<typename D1::Scalar, 5, 1> ret;
+ plus(d1, d2, ret);
+ return ret;
+}
+//
+//inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res)
+//{
+// plus(x.at('P'), x.at('O'), x.at('V'), tau.at('P'), tau.at('O'), tau.at('V'), res.at('P'), res.at('O'), res.at('V'));
+//}
+//
+//inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
+//{
+// VectorComposite res("POV", {3,4,3});
+//
+// plus(x, tau, res);
+//
+// return res;
+//}
+
+
+template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6>
+inline void diff(const MatrixBase<D1>& dp1, const typename D1::Scalar& do1, const MatrixBase<D2>& dv1,
+ const MatrixBase<D3>& dp2, const typename D1::Scalar& do2, const MatrixBase<D4>& dv2,
+ MatrixBase<D5>& diff_p, typename D1::Scalar& diff_o, MatrixBase<D6>& diff_v )
+{
+ diff_p = dp2 - dp1;
+ diff_v = dv2 - dv1;
+ diff_o = do2 - do1;
+}
+
+//template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7, typename D8, typename D9, typename D10, typename D11>
+//inline void diff(const MatrixBase<D1>& dp1, const QuaternionBase<D2>& dq1, const MatrixBase<D3>& dv1,
+// const MatrixBase<D4>& dp2, const QuaternionBase<D5>& dq2, const MatrixBase<D6>& dv2,
+// MatrixBase<D7>& diff_p, MatrixBase<D8>& diff_o, MatrixBase<D9>& diff_v ,
+// MatrixBase<D10>& J_do_dq1, MatrixBase<D11>& J_do_dq2)
+//{
+// diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v);
+//
+// J_do_dq1 = - jac_SO3_left_inv(diff_o);
+// J_do_dq2 = jac_SO3_right_inv(diff_o);
+//}
+
+template<typename D1, typename D2, typename D3>
+inline void diff(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2,
+ MatrixBase<D3>& err)
+{
+ Map<const Matrix<typename D1::Scalar, 2, 1> > dp1 ( & d1(0) );
+ Map<const Matrix<typename D1::Scalar, 2, 1> > dv1 ( & d1(3) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > dp2 ( & d2(0) );
+ Map<const Matrix<typename D2::Scalar, 2, 1> > dv2 ( & d2(3) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > diff_p ( & err(0) );
+ Map<Matrix<typename D3::Scalar, 2, 1> > diff_v ( & err(3) );
+
+ diff(dp1, d1(2), dv1, dp2, d2(2), dv2, diff_p, err(2), diff_v);
+}
+
+//template<typename D1, typename D2, typename D3, typename D4, typename D5>
+//inline void diff(const MatrixBase<D1>& d1,
+// const MatrixBase<D2>& d2,
+// MatrixBase<D3>& dif,
+// MatrixBase<D4>& J_diff_d1,
+// MatrixBase<D5>& J_diff_d2)
+//{
+// Map<const Matrix<typename D1::Scalar, 3, 1> > dp1 ( & d1(0) );
+// Map<const Quaternion<typename D1::Scalar> > dq1 ( & d1(3) );
+// Map<const Matrix<typename D1::Scalar, 3, 1> > dv1 ( & d1(7) );
+// Map<const Matrix<typename D2::Scalar, 3, 1> > dp2 ( & d2(0) );
+// Map<const Quaternion<typename D2::Scalar> > dq2 ( & d2(3) );
+// Map<const Matrix<typename D2::Scalar, 3, 1> > dv2 ( & d2(7) );
+// Map<Matrix<typename D3::Scalar, 3, 1> > diff_p ( & dif(0) );
+// Map<Matrix<typename D3::Scalar, 3, 1> > diff_o ( & dif(3) );
+// Map<Matrix<typename D3::Scalar, 3, 1> > diff_v ( & dif(6) );
+//
+// Matrix<typename D4::Scalar, 3, 3> J_do_dq1, J_do_dq2;
+//
+// diff(dp1, dq1, dv1, dp2, dq2, dv2, diff_p, diff_o, diff_v, J_do_dq1, J_do_dq2);
+//
+// /* d = diff(d1, d2) is
+// * dp = dp2 - dp1
+// * do = Log(dq1.conj * dq2)
+// * dv = dv2 - dv1
+// *
+// * With trivial Jacobians for dp and dv, and:
+// * J_do_dq1 = - J_l_inv(theta)
+// * J_do_dq2 = J_r_inv(theta)
+// */
+//
+// J_diff_d1 = - Matrix<typename D4::Scalar, 9, 9>::Identity();// d(p2 - p1) / d(p1) = - Identity
+// J_diff_d1.block(3,3,3,3) = J_do_dq1; // d(R1.tr*R2) / d(R1) = - J_l_inv(theta)
+//
+// J_diff_d2.setIdentity(); // d(R1.tr*R2) / d(R2) = Identity
+// J_diff_d2.block(3,3,3,3) = J_do_dq2; // d(R1.tr*R2) / d(R1) = J_r_inv(theta)
+//}
+
+template<typename D1, typename D2>
+inline Matrix<typename D1::Scalar, 5, 1> diff(const MatrixBase<D1>& d1,
+ const MatrixBase<D2>& d2)
+{
+ Matrix<typename D1::Scalar, 5, 1> ret;
+ diff(d1, d2, ret);
+ return ret;
+}
+
+template<typename D1, typename D2, typename D3>
+inline void exp(const MatrixBase<D1>& a,
+ const typename D1::Scalar& w,
+ const typename D1::Scalar& dt,
+ MatrixBase<D2>& dp,
+ typename D1::Scalar& dth,
+ MatrixBase<D3>& dv)
+{
+ MatrixSizeCheck<2,1>::check(a);
+ MatrixSizeCheck<2,1>::check(dp);
+ MatrixSizeCheck<2,1>::check(dv);
+
+ typedef typename D1::Scalar T;
+ dth = w*dt;
+ if(dth > Constants::EPS){
+ T s = std::sin(dth);
+ T c = std::cos(dth);
+ T aux1 = s/dth;
+ T aux2 = (1-c)/dth;
+ T aux3 = aux2/dth;
+ T aux4 = (dth-s)/(dth*dth);
+ Matrix<T,2,2> skw = SE2::skew((T) 1.0);
+
+ Matrix<T, 2, 2> P = Matrix<T, 2, 2>::Identity() * aux3 + skw*aux4;
+ Matrix<T, 2, 2> Q = Matrix<T, 2, 2>::Identity() * aux1 + skw*aux2;
+
+ dp = P*a*dt*dt;
+ dv = Q*a*dt;
+ //dth = dth;
+ }
+ else{
+ dp = (T)0.5*a*dt*dt;
+ dv = a*dt;
+ //dth = dth;
+ }
+}
+
+
+template<typename D1, typename D2, typename D3>
+inline void body2delta(const MatrixBase<D1>& a,
+ const typename D1::Scalar& w,
+ const typename D1::Scalar& dt,
+ MatrixBase<D2>& dp,
+ typename D1::Scalar& dth,
+ MatrixBase<D3>& dv)
+{
+ MatrixSizeCheck<2,1>::check(a);
+ MatrixSizeCheck<2,1>::check(dp);
+ MatrixSizeCheck<2,1>::check(dv);
+
+ //dp = 0.5 * a * dt * dt;
+ //dv = a * dt;
+ //dth = w * dt;
+ imu2d::exp(a, w, dt, dp, dth, dv);
+
+}
+
+//template<typename D1, typename D2, typename D3, typename D4, typename D5>
+//inline void body2delta(const MatrixBase<D1>& a,
+// const MatrixBase<D2>& w,
+// const typename D1::Scalar& dt,
+// MatrixBase<D3>& dp,
+// MatrixBase<D4>& dq,
+// MatrixBase<D5>& dv)
+//{
+// MatrixSizeCheck<3,1>::check(a);
+// MatrixSizeCheck<3,1>::check(w);
+// MatrixSizeCheck<3,1>::check(dp);
+// MatrixSizeCheck<4,1>::check(dq);
+// MatrixSizeCheck<3,1>::check(dv);
+//
+// Map<Quaternion<typename D4::Scalar>> mdq ( & dq(0) );
+//
+// body2delta(a, w, dt, dp, mdq, dv);
+//}
+
+template<typename D1>
+inline Matrix<typename D1::Scalar, 5, 1> body2delta(const MatrixBase<D1>& body,
+ const typename D1::Scalar& dt)
+{
+ MatrixSizeCheck<3,1>::check(body);
+
+ typedef typename D1::Scalar T;
+
+ Matrix<T, 5, 1> delta;
+
+ Map< Matrix<T, 2, 1>> dp ( & delta(0) );
+ Map< Matrix<T, 2, 1>> dv ( & delta(3) );
+
+ body2delta(body.block(0,0,2,1), body(2,0), dt, dp, delta(2), dv);
+
+ return delta;
+}
+
+//template<typename D1>
+//inline void body2delta(const MatrixBase<D1>& body,
+// const typename D1::Scalar& dt,
+// VectorComposite& _delta)
+//{
+// MatrixSizeCheck<6,1>::check(body);
+//
+// body2delta(body.block(0,0,3,1),
+// body.block(3,0,3,1),
+// dt,
+// _delta['P'],
+// _delta['O'],
+// _delta['V']);
+//}
+
+template<typename D1, typename D2, typename D3>
+inline void body2delta(const MatrixBase<D1>& body,
+ const typename D1::Scalar& dt,
+ MatrixBase<D2>& delta,
+ MatrixBase<D3>& jac_body)
+{
+ MatrixSizeCheck<3,1>::check(body);
+ MatrixSizeCheck<5,3>::check(jac_body);
+
+ typedef typename D1::Scalar T;
+
+ delta = body2delta(body, dt);
+
+ //Jacobians are not exact, we don't know the exact expression for the imu2d::exp() function
+ jac_body.setZero();
+ jac_body.block(0,0,2,2) = 0.5 * dt * dt * Matrix<T, 2, 2>::Identity();
+ jac_body(2,2) = dt;
+ jac_body.block(3,0,2,2) = dt * Matrix<T, 2, 2>::Identity();
+}
+
+//template<typename D1>
+//inline void body2delta(const MatrixBase<D1>& body,
+// const typename D1::Scalar& dt,
+// VectorComposite& delta,
+// MatrixComposite& jac_body)
+//{
+// MatrixSizeCheck<6,1>::check(body);
+//
+// typedef typename D1::Scalar T;
+//
+// body2delta(body, dt, delta);
+//
+// Matrix<T, 3, 1> w = body.block(3,0,3,1);
+//
+// jac_body.emplace('P','a', 0.5 * dt * dt * Matrix3d::Identity()); // 0,0
+// jac_body.emplace('P','w', Matrix3d::Zero()); // 0,3
+// jac_body.emplace('O','a', Matrix3d::Zero()); // 3,0
+// jac_body.emplace('O','w', dt * jac_SO3_right(w * dt)); // 3,3
+// jac_body.emplace('V','a', dt * Matrix3d::Identity()); // 6,0
+// jac_body.emplace('V','w', Matrix3d::Zero()); // 6,6
+//}
+//template<typename D1, typename D2, typename D3, typename D4, typename D5, typename D6, typename D7>
+//void motion2data(const MatrixBase<D1>& a, const MatrixBase<D2>& w, const QuaternionBase<D3>& q, const MatrixBase<D4>& a_b, const MatrixBase<D5>& w_b, MatrixBase<D6>& a_m, MatrixBase<D7>& w_m)
+//{
+// MatrixSizeCheck<3,1>::check(a);
+// MatrixSizeCheck<3,1>::check(w);
+// MatrixSizeCheck<3,1>::check(a_b);
+// MatrixSizeCheck<3,1>::check(w_b);
+// MatrixSizeCheck<3,1>::check(a_m);
+// MatrixSizeCheck<3,1>::check(w_m);
+//
+// // Note: data = (a_m , w_m)
+// a_m = a + a_b - q.conjugate()*gravity();
+// w_m = w + w_b;
+//}
+//
+///* Create Imu data from body motion
+// * Input:
+// * motion : [ax, ay, az, wx, wy, wz] the motion in body frame
+// * q : the current orientation wrt horizontal
+// * bias : the bias of the Imu
+// * Output:
+// * return : the data vector as created by the Imu (with motion, gravity, and bias)
+// */
+//template<typename D1, typename D2, typename D3>
+//Matrix<typename D1::Scalar, 6, 1> motion2data(const MatrixBase<D1>& motion, const QuaternionBase<D2>& q, const MatrixBase<D3>& bias)
+//{
+// Matrix<typename D1::Scalar, 6, 1> data;
+// Map<Matrix<typename D1::Scalar, 3, 1>> a_m (data.data() );
+// Map<Matrix<typename D1::Scalar, 3, 1>> w_m (data.data() + 3);
+//
+// motion2data(motion.block(0,0,3,1),
+// motion.block(3,0,3,1),
+// q,
+// bias.block(0,0,3,1),
+// bias.block(3,0,3,1),
+// a_m,
+// w_m );
+//
+// return data;
+//}
+
+} // namespace imu2d
+} // namespace wolf
+
+#endif /* Imu_TOOLS_H_ */
diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index d82a0afe492f938558eb28c53da70a65fb515b3f..502120cc1400bcd2da109903adc72c219e7a6efe 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -19,7 +19,7 @@ struct ParamsProcessorImu : public ParamsProcessorMotion
}
std::string print() const override
{
- return "\n" + ParamsProcessorMotion::print();
+ return ParamsProcessorMotion::print();
}
};
diff --git a/include/imu/processor/processor_imu2d.h b/include/imu/processor/processor_imu2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..bda61341dad3165764aaf78e1a6a5a7efdf8d373
--- /dev/null
+++ b/include/imu/processor/processor_imu2d.h
@@ -0,0 +1,99 @@
+#ifndef PROCESSOR_IMU2D_H
+#define PROCESSOR_IMU2D_H
+
+// Wolf
+#include "imu/capture/capture_imu.h"
+#include "imu/feature/feature_imu.h"
+#include "core/processor/processor_motion.h"
+
+namespace wolf {
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorImu2d);
+
+struct ParamsProcessorImu2d : public ParamsProcessorMotion
+{
+ ParamsProcessorImu2d() = default;
+ ParamsProcessorImu2d(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorMotion(_unique_name, _server)
+ {
+ //
+ }
+ std::string print() const override
+ {
+ return ParamsProcessorMotion::print();
+ }
+};
+
+WOLF_PTR_TYPEDEFS(ProcessorImu2d);
+
+//class
+class ProcessorImu2d : public ProcessorMotion{
+ public:
+ ProcessorImu2d(ParamsProcessorImu2dPtr _params_motion_Imu);
+ ~ProcessorImu2d() override;
+ void configure(SensorBasePtr _sensor) override { };
+
+ WOLF_PROCESSOR_CREATE(ProcessorImu2d, ParamsProcessorImu2d);
+ void preProcess() override;
+
+ protected:
+ 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 override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta,
+ Eigen::MatrixXd& _jacobian_delta_preint,
+ Eigen::MatrixXd& _jacobian_delta) const override;
+ void statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _Dt,
+ VectorComposite& _x_plus_delta) const override;
+ Eigen::VectorXd deltaZero() const override;
+ Eigen::VectorXd correctDelta(const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const override;
+ VectorXd getCalibration (const CaptureBasePtr _capture) const override;
+ void setCalibration(const CaptureBasePtr _capture, const VectorXd& _calibration) override;
+ bool voteForKeyFrame() const override;
+ CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin) override;
+ FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
+ FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
+ CaptureBasePtr _capture_origin) override;
+
+ protected:
+ ParamsProcessorImu2dPtr params_motion_Imu_;
+ Eigen::Matrix<double, 5, 5> unmeasured_perturbation_cov_;
+
+};
+
+}
+
+/////////////////////////////////////////////////////////
+// IMPLEMENTATION. Put your implementation includes here
+/////////////////////////////////////////////////////////
+
+namespace wolf{
+
+inline Eigen::VectorXd ProcessorImu2d::deltaZero() const
+{
+ return (Eigen::VectorXd(5) << 0,0, 0, 0,0 ).finished(); // p, q, v
+}
+
+} // namespace wolf
+
+#endif // PROCESSOR_Imu_H
diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
index e875fde93d4d5ed446e7c433e142b2f8c917cdb7..07b289c0ac0e21451dbe682774d3aeddc79ffa7b 100644
--- a/include/imu/sensor/sensor_imu.h
+++ b/include/imu/sensor/sensor_imu.h
@@ -42,7 +42,7 @@ struct ParamsSensorImu : public ParamsSensorBase
}
std::string print() const override
{
- return "\n" + ParamsSensorBase::print() + "\n"
+ 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"
diff --git a/include/imu/sensor/sensor_imu2d.h b/include/imu/sensor/sensor_imu2d.h
new file mode 100644
index 0000000000000000000000000000000000000000..43fb38693178aea9e06da9c2272071517ac68c5f
--- /dev/null
+++ b/include/imu/sensor/sensor_imu2d.h
@@ -0,0 +1,119 @@
+#ifndef SENSOR_IMU2D_H
+#define SENSOR_IMU2D_H
+
+//wolf includes
+#include "core/sensor/sensor_base.h"
+#include "core/utils/params_server.h"
+#include <iostream>
+
+namespace wolf {
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorImu2d);
+
+
+struct ParamsSensorImu2d : public ParamsSensorBase
+{
+ //noise std dev
+ double w_noise = 0.001; //standard deviation of Gyroscope noise (same for all the axis) in rad/sec/ sqrt(s)
+ double a_noise = 0.04; //standard deviation of Acceleration noise (same for all the axis) in m/s2/sqrt(s)
+
+ //Initial biases std dev
+ double ab_initial_stdev = 0.01; //accelerometer micro_g/sec
+ double wb_initial_stdev = 0.01; //gyroscope rad/sec
+
+ // bias rate of change std dev
+ double ab_rate_stdev = 0.00001;
+ double wb_rate_stdev = 0.00001;
+
+ ~ParamsSensorImu2d() override = default;
+ ParamsSensorImu2d()
+ {
+ //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
+ }
+ 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");
+ }
+ 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";
+ }
+};
+
+WOLF_PTR_TYPEDEFS(SensorImu2d);
+
+class SensorImu2d : public SensorBase
+{
+
+ protected:
+ double a_noise; //Power Spectral Density (same for all the axis) in micro_g/ sqrt(Hz)
+ double w_noise; //Rate Noise Spectral Density (same for all the axis) in deg/sec/ sqrt(Hz)
+
+ //This is a trial to factor how much can the bias change in 1 sec at most
+ double ab_initial_stdev; //accelerometer m/sec^s
+ double wb_initial_stdev; //gyroscope rad/sec
+ double ab_rate_stdev; //accelerometer m/sec^2 / sqrt(sec)
+ double wb_rate_stdev; //gyroscope rad/sec / sqrt(sec)
+
+ public:
+
+ SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params);
+ SensorImu2d(const Eigen::VectorXd& _extrinsics, ParamsSensorImu2dPtr _params);
+ WOLF_SENSOR_CREATE(SensorImu2d, ParamsSensorImu2d, 3);
+
+ double getGyroNoise() const;
+ double getAccelNoise() const;
+ double getWbInitialStdev() const;
+ double getAbInitialStdev() const;
+ double getWbRateStdev() const;
+ double getAbRateStdev() const;
+
+ ~SensorImu2d() override;
+
+};
+
+inline double SensorImu2d::getGyroNoise() const
+{
+ return w_noise;
+}
+
+inline double SensorImu2d::getAccelNoise() const
+{
+ return a_noise;
+}
+
+inline double SensorImu2d::getWbInitialStdev() const
+{
+ return wb_initial_stdev;
+}
+
+inline double SensorImu2d::getAbInitialStdev() const
+{
+ return ab_initial_stdev;
+}
+
+inline double SensorImu2d::getWbRateStdev() const
+{
+ return wb_rate_stdev;
+}
+
+inline double SensorImu2d::getAbRateStdev() const
+{
+ return ab_rate_stdev;
+}
+
+} // namespace wolf
+
+#endif // SENSOR_Imu2D_H
diff --git a/src/capture/capture_imu.cpp b/src/capture/capture_imu.cpp
index e0ddfeb680b3f15fb0f8655631a654564f479c7e..75230fafd5663ca7a422497fbd834e4a97e31fe3 100644
--- a/src/capture/capture_imu.cpp
+++ b/src/capture/capture_imu.cpp
@@ -17,7 +17,7 @@ CaptureImu::CaptureImu(const TimeStamp& _init_ts,
_capture_origin_ptr,
nullptr,
nullptr,
- std::make_shared<StateBlock>(6, false))
+ std::make_shared<StateBlock>((_sensor_ptr->getProblem()->getDim() == 2 ? 3 : 6), false))
{
//
}
@@ -26,7 +26,7 @@ CaptureImu::CaptureImu(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
const Eigen::Vector6d& _acc_gyro_data,
const Eigen::MatrixXd& _data_cov,
- const Vector6d& _bias,
+ const VectorXd& _bias,
CaptureBasePtr _capture_origin_ptr) :
CaptureMotion("CaptureImu",
_init_ts,
@@ -38,7 +38,7 @@ CaptureImu::CaptureImu(const TimeStamp& _init_ts,
nullptr,
std::make_shared<StateBlock>(_bias, false))
{
- //
+ assert((_bias.size() == 3) or (_bias.size() == 6));
}
CaptureImu::~CaptureImu()
diff --git a/src/feature/feature_imu2d.cpp b/src/feature/feature_imu2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1a5e61a0d0a88629f37d84f96e3605933636ce3e
--- /dev/null
+++ b/src/feature/feature_imu2d.cpp
@@ -0,0 +1,28 @@
+#include "imu/feature/feature_imu2d.h"
+
+namespace wolf {
+
+FeatureImu2d::FeatureImu2d(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::Vector3d& _bias,
+ const Eigen::Matrix<double,5,3>& _dD_db_jacobians,
+ CaptureMotionPtr _cap_imu_ptr) :
+ FeatureBase("FeatureImu2d", _delta_preintegrated, _delta_preintegrated_covariance),
+ bias_preint_(_bias),
+ jacobian_bias_(_dD_db_jacobians)
+{
+}
+
+FeatureImu2d::FeatureImu2d(CaptureMotionPtr _cap_imu_ptr):
+ FeatureBase("FeatureImu2d", _cap_imu_ptr->getDeltaPreint(), _cap_imu_ptr->getDeltaPreintCov()),
+ bias_preint_ (_cap_imu_ptr->getCalibrationPreint()),
+ jacobian_bias_(_cap_imu_ptr->getJacobianCalib())
+{
+}
+
+FeatureImu2d::~FeatureImu2d()
+{
+ //
+}
+
+} // namespace wolf
diff --git a/src/processor/processor_imu2d.cpp b/src/processor/processor_imu2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5c5e911c1edcf6319946b1d3a095e29317820522
--- /dev/null
+++ b/src/processor/processor_imu2d.cpp
@@ -0,0 +1,245 @@
+// imu
+#include "imu/processor/processor_imu2d.h"
+#include "imu/factor/factor_imu2d.h"
+#include "imu/math/imu2d_tools.h"
+
+// wolf
+#include <core/state_block/state_composite.h>
+
+namespace wolf {
+
+ ProcessorImu2d::ProcessorImu2d(ParamsProcessorImu2dPtr _params_motion_imu) :
+ ProcessorMotion("ProcessorImu2d", "POV", 2, 5, 5, 5, 6, 3, _params_motion_imu),
+ params_motion_Imu_(std::make_shared<ParamsProcessorImu2d>(*_params_motion_imu))
+ {
+ // Set constant parts of Jacobians
+ jacobian_delta_preint_.setIdentity(5,5); // dDp'/dDp, dDv'/dDv, all zeros
+ jacobian_delta_.setIdentity(5,5); //
+ jacobian_calib_.setZero(5,3);
+ unmeasured_perturbation_cov_ = pow(params_motion_Imu_->unmeasured_perturbation_std, 2.0) * Eigen::Matrix<double, 5, 5>::Identity();
+ }
+
+ ProcessorImu2d::~ProcessorImu2d()
+ {
+ //
+ }
+
+ void ProcessorImu2d::preProcess()
+ {
+ auto cap_ptr = std::dynamic_pointer_cast<CaptureImu>(incoming_ptr_);
+ assert(cap_ptr != nullptr && ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureImu").c_str());
+ }
+
+ bool ProcessorImu2d::voteForKeyFrame() const
+ {
+ // time span
+ if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_Imu_->max_time_span)
+ {
+ WOLF_DEBUG( "PM: vote: time span" );
+ return true;
+ }
+ // buffer length
+ if (getBuffer().size() > params_motion_Imu_->max_buff_length)
+ {
+ WOLF_DEBUG( "PM: vote: buffer length" );
+ return true;
+ }
+ // angle turned
+ double angle = std::abs(delta_integrated_(2));
+ if (angle > params_motion_Imu_->angle_turned)
+ {
+ WOLF_DEBUG( "PM: vote: angle turned" );
+ return true;
+ }
+ //WOLF_DEBUG( "PM: do not vote" );
+ return false;
+ }
+
+
+ CaptureMotionPtr ProcessorImu2d::emplaceCapture(const FrameBasePtr& _frame_own,
+ const SensorBasePtr& _sensor,
+ const TimeStamp& _ts,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
+ const CaptureBasePtr& _capture_origin)
+ {
+ auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureImu>(_frame_own,
+ _ts,
+ _sensor,
+ _data,
+ _data_cov,
+ _capture_origin));
+ setCalibration(cap_motion, _calib);
+ cap_motion->setCalibrationPreint(_calib_preint);
+
+ return cap_motion;
+ }
+
+ FeatureBasePtr ProcessorImu2d::emplaceFeature(CaptureMotionPtr _capture_motion)
+ {
+ auto feature = FeatureBase::emplace<FeatureImu2d>(_capture_motion,
+ _capture_motion->getBuffer().back().delta_integr_,
+ _capture_motion->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
+ _capture_motion->getCalibrationPreint(),
+ _capture_motion->getBuffer().back().jacobian_calib_);
+ return feature;
+ }
+
+ VectorXd ProcessorImu2d::getCalibration (const CaptureBasePtr _capture) const
+ {
+ if (_capture)
+ return _capture->getStateBlock('I')->getState();
+ else
+ return getSensor()->getStateBlockDynamic('I')->getState();
+ }
+
+ void ProcessorImu2d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration)
+ {
+ _capture->getSensorIntrinsic()->setState(_calibration);
+ }
+
+ FactorBasePtr ProcessorImu2d::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+ {
+ 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;
+ }
+
+ void ProcessorImu2d::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& _jac_delta_calib) const
+ {
+ using namespace Eigen;
+ assert(_data.size() == data_size_ && "Wrong data size!");
+ Vector3d data_2d;
+ data_2d << _data(0), _data(1), _data(5);
+ Matrix3d data_cov_2d;
+ data_cov_2d << _data_cov(0,0), _data_cov(0,1), _data_cov(0,5),
+ _data_cov(1,0), _data_cov(1,1), _data_cov(1,5),
+ _data_cov(5,0), _data_cov(5,1), _data_cov(5,5);
+
+
+
+ Matrix<double, 5, 3> jac_delta_data;
+ /*
+ * We have the following computing pipeline:
+ * Input: data, calib, dt
+ * Output: delta, delta_cov, jac_calib
+ *
+ * We do:
+ * body = data - calib (measurement - bias)
+ * delta = body2delta(body, dt) --> jac_body
+ * jac_data = jac_body
+ * jac_calib = - jac_body
+ * delta_cov <-- propagate data_cov using jac_data
+ *
+ * where body2delta(.) produces a delta from body=(a,w) as follows:
+ * dp = P * a * dt^2
+ * dth = exp(w * dt) = w * dt
+ * dv = Q * a * dt
+ * where P and Q are defined in imu2d::exp(), and also a jacobian J^delta_data
+ */
+
+ //create delta
+ imu2d::body2delta(data_2d - _calib, _dt, _delta, jac_delta_data);
+
+ // compute delta_cov
+ _delta_cov = jac_delta_data * data_cov_2d * jac_delta_data.transpose();
+
+ // compute jacobian_calib
+ _jac_delta_calib = - jac_delta_data;
+ }
+
+ void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta) const
+ {
+ /* MATHS:
+ * Simple composition,
+ * (D o d)p = Dp + Dth*dp + Dv*dt
+ * (D o d)th = Dth+dth
+ * (D o d)v = Dv + Dth*dv
+ * where Dth*dp and Dth*dv are rotations and not scalar products. The version with jacobians is not used here.
+ */
+ _delta_preint_plus_delta = imu2d::compose(_delta_preint, _delta, _dt);
+ }
+
+ void ProcessorImu2d::statePlusDelta(const VectorComposite& _x,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ VectorComposite& _x_plus_delta) const
+ {
+ assert(_x.includesStructure("POV") && "Any key missing in _x");
+ assert(_delta.size() == 5 && "Wrong _delta vector size");
+ assert(_dt >= 0 && "Time interval _dt is negative!");
+
+ const auto& delta = VectorComposite(_delta, "POV", {2,1,2});
+ /*
+ * MATHS:
+ *
+ * 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);
+
+ }
+
+ void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd& _delta_preint,
+ const Eigen::VectorXd& _delta,
+ const double _dt,
+ Eigen::VectorXd& _delta_preint_plus_delta,
+ Eigen::MatrixXd& _jacobian_delta_preint,
+ Eigen::MatrixXd& _jacobian_delta) const
+ {
+ /*
+ * Expression of the delta integration step, D' = D (+) d:
+ *
+ * Dp' = Dp + Dv*dt + Dq*dp
+ * Dv' = Dv + Dq*dv
+ * Dq' = Dq * dq
+ *
+ * Jacobians for covariance propagation.
+ *
+ * a. With respect to Delta, gives _jacobian_delta_preint = D'_D as:
+ *
+ * D'_D = [ I DR*skew(1)*dp I*dt
+ * 0 1 0
+ * 0 DR*skew(1)*dv I ]
+ *
+ * b. With respect to delta, gives _jacobian_delta = D'_d as:
+ *
+ * D'_d = [ DR 0 0
+ * 0 1 0
+ * 0 0 DR ]
+ *
+ * Note: covariance propagation, i.e., P+ = D_D * P * D_D' + D_d * M * D_d', is done in ProcessorMotion.
+ */
+ imu2d::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in imu2d_tools
+ }
+
+ Eigen::VectorXd ProcessorImu2d::correctDelta (const Eigen::VectorXd& delta_preint,
+ const Eigen::VectorXd& delta_step) const
+ {
+ return imu2d::plus(delta_preint, delta_step);
+ }
+
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/processor/factory_processor.h"
+
+namespace wolf
+{
+ WOLF_REGISTER_PROCESSOR(ProcessorImu2d)
+ WOLF_REGISTER_PROCESSOR_AUTO(ProcessorImu2d)
+}
diff --git a/src/sensor/sensor_imu.cpp b/src/sensor/sensor_imu.cpp
index 488378b4f1f60b1baf670bcb80b22f17180d1f52..8f12c8188ea85bdfff31e1a524a667dddd3cd178 100644
--- a/src/sensor/sensor_imu.cpp
+++ b/src/sensor/sensor_imu.cpp
@@ -24,7 +24,7 @@ SensorImu::SensorImu(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu&
ab_rate_stdev(_params.ab_rate_stdev),
wb_rate_stdev(_params.wb_rate_stdev)
{
- assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 2d.");
+ assert(_extrinsics.size() == 7 && "Wrong extrinsics vector size! Should be 7 for 3d.");
}
SensorImu::~SensorImu()
diff --git a/src/sensor/sensor_imu2d.cpp b/src/sensor/sensor_imu2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5b4c94103b4a18d155043100692feb49e02443db
--- /dev/null
+++ b/src/sensor/sensor_imu2d.cpp
@@ -0,0 +1,37 @@
+#include <imu/sensor/sensor_imu2d.h>
+#include <core/state_block/state_block.h>
+#include <core/state_block/state_angle.h>
+
+namespace wolf {
+
+SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, ParamsSensorImu2dPtr _params) :
+ SensorImu2d(_extrinsics, *_params)
+{
+ //
+}
+
+SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorImu2d& _params) :
+ SensorBase("SensorImu2d", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), std::make_shared<StateBlock>(3, false, nullptr), (Eigen::Vector3d()<<_params.a_noise,_params.a_noise,_params.w_noise).finished(), false, false, true),
+ a_noise(_params.a_noise),
+ w_noise(_params.w_noise),
+ 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)
+{
+ assert(_extrinsics.size() == 3 && "Wrong extrinsics vector size! Should be 3 for 2d.");
+}
+
+SensorImu2d::~SensorImu2d()
+{
+ //
+}
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorImu2d)
+WOLF_REGISTER_SENSOR_AUTO(SensorImu2d);
+} // namespace wolf
diff --git a/src/yaml/processor_imu2d_yaml.cpp b/src/yaml/processor_imu2d_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5d8e528cf6d1a12d0f31e2dee03bb2d9d9874ee9
--- /dev/null
+++ b/src/yaml/processor_imu2d_yaml.cpp
@@ -0,0 +1,53 @@
+/**
+ * \file processor_imu2d_yaml.cpp
+ *
+ * Created on: Nov 24, 2020
+ * \author: igeer
+ */
+
+// wolf yaml
+#include "imu/processor/processor_imu2d.h"
+#include "core/yaml/yaml_conversion.h"
+
+// wolf
+#include "core/common/factory.h"
+
+// yaml-cpp library
+#include <yaml-cpp/yaml.h>
+
+namespace wolf
+{
+
+namespace
+{
+static ParamsProcessorBasePtr createProcessorImu2dParams(const std::string & _filename_dot_yaml)
+{
+ YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+ std::cout << _filename_dot_yaml << '\n';
+
+ if (config["type"].as<std::string>() == "ProcessorImu2d")
+ {
+ YAML::Node kf_vote = config["keyframe_vote"];
+
+ ParamsProcessorImu2dPtr params = std::make_shared<ParamsProcessorImu2d>();
+ params->time_tolerance = config["time_tolerance"] .as<double>();
+ params->unmeasured_perturbation_std = config["unmeasured_perturbation_std"].as<double>();
+
+ params->max_time_span = kf_vote["max_time_span"] .as<double>();
+ params->max_buff_length = kf_vote["max_buff_length"] .as<SizeEigen>();
+ params->dist_traveled = kf_vote["dist_traveled"] .as<double>();
+ params->angle_turned = kf_vote["angle_turned"] .as<double>();
+ params->voting_active = kf_vote["voting_active"] .as<bool>();
+ return params;
+ }
+
+ std::cout << "Bad configuration file. No processor type found." << std::endl;
+ return nullptr;
+}
+
+// Register in the FactorySensor
+const bool WOLF_UNUSED registered_prc_imu2d = FactoryParamsProcessor::registerCreator("ProcessorImu2d", createProcessorImu2dParams);
+
+} // namespace [unnamed]
+
+} // namespace wolf
diff --git a/src/yaml/sensor_imu2d_yaml.cpp b/src/yaml/sensor_imu2d_yaml.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..286e6fb247c253bed93919475c63e753d12f09e7
--- /dev/null
+++ b/src/yaml/sensor_imu2d_yaml.cpp
@@ -0,0 +1,54 @@
+/**
+ * \file sensor_imu2d_yaml.cpp
+ *
+ * Created on: Nov 24, 2020
+ * \author: igeer
+ */
+
+// wolf yaml
+#include "imu/sensor/sensor_imu2d.h"
+#include "core/yaml/yaml_conversion.h"
+
+// wolf
+#include "core/common/factory.h"
+
+// yaml-cpp library
+#include <yaml-cpp/yaml.h>
+
+namespace wolf
+{
+
+namespace
+{
+
+static ParamsSensorBasePtr createParamsSensorImu2d(const std::string & _filename_dot_yaml)
+{
+ YAML::Node config = YAML::LoadFile(_filename_dot_yaml);
+
+ if (config["type"].as<std::string>() == "SensorImu2d")
+ {
+ YAML::Node variances = config["motion_variances"];
+ YAML::Node kf_vote = config["keyframe_vote"];
+
+ ParamsSensorImu2dPtr params = std::make_shared<ParamsSensorImu2d>();
+
+ params->a_noise = variances["a_noise"] .as<double>();
+ params->w_noise = variances["w_noise"] .as<double>();
+ params->ab_initial_stdev = variances["ab_initial_stdev"] .as<double>();
+ 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>();
+
+ return params;
+ }
+
+ std::cout << "Bad configuration file. No sensor type found." << std::endl;
+ return nullptr;
+}
+
+// Register in the FactorySensor
+const bool WOLF_UNUSED registered_imu2d_intr = FactoryParamsSensor::registerCreator("SensorImu2d", createParamsSensorImu2d);
+
+} // namespace [unnamed]
+
+} // namespace wolf
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 42bce1a6d1364c4ea6af8943a55efcbc88345aad..d16b4a3857c88316a48fa5cc79f77a6bc4f7b40d 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -16,18 +16,27 @@ target_link_libraries(gtest_example ${PLUGIN_NAME}) #
wolf_add_gtest(gtest_processor_imu gtest_processor_imu.cpp)
target_link_libraries(gtest_processor_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
+wolf_add_gtest(gtest_processor_imu2d gtest_processor_imu2d.cpp)
+target_link_libraries(gtest_processor_imu2d ${PLUGIN_NAME} ${wolf_LIBRARY})
+
wolf_add_gtest(gtest_imu gtest_imu.cpp)
target_link_libraries(gtest_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
wolf_add_gtest(gtest_imu_tools gtest_imu_tools.cpp)
target_link_libraries(gtest_imu_tools ${PLUGIN_NAME} ${wolf_LIBRARY})
+wolf_add_gtest(gtest_imu2d_tools gtest_imu2d_tools.cpp)
+target_link_libraries(gtest_imu2d_tools ${PLUGIN_NAME} ${wolf_LIBRARY})
+
wolf_add_gtest(gtest_processor_imu_jacobians gtest_processor_imu_jacobians.cpp)
target_link_libraries(gtest_processor_imu_jacobians ${PLUGIN_NAME} ${wolf_LIBRARY})
wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp)
target_link_libraries(gtest_feature_imu ${PLUGIN_NAME} ${wolf_LIBRARY})
+wolf_add_gtest(gtest_factor_imu2d gtest_factor_imu2d.cpp)
+target_link_libraries(gtest_factor_imu2d ${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} ${wolf_LIBRARY})
@@ -43,4 +52,4 @@ wolf_add_gtest(gtest_processor_motion_intrinsics_update gtest_processor_motion_i
target_link_libraries(gtest_processor_motion_intrinsics_update ${PLUGIN_NAME} ${wolf_LIBRARY})
wolf_add_gtest(gtest_sensor_compass gtest_sensor_compass.cpp)
-target_link_libraries(gtest_sensor_compass ${PLUGIN_NAME} ${wolf_LIBRARY})
\ No newline at end of file
+target_link_libraries(gtest_sensor_compass ${PLUGIN_NAME} ${wolf_LIBRARY})
diff --git a/test/gtest_factor_imu2d.cpp b/test/gtest_factor_imu2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..037783f6966b9c96f7016371469c7b522a6ded14
--- /dev/null
+++ b/test/gtest_factor_imu2d.cpp
@@ -0,0 +1,425 @@
+#include <core/ceres_wrapper/solver_ceres.h>
+#include <core/utils/utils_gtest.h>
+
+#include "imu/factor/factor_imu2d.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
+SensorBasePtr sensor = std::make_shared<SensorImu2d>( Vector3d::Zero(), ParamsSensorImu2d() ); // default params: see sensor_imu2d.h
+
+//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(FactorImu2d, check_tree)
+{
+ ASSERT_TRUE(problem_ptr->check(0));
+}
+
+TEST(FactorImu2d, bias_zero_solve_f1)
+{
+ 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));
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and biases, perturb frm1
+ frm0->fix();
+ cap0->fix();
+ frm1->unfix();
+ cap1->fix();
+ frm1->perturb(0.01);
+
+ // solve
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+ //WOLF_INFO(frm1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+}
+}
+
+TEST(FactorImu2d, 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));
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and biases, perturb frm1
+ frm0->unfix();
+ cap0->fix();
+ frm1->fix();
+ cap1->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(FactorImu2d, 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));
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ //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);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
+ frm0->fix();
+ cap0->fix();
+ frm1->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(FactorImu2d, 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 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::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ frm0->fix();
+ cap0->unfix();
+ frm1->fix();
+ cap1->fix();
+ cap0->perturb(0.1);
+
+ // solve
+ std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(cap0->getStateVector(), b0, 1e-6);
+ //WOLF_INFO(cap0->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+}
+}
+
+TEST(FactorImu2d, 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));
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ //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);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and frm1, unfix bias, perturb cap1
+ frm0->fix();
+ cap0->fix();
+ frm1->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(FactorImu2d, 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 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::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ frm0->unfix();
+ cap0->fix();
+ frm1->fix();
+ cap1->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(FactorImu2d, 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 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::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ frm0->fix();
+ cap0->fix();
+ frm1->unfix();
+ cap1->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(FactorImu2d, 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 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::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ frm0->fix();
+ cap0->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();
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ // ::testing::GTEST_FLAG(filter) = "FactorImu2d.no_bias_fixed"; // Test only this one
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_feature_imu.cpp b/test/gtest_feature_imu.cpp
index ef63025f2b0db475bae9ceba850f960755570e08..6d2403b6827319b85236fbf444bc3af8987dbc27 100644
--- a/test/gtest_feature_imu.cpp
+++ b/test/gtest_feature_imu.cpp
@@ -68,7 +68,7 @@ class FeatureImu_test : public testing::Test
sensor_ptr,
data_,
Eigen::Matrix6d::Identity(),
- Eigen::Vector6d::Zero()) );
+ Eigen::Vector6d::Zero().eval()) );
//process data
data_ << 2, 0, 9.8, 0, 0, 0;
diff --git a/test/gtest_imu2d_tools.cpp b/test/gtest_imu2d_tools.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f5ce0cd3d35458ddafeca138fc7fa34db8a8f88d
--- /dev/null
+++ b/test/gtest_imu2d_tools.cpp
@@ -0,0 +1,648 @@
+/*
+ * gtest_imu2d_tools.cpp
+ *
+ * Created on: Nov 17, 2020
+ * Author: igeer
+ */
+
+#include "imu/math/imu2d_tools.h"
+#include <core/utils/utils_gtest.h>
+
+using namespace Eigen;
+using namespace wolf;
+using namespace imu2d;
+
+TEST(Imu2d_tools, identity)
+{
+ VectorXd id_true;
+ id_true.setZero(5);
+
+ VectorXd id = identity<double>();
+ ASSERT_MATRIX_APPROX(id, id_true, 1e-10);
+}
+
+//TEST(Imu2d_tools, identity_split)
+//{
+// VectorXd p(3), qv(4), v(3);
+// Quaterniond q;
+//
+// identity(p,q,v);
+// ASSERT_MATRIX_APPROX(p, Vector3d::Zero(), 1e-10);
+// ASSERT_QUATERNION_APPROX(q, Quaterniond::Identity(), 1e-10);
+// ASSERT_MATRIX_APPROX(v, Vector3d::Zero(), 1e-10);
+//
+// identity(p,qv,v);
+// ASSERT_MATRIX_APPROX(p , Vector3d::Zero(), 1e-10);
+// ASSERT_MATRIX_APPROX(qv, (Vector4d()<<0,0,0,1).finished(), 1e-10);
+// ASSERT_MATRIX_APPROX(v , Vector3d::Zero(), 1e-10);
+//}
+
+TEST(Imu2d_tools, inverse)
+{
+ VectorXd d(5), id(5), iid(5), iiid(5), I(5);
+ double dt = 0.1;
+
+ d << 0, 1, 2, 3, 4;
+
+ id = inverse(d, dt);
+
+ compose(id, d, dt, I);
+ ASSERT_MATRIX_APPROX(I, imu2d::identity(), 1e-10);
+ compose(d, id, -dt, I); // Observe -dt is reversed !!
+ ASSERT_MATRIX_APPROX(I, imu2d::identity(), 1e-10);
+
+ inverse(id, -dt, iid); // Observe -dt is reversed !!
+ ASSERT_MATRIX_APPROX( d, iid, 1e-10);
+ iiid = inverse(iid, dt);
+ ASSERT_MATRIX_APPROX(id, iiid, 1e-10);
+}
+
+TEST(Imu2d_tools, compose){
+ //Compose 2 vectors
+ VectorXd dx1(5), dx2(5), dx3(5);
+ Matrix<double, 5, 1> d1, d2, d3;
+ double dt = 0.001;
+ dx1 << 1, 2, 0.78539816339, 3, 4;
+ d1 = dx1;
+ dx2 << 5, 6, 0.39269908169, 7, 8;
+ d2 = dx2;
+
+ //base function
+ Rotation2D<double> dR1(dx1(2));
+ Vector2d dp2_rot = dR1*dx2.head(2);
+ Vector2d dv2_rot = dR1*dx2.tail(2);
+ Vector2d cdp = dx1.head(2) + dp2_rot + dx1.tail(2)*dt;
+ Vector2d cdv = dx1.tail(2) + dv2_rot;
+ dx3 << cdp,
+ dx1(2) + dx2(2),
+ cdv;
+ VectorXd composedmatrix = compose(dx1, dx2, dt);
+ ASSERT_MATRIX_APPROX(dx3, composedmatrix, 1e-10);
+
+ //// combinations of templates for sum()
+ compose(dx1, dx2, dt, dx3);
+ compose(d1, d2, dt, d3);
+ ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
+
+ compose(d1, dx2, dt, dx3);
+ d3 = compose(dx1, d2, dt);
+ ASSERT_MATRIX_APPROX(d3, dx3, 1e-10);
+
+}
+
+TEST(Imu2d_tools, compose_between)
+{
+ VectorXd dx1(5), dx2(5), dx3(5);
+ Matrix<double, 5, 1> d1, d2, d3;
+ double dt = 0.1;
+
+ dx1 << 0, 1, pi2pi(2), 3, 4;
+ d1 = dx1;
+ dx2 << 5, 6, pi2pi(7), 8, 9;
+ d2 = dx2;
+ compose(dx1, dx2, dt, dx3);
+ compose(d1, d2, dt, d3);
+
+ // minus(d1, d3) should recover delta_2
+ VectorXd diffx(5);
+ Matrix<double,5,1> diff;
+
+ between(dx1, dx3, dt, diffx);
+ ASSERT_MATRIX_APPROX(diffx, dx2, 1e-10);
+ between(d1, d3, dt, diff);
+ ASSERT_MATRIX_APPROX(diff, d2, 1e-10);
+
+ // minus(d3, d1, -dt) should recover inverse(d2, dt)
+ diff = between(d3, d1, -dt);
+ ASSERT_MATRIX_APPROX(diff, inverse(d2, dt), 1e-10);
+ diffx = between(dx3, dx1, -dt);
+ 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, lift_retract)
+//{
+// VectorXd d_min(9); d_min << 0, 1, 2, .3, .4, .5, 6, 7, 8; // use angles in the ball theta < pi
+//
+// // transform to delta
+// VectorXd delta = exp_Imu(d_min);
+//
+// // expected delta
+// Vector3d dp = d_min.head(3);
+// Quaterniond dq = v2q(d_min.segment(3,3));
+// Vector3d dv = d_min.tail(3);
+// VectorXd delta_true(10); delta_true << dp, dq.coeffs(), dv;
+// ASSERT_MATRIX_APPROX(delta, delta_true, 1e-10);
+//
+// // transform to a new tangent -- should be the original tangent
+// VectorXd d_from_delta = log_Imu(delta);
+// ASSERT_MATRIX_APPROX(d_from_delta, d_min, 1e-10);
+//
+// // transform to a new delta -- should be the previous delta
+// VectorXd delta_from_d = exp_Imu(d_from_delta);
+// ASSERT_MATRIX_APPROX(delta_from_d, delta, 1e-10);
+//}
+//
+TEST(Imu2d_tools, plus)
+{
+ VectorXd d1(5), d2(5), d3(5);
+ VectorXd err(5);
+ d1 << 0, 1, 2, 3, 4;
+ err << 0.01, 0.02, 0.03, 0.04, 0.05;
+
+ WOLF_INFO("doing sums");
+ d3.head(2) = d1.head(2) + err.head(2);
+ d3(2) = d1(2) + err(2);
+ d3.tail(2) = d1.tail(2) + err.tail(2);
+
+ WOLF_INFO("doing plus()");
+ imu2d::plus(d1, err, d2);
+ ASSERT_MATRIX_APPROX(diff(d3, d2), VectorXd::Zero(5), 1e-10);
+}
+
+TEST(Imu2d_tools, diff)
+{
+ VectorXd d1(5), d2(5);
+ d1 << 0, 1, 7, 8, 9;
+ d2 = d1;
+
+ VectorXd err(5);
+ diff(d1, d2, err);
+ ASSERT_MATRIX_APPROX(err, VectorXd::Zero(5), 1e-10);
+ ASSERT_MATRIX_APPROX(diff(d1, d2), VectorXd::Zero(5), 1e-10);
+
+ VectorXd d3(5);
+ d3.setRandom();
+ err.head(2) = d3.head(2) - d1.head(2);
+ err(2) = d3(2) - d1(2);
+ err.tail(2) = d3.tail(2) - d1.tail(2);
+
+ ASSERT_MATRIX_APPROX(err, diff(d1, d3), 1e-10);
+
+}
+
+TEST(Imu2d_tools, compose_jacobians)
+{
+ VectorXd d1(5), d2(5), d3(5), d1_pert(5), d2_pert(5), d3_pert(5); // deltas
+ VectorXd t1(5), t2(5); // tangents
+ Matrix<double, 5, 5> J1_a, J2_a, J1_n, J2_n;
+ double dt = 0.1;
+ double dx = 1e-6;
+ d1 << 0, 1, pi2pi(2), 3, 4;
+ d2 << 5, 6, pi2pi(7), 8, 9;
+
+ // analytical jacobians
+ compose(d1, d2, dt, d3, J1_a, J2_a);
+
+ // numerical jacobians
+ for (unsigned int i = 0; i<5; i++)
+ {
+ t1 . setZero();
+ t1(i) = dx;
+
+ // Jac wrt first delta
+ d1_pert = imu2d::plus(d1, t1); // (d1 + t1)
+ d3_pert = compose(d1_pert, d2, dt); // (d1 + t1) + d2
+ t2 = diff(d3, d3_pert); // { (d2 + t1) + d2 } - { d1 + d2 }
+ J1_n.col(i) = t2/dx; // [ { (d2 + t1) + d2 } - { d1 + d2 } ] / dx
+
+ // Jac wrt second delta
+ d2_pert = imu2d::plus(d2, t1); // (d2 + t1)
+ d3_pert = compose(d1, d2_pert, dt); // d1 + (d2 + t1)
+ t2 = diff(d3, d3_pert); // { d1 + (d2 + t1) } - { d1 + d2 }
+ J2_n.col(i) = t2/dx; // [ { d1 + (d2 + t1) } - { d1 + d2 } ] / dx
+ }
+
+ // check that numerical and analytical match
+ ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
+ ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
+}
+
+//TEST(Imu2d_tools, diff_jacobians)
+//{
+// //diff with jacobians is not implemented and doesn't seem necessary
+// VectorXd d1(5), d2(5), d3(5), d1_pert(5), d2_pert(5), d3_pert(5); // deltas
+// VectorXd t1(5), t2(5); // tangents
+// Matrix<double, 5, 5> J1_a, J2_a, J1_n, J2_n;
+// double dx = 1e-6;
+// d1 << 0, 1, pi2pi(2), 3, 4;
+// d2 << 5, 6, pi2pi(7), 8, 9;
+//
+// // analytical jacobians
+// diff(d1, d2, d3, J1_a, J2_a);
+//
+// // numerical jacobians
+// for (unsigned int i = 0; i<9; i++)
+// {
+// t1 . setZero();
+// t1(i) = dx;
+//
+// // Jac wrt first delta
+// d1_pert = plus(d1, t1); // (d1 + t1)
+// d3_pert = diff(d1_pert, d2); // d2 - (d1 + t1)
+// t2 = d3_pert - d3; // { d2 - (d1 + t1) } - { d2 - d1 }
+// J1_n.col(i) = t2/dx; // [ { d2 - (d1 + t1) } - { d2 - d1 } ] / dx
+//
+// // Jac wrt second delta
+// d2_pert = plus(d2, t1); // (d2 + t1)
+// d3_pert = diff(d1, d2_pert); // (d2 + t1) - d1
+// t2 = d3_pert - d3; // { (d2 + t1) - d1 } - { d2 - d1 }
+// J2_n.col(i) = t2/dx; // [ { (d2 + t1) - d1 } - { d2 - d1 } ] / dx
+// }
+//
+// // check that numerical and analytical match
+// ASSERT_MATRIX_APPROX(J1_a, J1_n, 1e-4);
+// ASSERT_MATRIX_APPROX(J2_a, J2_n, 1e-4);
+//}
+
+TEST(Imu2d_tools, body2delta_jacobians)
+{
+ VectorXd delta(5), delta_pert(5); // deltas
+ VectorXd body(3), pert(3);
+ VectorXd tang(5); // tangents
+ Matrix<double, 5, 3> J_a, J_n; // analytic and numerical jacs
+ double dt = 0.01;
+ double dx = 1e-6;
+ body << -1, 1, 0; // jacobians not exact if w != 0
+
+ // analytical jacobians
+ body2delta(body, dt, delta, J_a);
+
+ // numerical jacobians
+ for (unsigned int i = 0; i<3; i++)
+ {
+ pert . setZero();
+ pert(i) = dx;
+
+ // Jac wrt first delta
+ delta_pert = body2delta(body + pert, dt); // delta(body + pert)
+ tang = diff(delta, delta_pert); // delta(body + pert) -- delta(body)
+ J_n.col(i) = tang/dx; // ( delta(body + pert) -- delta(body) ) / dx
+ }
+
+ // check that numerical and analytical match
+ ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
+}
+
+//motion2data is not (yet) implemented
+
+//TEST(motion2data, zero)
+//{
+// Vector6d motion;
+// Vector6d bias;
+// Quaterniond q;
+//
+// motion .setZero();
+// bias .setZero();
+// q .setIdentity();
+//
+// Vector6d data = imu2d::motion2data(motion, q, bias);
+//
+// Vector6d data_true; data_true << -gravity(), Vector3d::Zero();
+//
+// ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
+//}
+//
+//TEST(motion2data, motion)
+//{
+// Vector6d motion, g_extended;
+// Vector6d bias;
+// Quaterniond q;
+//
+// g_extended << gravity() , Vector3d::Zero();
+//
+// motion << 1,2,3, 4,5,6;
+// bias .setZero();
+// q .setIdentity();
+//
+// Vector6d data = imu2d::motion2data(motion, q, bias);
+//
+// Vector6d data_true; data_true = motion - g_extended;
+//
+// ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
+//}
+//
+//TEST(motion2data, bias)
+//{
+// Vector6d motion, g_extended;
+// Vector6d bias;
+// Quaterniond q;
+//
+// g_extended << gravity() , Vector3d::Zero();
+//
+// motion .setZero();
+// bias << 1,2,3, 4,5,6;
+// q .setIdentity();
+//
+// Vector6d data = imu2d::motion2data(motion, q, bias);
+//
+// Vector6d data_true; data_true = bias - g_extended;
+//
+// ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
+//}
+//
+//TEST(motion2data, orientation)
+//{
+// Vector6d motion, g_extended;
+// Vector6d bias;
+// Quaterniond q;
+//
+// g_extended << gravity() , Vector3d::Zero();
+//
+// motion .setZero();
+// bias .setZero();
+// q = v2q(Vector3d(M_PI/2, 0, 0)); // turn 90 deg in X axis
+//
+// Vector6d data = imu2d::motion2data(motion, q, bias);
+//
+// Vector6d data_true; data_true << 0,-gravity()(2),0, 0,0,0;
+//
+// ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
+//}
+//
+//TEST(motion2data, AllRandom)
+//{
+// Vector6d motion, g_extended;
+// Vector6d bias;
+// Quaterniond q;
+//
+// motion .setRandom();
+// bias .setRandom();
+// q.coeffs() .setRandom().normalize();
+//
+// g_extended << q.conjugate()*gravity() , Vector3d::Zero();
+//
+// Vector6d data = imu2d::motion2data(motion, q, bias);
+//
+// Vector6d data_true; data_true = motion + bias - g_extended;
+//
+// ASSERT_MATRIX_APPROX(data, data_true, 1e-12);
+//}
+//
+///* Integrate acc and angVel motion, obtain Delta_preintegrated
+// * Input:
+// * N: number of steps
+// * q0: initial orientaiton
+// * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
+// * bias_real: the real bias of the Imu
+// * bias_preint: the bias used for Delta pre-integration
+// * Output:
+// * return: the preintegrated delta
+// */
+//VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt)
+//{
+// VectorXd data(6);
+// VectorXd body(6);
+// VectorXd delta(10);
+// VectorXd Delta(10), Delta_plus(10);
+// Delta << 0,0,0, 0,0,0,1, 0,0,0;
+// Quaterniond q(q0);
+// for (int n = 0; n<N; n++)
+// {
+// data = motion2data(motion, q, bias_real);
+// q = q*exp_q(motion.tail(3)*dt);
+// body = data - bias_preint;
+// delta = body2delta(body, dt);
+// Delta_plus = compose(Delta, delta, dt);
+// Delta = Delta_plus;
+// }
+// return Delta;
+//}
+//
+///* Integrate acc and angVel motion, obtain Delta_preintegrated
+// * Input:
+// * N: number of steps
+// * q0: initial orientaiton
+// * motion: [ax, ay, az, wx, wy, wz] as the true magnitudes in body brame
+// * bias_real: the real bias of the Imu
+// * bias_preint: the bias used for Delta pre-integration
+// * Output:
+// * J_D_b: the Jacobian of the preintegrated delta wrt the bias
+// * return: the preintegrated delta
+// */
+//VectorXd integrateDelta(int N, const Quaterniond& q0, const VectorXd& motion, const VectorXd& bias_real, const VectorXd& bias_preint, double dt, Matrix<double, 9, 6>& J_D_b)
+//{
+// VectorXd data(6);
+// VectorXd body(6);
+// VectorXd delta(10);
+// Matrix<double, 9, 6> J_d_d, J_d_b;
+// Matrix<double, 9, 9> J_D_D, J_D_d;
+// VectorXd Delta(10), Delta_plus(10);
+// Quaterniond q;
+//
+// Delta << 0,0,0, 0,0,0,1, 0,0,0;
+// J_D_b.setZero();
+// q = q0;
+// for (int n = 0; n<N; n++)
+// {
+// // Simulate data
+// data = motion2data(motion, q, bias_real);
+// q = q*exp_q(motion.tail(3)*dt);
+// // Motion::integrateOneStep()
+// { // Imu::computeCurrentDelta
+// body = data - bias_preint;
+// body2delta(body, dt, delta, J_d_d);
+// J_d_b = - J_d_d;
+// }
+// { // Imu::deltaPlusDelta
+// compose(Delta, delta, dt, Delta_plus, J_D_D, J_D_d);
+// }
+// // Motion:: jac calib
+// J_D_b = J_D_D*J_D_b + J_D_d*J_d_b;
+// // Motion:: buffer
+// Delta = Delta_plus;
+// }
+// return Delta;
+//}
+//
+//TEST(Imu2d_tools, integral_jacobian_bias)
+//{
+// VectorXd Delta(10), Delta_pert(10); // deltas
+// VectorXd bias_real(6), pert(6), bias_pert(6), bias_preint(6);
+// VectorXd body(6), data(6), motion(6);
+// VectorXd tang(9); // tangents
+// Matrix<double, 9, 6> J_a, J_n; // analytic and numerical jacs
+// double dt = 0.001;
+// double dx = 1e-4;
+// Quaterniond q0(3, 4, 5, 6); q0.normalize();
+// motion << .1, .2, .3, .3, .2, .1;
+// bias_real << .002, .004, .001, .003, .002, .001;
+// bias_preint << .004, .005, .006, .001, .002, .003;
+//
+// int N = 500; // # steps of integration
+//
+// // Analytical Jacobians
+// Delta = integrateDelta(N, q0, motion, bias_real, bias_preint, dt, J_a);
+//
+// // numerical jacobians
+// for (unsigned int i = 0; i<6; i++)
+// {
+// pert . setZero();
+// pert(i) = dx;
+//
+// bias_pert = bias_preint + pert;
+// Delta_pert = integrateDelta(N, q0, motion, bias_real, bias_pert, dt);
+// tang = diff(Delta, Delta_pert); // Delta(bias + pert) -- Delta(bias)
+// J_n.col(i) = tang/dx; // ( Delta(bias + pert) -- Delta(bias) ) / dx
+// }
+//
+// // check that numerical and analytical match
+// ASSERT_MATRIX_APPROX(J_a, J_n, 1e-4);
+//}
+//
+//TEST(Imu2d_tools, delta_correction)
+//{
+// VectorXd Delta(10), Delta_preint(10), Delta_corr; // deltas
+// VectorXd bias(6), pert(6), bias_preint(6);
+// VectorXd body(6), motion(6);
+// VectorXd tang(9); // tangents
+// Matrix<double, 9, 6> J_b; // analytic and numerical jacs
+// Vector4d qv;;
+// double dt = 0.001;
+// Quaterniond q0(3, 4, 5, 6); q0.normalize();
+// motion << 1, 2, 3, 3, 2, 1; motion *= .1;
+//
+// int N = 1000; // # steps of integration
+//
+// // preintegration with correct bias
+// bias << .004, .005, .006, .001, .002, .003;
+// Delta = integrateDelta(N, q0, motion, bias, bias, dt);
+//
+// // preintegration with wrong bias
+// pert << .002, -.001, .003, -.0003, .0002, -.0001;
+// bias_preint = bias + pert;
+// Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b);
+//
+// // correct perturbated
+// Vector9d step = J_b*(bias-bias_preint);
+// Delta_corr = plus(Delta_preint, step);
+//
+// // Corrected delta should match real delta
+// ASSERT_MATRIX_APPROX(Delta, Delta_corr, 1e-5);
+//
+// // diff between real and corrected should be zero
+// ASSERT_MATRIX_APPROX(diff(Delta, Delta_corr), Vector9d::Zero(), 1e-5);
+//
+// // diff between preint and corrected deltas should be the jacobian-computed step
+// ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-5);
+//}
+//
+//TEST(Imu2d_tools, full_delta_residual)
+//{
+// VectorXd x1(10), x2(10);
+// VectorXd Delta(10), Delta_preint(10), Delta_corr(10);
+// VectorXd Delta_real(9), Delta_err(9), Delta_diff(10), Delta_exp(10);
+// VectorXd bias(6), pert(6), bias_preint(6), bias_null(6);
+// VectorXd body(6), motion(6);
+// VectorXd tang(9); // tangents
+// Matrix<double, 9, 6> J_b; // analytic and numerical jacs
+// double dt = 0.001;
+// Quaterniond q0; q0.setIdentity();
+//
+// x1 << 0,0,0, 0,0,0,1, 0,0,0;
+// motion << .3, .2, .1, .1, .2, .3; // acc and gyro
+//// motion << .3, .2, .1, .0, .0, .0; // only acc
+//// motion << .0, .0, .0, .1, .2, .3; // only gyro
+// bias << 0.01, 0.02, 0.003, 0.002, 0.005, 0.01;
+// bias_null << 0, 0, 0, 0, 0, 0;
+//// bias_preint << 0.003, 0.002, 0.001, 0.001, 0.002, 0.003;
+// bias_preint = bias_null;
+//
+// int N = 1000; // # steps of integration
+//
+// // preintegration with no bias
+// Delta_real = integrateDelta(N, q0, motion, bias_null, bias_null, dt);
+//
+// // final state
+// x2 = composeOverState(x1, Delta_real, 1000*dt);
+//
+// // preintegration with the correct bias
+// Delta = integrateDelta(N, q0, motion, bias, bias, dt);
+//
+// ASSERT_MATRIX_APPROX(Delta, Delta_real, 1e-4);
+//
+// // preintegration with wrong bias
+// Delta_preint = integrateDelta(N, q0, motion, bias, bias_preint, dt, J_b);
+//
+// // compute correction step
+// Vector9d step = J_b*(bias-bias_preint);
+//
+// // correct preintegrated delta
+// Delta_corr = plus(Delta_preint, step);
+//
+// // Corrected delta should match real delta
+// ASSERT_MATRIX_APPROX(Delta_real, Delta_corr, 1e-3);
+//
+// // diff between real and corrected should be zero
+// ASSERT_MATRIX_APPROX(diff(Delta_real, Delta_corr), Vector9d::Zero(), 1e-3);
+//
+// // expected delta
+// Delta_exp = betweenStates(x1, x2, N*dt);
+//
+// ASSERT_MATRIX_APPROX(Delta_exp, Delta_corr, 1e-3);
+//
+// // compute diff between expected and corrected
+//// Matrix<double, 9, 9> J_err_exp, J_err_corr;
+// diff(Delta_exp, Delta_corr, Delta_err); //, J_err_exp, J_err_corr);
+//
+// ASSERT_MATRIX_APPROX(Delta_err, Vector9d::Zero(), 1e-3);
+//
+// // compute error between expected and preintegrated
+// Delta_err = diff(Delta_preint, Delta_exp);
+//
+// // diff between preint and corrected deltas should be the jacobian-computed step
+// ASSERT_MATRIX_APPROX(diff(Delta_preint, Delta_corr), step, 1e-3);
+// /* This implies:
+// * - Since D_corr is expected to be similar to D_exp
+// * step ~ diff(Delta_preint, Delta_exp)
+// * - the residual can be computed with a regular '-' :
+// * residual = diff(D_preint, D_exp) - J_bias * (bias - bias_preint)
+// */
+//
+//// WOLF_TRACE("Delta real: ", Delta_real.transpose());
+//// WOLF_TRACE("x2: ", x2.transpose());
+//// WOLF_TRACE("Delta: ", Delta.transpose());
+//// WOLF_TRACE("Delta pre: ", Delta_preint.transpose());
+//// WOLF_TRACE("Jac bias: \n", J_b);
+//// WOLF_TRACE("Delta step: ", step.transpose());
+//// WOLF_TRACE("Delta cor: ", Delta_corr.transpose());
+//// WOLF_TRACE("Delta exp: ", Delta_exp.transpose());
+//// WOLF_TRACE("Delta err: ", Delta_err.transpose());
+//// WOLF_TRACE("Delta err exp-pre: ", Delta_err.transpose());
+//}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+// ::testing::GTEST_FLAG(filter) = "Imu2d_tools.delta_correction";
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_processor_imu.cpp b/test/gtest_processor_imu.cpp
index 4099cf133f000dff71b7eb890c22a9c25c529605..79db6e39e80ca013f5ea4bc833844984fa69d5c3 100644
--- a/test/gtest_processor_imu.cpp
+++ b/test/gtest_processor_imu.cpp
@@ -66,7 +66,7 @@ class ProcessorImut : public testing::Test
x0.resize(10);
// Create one capture to store the Imu data arriving from (sensor / callback / file / etc.)
- cap_imu_ptr = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
+ cap_imu_ptr = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector6d::Zero().eval());
}
void TearDown() override
@@ -154,7 +154,7 @@ TEST(ProcessorImu, voteForKeyFrame)
data_cov(0,0) = 0.5;
// Create the captureImu to store the Imu data arriving from (sensor / callback / file / etc.)
- std::shared_ptr<wolf::CaptureImu> cap_imu_ptr = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
+ std::shared_ptr<wolf::CaptureImu> cap_imu_ptr = make_shared<CaptureImu>(t, sensor_ptr, data, data_cov, Vector6d::Zero().eval());
// process this capture for joining prior KF (t=0) and set it as origin KF
cap_imu_ptr->process();
diff --git a/test/gtest_processor_imu2d.cpp b/test/gtest_processor_imu2d.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4e0084f3002024e2bd04cd9896bcb49e798b8cd7
--- /dev/null
+++ b/test/gtest_processor_imu2d.cpp
@@ -0,0 +1,243 @@
+/**
+ * \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;
+ 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.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(ProcessorImu2d_constructors, ALL)
+{
+ //constructor with ProcessorImu2dParamsPtr argument only
+ ParamsProcessorImu2dPtr param_ptr = std::make_shared<ParamsProcessorImu2d>();
+ param_ptr->max_time_span = 2.0;
+ param_ptr->max_buff_length = 20000;
+ param_ptr->dist_traveled = 2.0;
+ param_ptr->angle_turned = 2.0;
+
+ ProcessorImu2dPtr prc1 = std::make_shared<ProcessorImu2d>(param_ptr);
+ ASSERT_EQ(prc1->getMaxTimeSpan(), param_ptr->max_time_span);
+ ASSERT_EQ(prc1->getMaxBuffLength(), param_ptr->max_buff_length);
+ ASSERT_EQ(prc1->getDistTraveled(), param_ptr->dist_traveled);
+ ASSERT_EQ(prc1->getAngleTurned(), param_ptr->angle_turned);
+
+ //Factory constructor with pointers
+ std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+ ProblemPtr problem = Problem::create("POV", 2);
+ Vector3d extrinsics = (Vector3d()<<1,0, 0).finished();
+ std::cout << "creating sensor_ptr" << std::endl;
+ SensorBasePtr sensor_ptr = problem->installSensor("SensorImu2d", "Main Imu", extrinsics, wolf_root + "/demos/sensor_imu2d.yaml");
+ std::cout << "created sensor_ptr" << std::endl;
+ ParamsProcessorImu2dPtr params_default = std::make_shared<ParamsProcessorImu2d>();
+ ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Imu pre-integrator", sensor_ptr, params_default);
+ ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxTimeSpan(), params_default->max_time_span);
+ ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxBuffLength(), params_default->max_buff_length);
+ ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getDistTraveled(), params_default->dist_traveled);
+ ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getAngleTurned(), params_default->angle_turned);
+
+ //Factory constructor with yaml
+ processor_ptr = problem->installProcessor("ProcessorImu2d", "Sec Imu pre-integrator", "Main Imu", wolf_root + "/demos/processor_imu2d.yaml");
+ ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxTimeSpan(), 2.0);
+ ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxBuffLength(), 20000);
+ ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getDistTraveled(), 2.0);
+ ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getAngleTurned(), 0.2);
+}
+
+TEST_F(ProcessorImu2dTest, Prior)
+{
+ // Set the origin
+ x0c['P'] = Vector2d(1,2);
+ x0c['O'] = Vector1d(0);
+ x0c['V'] = Vector2d(4,5);
+ auto KF0 = problem->setPriorFix(x0c, t0, dt/2);
+ processor_motion->setOrigin(KF0);
+ //WOLF_DEBUG("x0 =", x0c);
+ //WOLF_DEBUG("KF0 state =", problem->getTrajectory()->getFrameMap().at(t)->getState("POV"));
+}
+
+TEST_F(ProcessorImu2dTest, MRUA)
+{
+ data << 1, 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, dt/2);
+ processor_motion->setOrigin(KF0);
+
+ //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_DEBUG("Current State: ", problem->getState()['P'].transpose());
+ //WOLF_DEBUG((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)*std::pow(t-t0, 2), problem->getState()['P'], 1e-9);
+ }
+}
+
+TEST_F(ProcessorImu2dTest, parabola)
+{
+ double v0 = 10;
+ double a = 1;
+
+ Vector2d pos;
+ // Set the origin
+ x0c['P'] = Vector2d(0, 0);
+ x0c['O'] = Vector1d(0);
+ x0c['V'] = Vector2d(v0, 0);
+
+ data_cov *= 1e-3;
+ auto KF0 = problem->setPriorFix(x0c, t0, dt/2);
+ processor_motion->setOrigin(KF0);
+
+ for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){
+ C->setTimeStamp(t);
+ data << 0, a, 0, 0, 0, 0;
+ C->setData(data);
+ C->setDataCovariance(data_cov);
+ C->process();
+ //WOLF_DEBUG("Current State: ", problem->getState());
+ pos << v0*(t-t0),
+ 0.5*a*std::pow(t-t0, 2);
+ //WOLF_DEBUG("Calculated State: ", pos.transpose());
+ EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9);
+ EXPECT_MATRIX_APPROX(Vector2d(v0, a*(t-t0)), problem->getState()['V'], 1e-9);
+ }
+}
+
+TEST_F(ProcessorImu2dTest, parabola_deluxe)
+{
+ Vector2d v0(13, 56);
+ Vector2d a(1, 2);
+
+ Vector2d pos;
+ // Set the origin
+ x0c['P'] = Vector2d(0, 0);
+ x0c['O'] = Vector1d(0);
+ x0c['V'] = v0;
+
+ data_cov *= 1e-3;
+ auto KF0 = problem->setPriorFix(x0c, t0, dt/2);
+ processor_motion->setOrigin(KF0);
+
+ for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){
+ C->setTimeStamp(t);
+ data << a(0), a(1), 0, 0, 0, 0;
+ C->setData(data);
+ C->setDataCovariance(data_cov);
+ C->process();
+ //WOLF_DEBUG("Current State: ", problem->getState());
+ pos = v0*(t-t0) + 0.5*a*std::pow(t-t0, 2);
+ //WOLF_DEBUG("Calculated State: ", pos.transpose());
+ EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9);
+ EXPECT_MATRIX_APPROX(v0 + a*(t-t0), problem->getState()['V'], 1e-9);
+ }
+}
+
+TEST_F(ProcessorImu2dTest, Circular_move)
+{
+ double pi = 3.14159265358993;
+ double r = 1;
+ double w = 1;
+ double alpha = pi/4.;
+ 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);
+
+ WOLF_DEBUG("Current State: ", problem->getState());
+ 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;
+ C->setData(data);
+ C->setDataCovariance(data_cov);
+ C->process();
+ WOLF_DEBUG("Current State: ", problem->getState());
+ pos << r*std::cos( w*(t-t0) ),
+ r*std::sin( w*(t-t0) );
+ WOLF_DEBUG("Calculated State: ", pos.transpose());
+ EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ //::testing::GTEST_FLAG(filter) = "ProcessorImu2dt.check_Covariance";
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/processor_imu2d_UnitTester.cpp b/test/processor_imu2d_UnitTester.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6611c9b1461993fb9fb09268a756d7f13c93f4ec
--- /dev/null
+++ b/test/processor_imu2d_UnitTester.cpp
@@ -0,0 +1,13 @@
+#include "processor_imu2d_UnitTester.h"
+
+namespace wolf {
+
+ProcessorImu2d_UnitTester::ProcessorImu2d_UnitTester() :
+ ProcessorImu2d(std::make_shared<ParamsProcessorImu2d>()),
+ Dq_out_(nullptr)
+{}
+
+ProcessorImu2d_UnitTester::~ProcessorImu2d_UnitTester(){}
+
+} // namespace wolf
+
diff --git a/test/processor_imu2d_UnitTester.h b/test/processor_imu2d_UnitTester.h
new file mode 100644
index 0000000000000000000000000000000000000000..30cd61fed36b4b2e74609c7cfc2af748f0f51f38
--- /dev/null
+++ b/test/processor_imu2d_UnitTester.h
@@ -0,0 +1,379 @@
+
+#ifndef PROCESSOR_IMU2D_UNITTESTER_H
+#define PROCESSOR_IMU2D_UNITTESTER_H
+
+// Wolf
+#include "imu/processor/processor_imu2d.h"
+#include "core/processor/processor_motion.h"
+
+namespace wolf {
+ struct Imu_jac_bias{ //struct used for checking jacobians by finite difference
+
+ Imu_jac_bias(Eigen::Matrix<Eigen::VectorXd,6,1> _Deltas_noisy_vect,
+ Eigen::VectorXd _Delta0 ,
+ Eigen::Matrix3d _dDp_dab,
+ Eigen::Matrix3d _dDv_dab,
+ Eigen::Matrix3d _dDp_dwb,
+ Eigen::Matrix3d _dDv_dwb,
+ Eigen::Matrix3d _dDq_dwb) :
+ Deltas_noisy_vect_(_Deltas_noisy_vect),
+ Delta0_(_Delta0) ,
+ dDp_dab_(_dDp_dab),
+ dDv_dab_(_dDv_dab),
+ dDp_dwb_(_dDp_dwb),
+ dDv_dwb_(_dDv_dwb),
+ dDq_dwb_(_dDq_dwb)
+ {
+ //
+ }
+
+ Imu_jac_bias(){
+
+ for (int i=0; i<6; i++){
+ Deltas_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
+ }
+
+ Delta0_ = Eigen::VectorXd::Zero(1,1);
+ dDp_dab_ = Eigen::Matrix3d::Zero();
+ dDv_dab_ = Eigen::Matrix3d::Zero();
+ dDp_dwb_ = Eigen::Matrix3d::Zero();
+ dDv_dwb_ = Eigen::Matrix3d::Zero();
+ dDq_dwb_ = Eigen::Matrix3d::Zero();
+ }
+
+ Imu_jac_bias(Imu_jac_bias const & toCopy){
+
+ Deltas_noisy_vect_ = toCopy.Deltas_noisy_vect_;
+ Delta0_ = toCopy.Delta0_;
+ dDp_dab_ = toCopy.dDp_dab_;
+ dDv_dab_ = toCopy.dDv_dab_;
+ dDp_dwb_ = toCopy.dDp_dwb_;
+ dDv_dwb_ = toCopy.dDv_dwb_;
+ dDq_dwb_ = toCopy.dDq_dwb_;
+ }
+
+ public:
+ /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
+ place 1 : added da_bx in data place 2 : added da_by in data place 3 : added da_bz in data
+ place 4 : added dw_bx in data place 5 : added dw_by in data place 6 : added dw_bz in data
+ */
+ Eigen::Matrix<Eigen::VectorXd,6,1> Deltas_noisy_vect_;
+ Eigen::VectorXd Delta0_;
+ Eigen::Matrix3d dDp_dab_;
+ Eigen::Matrix3d dDv_dab_;
+ Eigen::Matrix3d dDp_dwb_;
+ Eigen::Matrix3d dDv_dwb_;
+ Eigen::Matrix3d dDq_dwb_;
+
+ public:
+ void copyfrom(Imu_jac_bias const& right){
+
+ Deltas_noisy_vect_ = right.Deltas_noisy_vect_;
+ Delta0_ = right.Delta0_;
+ dDp_dab_ = right.dDp_dab_;
+ dDv_dab_ = right.dDv_dab_;
+ dDp_dwb_ = right.dDp_dwb_;
+ dDv_dwb_ = right.dDv_dwb_;
+ dDq_dwb_ = right.dDq_dwb_;
+ }
+ };
+
+ struct Imu_jac_deltas{
+
+ Imu_jac_deltas(Eigen::VectorXd _Delta0,
+ Eigen::VectorXd _delta0,
+ Eigen::Matrix<Eigen::VectorXd,9,1> _Delta_noisy_vect,
+ Eigen::Matrix<Eigen::VectorXd,9,1> _delta_noisy_vect,
+ Eigen::MatrixXd _jacobian_delta_preint,
+ Eigen::MatrixXd _jacobian_delta ) :
+ Delta0_(_Delta0),
+ delta0_(_delta0),
+ Delta_noisy_vect_(_Delta_noisy_vect),
+ delta_noisy_vect_(_delta_noisy_vect),
+ jacobian_delta_preint_(_jacobian_delta_preint),
+ jacobian_delta_(_jacobian_delta)
+ {
+ //
+ }
+
+ Imu_jac_deltas(){
+ for (int i=0; i<9; i++){
+ Delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
+ delta_noisy_vect_(i) = Eigen::VectorXd::Zero(1,1);
+ }
+
+ Delta0_ = Eigen::VectorXd::Zero(1,1);
+ delta0_ = Eigen::VectorXd::Zero(1,1);
+ jacobian_delta_preint_ = Eigen::MatrixXd::Zero(9,9);
+ jacobian_delta_ = Eigen::MatrixXd::Zero(9,9);
+ }
+
+ Imu_jac_deltas(Imu_jac_deltas const & toCopy){
+
+ Delta_noisy_vect_ = toCopy.Delta_noisy_vect_;
+ delta_noisy_vect_ = toCopy.delta_noisy_vect_;
+
+ Delta0_ = toCopy.Delta0_;
+ delta0_ = toCopy.delta0_;
+ jacobian_delta_preint_ = toCopy.jacobian_delta_preint_;
+ jacobian_delta_ = toCopy.jacobian_delta_;
+ }
+
+ public:
+ /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
+ Elements at place 0 are those not affected by the bias noise that we add (Delta_noise, delta_noise -> dPx, dpx, dVx, dvx,..., dOz,doz).
+ Delta_noisy_vect_ delta_noisy_vect_
+ 0: + 0, 0: + 0
+ 1: +dPx, 2: +dPy, 3: +dPz 1: + dpx, 2: +dpy, 3: +dpz
+ 4: +dOx, 5: +dOy, 6: +dOz 4: + dox, 5: +doy, 6: +doz
+ 7: +dVx, 8: +dVy, 9: +dVz 7: + dvx, 9: +dvy, +: +dvz
+ */
+ Eigen::VectorXd Delta0_; //this will contain the Delta not affected by noise
+ Eigen::VectorXd delta0_; //this will contain the delta not affected by noise
+ Eigen::Matrix<Eigen::VectorXd,9,1> Delta_noisy_vect_; //this will contain the Deltas affected by noises
+ Eigen::Matrix<Eigen::VectorXd,9,1> delta_noisy_vect_; //this will contain the deltas affected by noises
+ Eigen::MatrixXd jacobian_delta_preint_;
+ Eigen::MatrixXd jacobian_delta_;
+
+ public:
+ void copyfrom(Imu_jac_deltas const& right){
+
+ Delta_noisy_vect_ = right.Delta_noisy_vect_;
+ delta_noisy_vect_ = right.delta_noisy_vect_;
+ Delta0_ = right.Delta0_;
+ delta0_ = right.delta0_;
+ jacobian_delta_preint_ = right.jacobian_delta_preint_;
+ jacobian_delta_ = right.jacobian_delta_;
+ }
+ };
+
+ class ProcessorImu2d_UnitTester : public ProcessorImu2d{
+
+ public:
+
+ ProcessorImu2d_UnitTester();
+ ~ProcessorImu2d_UnitTester() override;
+
+ //Functions to test jacobians with finite difference method
+
+ /* params :
+ _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
+ _dt : time interval between 2 Imu measurements
+ da_b : bias noise to add - scalar because adding the same noise to each component of bias (abx, aby, abz, wbx, wby, wbz) one by one.
+ */
+ Imu_jac_bias finite_diff_ab(const double _dt,
+ Eigen::Vector6d& _data,
+ const double& da_b,
+ const Eigen::Matrix<double,10,1>& _delta_preint0);
+
+ /* params :
+ _data : input data vector (size 6 : ax,ay,az,wx,wy,wz)
+ _dt : time interval between 2 Imu measurements
+ _Delta_noise : noise to add to Delta_preint (D1 in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
+ _delta_noise : noise to add to instantaneous delta (d in D = D1 + d), vector 9 because rotation expressed as a vector (R2v(q.matrix()))
+ */
+ Imu_jac_deltas finite_diff_noise(const double& _dt,
+ Eigen::Vector6d& _data,
+ const Eigen::Matrix<double,9,1>& _Delta_noise,
+ const Eigen::Matrix<double,9,1>& _delta_noise,
+ const Eigen::Matrix<double,10,1>& _Delta0);
+
+ public:
+ static ProcessorBasePtr create(const std::string& _unique_name,
+ const ParamsProcessorBasePtr _params,
+ const SensorBasePtr = nullptr);
+
+ public:
+ // Maps quat, to be used as temporary
+ Eigen::Map<Eigen::Quaterniond> Dq_out_;
+
+ };
+
+}
+
+/////////////////////////////////////////////////////////
+// IMPLEMENTATION. Put your implementation includes here
+/////////////////////////////////////////////////////////
+
+// Wolf
+#include "core/state_block/state_block.h"
+#include "core/math/rotations.h"
+
+namespace wolf{
+
+ //Functions to test jacobians with finite difference method
+inline Imu_jac_bias ProcessorImu2d_UnitTester::finite_diff_ab(const double _dt,
+ Eigen::Vector6d& _data,
+ const double& da_b,
+ const Eigen::Matrix<double,10,1>& _delta_preint0)
+{
+ //TODO : need to use a reset function here to make sure jacobians have not been used before --> reset everything
+ ///Define all the needed variables
+ Eigen::VectorXd Delta0;
+ Eigen::Matrix<Eigen::VectorXd,6,1> Deltas_noisy_vect;
+ Eigen::Vector6d data0;
+ data0 = _data;
+
+ Eigen::MatrixXd data_cov;
+ Eigen::MatrixXd jacobian_delta_preint;
+ Eigen::MatrixXd jacobian_delta;
+ Eigen::VectorXd delta_preint_plus_delta0;
+ data_cov.resize(6,6);
+ jacobian_delta_preint.resize(9,9);
+ jacobian_delta.resize(9,9);
+ delta_preint_plus_delta0.resize(10);
+
+ //set all variables to 0
+ data_cov = Eigen::MatrixXd::Zero(6,6);
+ jacobian_delta_preint = Eigen::MatrixXd::Zero(9,9);
+ jacobian_delta = Eigen::MatrixXd::Zero(9,9);
+ delta_preint_plus_delta0 << 0,0,0, 0,0,0,1 ,0,0,0; //PQV
+
+ Vector6d bias = Vector6d::Zero();
+
+ /*The following vectors will contain all the matrices and deltas needed to compute the finite differences.
+ place 1 : added da_bx in data place 2 : added da_by in data place 3 : added da_bz in data
+ place 4 : added dw_bx in data place 5 : added dw_by in data place 6 : added dw_bz in data
+ */
+
+ Eigen::Matrix3d dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb;
+
+ //Deltas without use of da_b
+ computeCurrentDelta(_data, data_cov, bias, _dt,delta_,delta_cov_,jacobian_delta_calib_);
+ deltaPlusDelta(_delta_preint0, delta_, _dt, delta_preint_plus_delta0, jacobian_delta_preint, jacobian_delta);
+ MatrixXd jacobian_bias = jacobian_delta * jacobian_delta_calib_;
+ Delta0 = delta_preint_plus_delta0; //this is the first preintegrated delta, not affected by any added bias noise
+ dDp_dab = jacobian_bias.block(0,0,3,3);
+ dDp_dwb = jacobian_bias.block(0,3,3,3);
+ dDq_dwb = jacobian_bias.block(3,3,3,3);
+ dDv_dab = jacobian_bias.block(6,0,3,3);
+ dDv_dwb = jacobian_bias.block(6,3,3,3);
+
+
+ // propagate bias noise
+ for(int i=0; i<6; i++){
+ //need to reset stuff here
+ delta_preint_plus_delta0 << 0,0,0, 0,0,0,1 ,0,0,0; //PQV
+ data_cov = Eigen::MatrixXd::Zero(6,6);
+
+ // add da_b
+ _data = data0;
+ _data(i) = _data(i) - da_b; //- because a = a_m − a_b + a_n, in out case, a = a_m − a_b - da_b + a_n
+ //data2delta
+ computeCurrentDelta(_data, data_cov, bias, _dt, delta_, delta_cov_, jacobian_delta_calib_);
+ deltaPlusDelta(_delta_preint0, delta_, _dt, delta_preint_plus_delta0, jacobian_delta_preint, jacobian_delta);
+ Deltas_noisy_vect(i) = delta_preint_plus_delta0; //preintegrated deltas affected by added bias noise
+ }
+
+ Imu_jac_bias bias_jacobians(Deltas_noisy_vect, Delta0, dDp_dab, dDv_dab, dDp_dwb, dDv_dwb, dDq_dwb);
+ return bias_jacobians;
+}
+
+inline Imu_jac_deltas ProcessorImu2d_UnitTester::finite_diff_noise(const double& _dt, Eigen::Vector6d& _data, const Eigen::Matrix<double,9,1>& _Delta_noise, const Eigen::Matrix<double,9,1>& _delta_noise, const Eigen::Matrix<double,10,1>& _Delta0)
+{
+ //we do not propagate any noise from data vector
+ Eigen::VectorXd Delta_; //will contain the preintegrated Delta affected by added noise
+ Eigen::VectorXd delta0; //will contain the delta not affected by added noise
+ // delta_ that /will contain the delta affected by added noise is declared in processor_motion.h
+ Eigen::VectorXd delta_preint_plus_delta;
+ delta0.resize(10);
+ delta_preint_plus_delta.resize(10);
+ delta_preint_plus_delta << 0,0,0 ,0,0,0,1 ,0,0,0;
+
+ Eigen::MatrixXd jacobian_delta_preint; //will be used as input for deltaPlusDelta
+ Eigen::MatrixXd jacobian_delta; //will be used as input for deltaPlusDelta
+ jacobian_delta_preint.resize(9,9);
+ jacobian_delta.resize(9,9);
+ jacobian_delta_preint.setIdentity(9,9);
+ jacobian_delta.setIdentity(9,9);
+ Eigen::MatrixXd jacobian_delta_preint0; //will contain the jacobian we want to check
+ Eigen::MatrixXd jacobian_delta0; //will contain the jacobian we want to check
+ jacobian_delta_preint0.resize(9,9);
+ jacobian_delta0.resize(9,9);
+ jacobian_delta_preint0.setIdentity(9,9);
+ jacobian_delta0.setIdentity(9,9);
+
+ Eigen::MatrixXd data_cov; //will be used filled with Zeros as input for data2delta
+ data_cov.resize(6,6);
+ data_cov = Eigen::MatrixXd::Zero(6,6);
+
+ Eigen::Matrix<Eigen::VectorXd,9,1> Delta_noisy_vect; //this will contain the Deltas affected by noises
+ Eigen::Matrix<Eigen::VectorXd,9,1> delta_noisy_vect; //this will contain the deltas affected by noises
+
+ Vector6d bias = Vector6d::Zero();
+
+ computeCurrentDelta(_data, data_cov, bias,_dt,delta_,delta_cov_,jacobian_delta_calib_); //Affects dp_out, dv_out and dq_out
+ delta0 = delta_; //save the delta that is not affected by noise
+ deltaPlusDelta(_Delta0, delta0, _dt, delta_preint_plus_delta, jacobian_delta_preint, jacobian_delta);
+ jacobian_delta_preint0 = jacobian_delta_preint;
+ jacobian_delta0 = jacobian_delta;
+
+ //We compute all the jacobians wrt deltas and noisy deltas
+ for(int i=0; i<3; i++) //for 3 first and 3 last components we just add to add noise as vector component since it is in the R^3 space
+ {
+ //PQV formulation
+ //Add perturbation in position
+ delta_ = delta0;
+ delta_(i) = delta_(i) + _delta_noise(i); //noise has been added
+ delta_noisy_vect(i) = delta_;
+
+ //Add perturbation in velocity
+ /*
+ delta_ is size 10 (P Q V), _delta_noise is size 9 (P O V)
+ */
+ delta_ = delta0;
+ delta_(i+7) = delta_(i+7) + _delta_noise(i+6); //noise has been added
+ delta_noisy_vect(i+6) = delta_;
+ }
+
+ for(int i=0; i<3; i++) //for noise dtheta, it is in SO3, need to work on quaternions
+ {
+ //PQV formulation
+ //fist we need to reset some stuff
+ Eigen::Vector3d dtheta = Eigen::Vector3d::Zero();
+
+ delta_ = delta0;
+ new (&Dq_out_) Map<Quaterniond>(delta_.data() + 3); //not sure that we need this
+ dtheta(i) += _delta_noise(i+3); //introduce perturbation
+ Dq_out_ = Dq_out_ * v2q(dtheta);
+ delta_noisy_vect(i+3) = delta_;
+ }
+
+ //We compute all the jacobians wrt Deltas and noisy Deltas
+ for(int i=0; i<3; i++) //for 3 first and 3 last components we just add to add noise as vector component since it is in the R^3 space
+ {
+ //PQV formulation
+ //Add perturbation in position
+ Delta_ = _Delta0;
+ Delta_(i) = Delta_(i) + _Delta_noise(i); //noise has been added
+ Delta_noisy_vect(i) = Delta_;
+
+ //Add perturbation in velocity
+ /*
+ Delta_ is size 10 (P Q V), _Delta_noise is size 9 (P O V)
+ */
+ Delta_ = _Delta0;
+ Delta_(i+7) = Delta_(i+7) + _Delta_noise(i+6); //noise has been added
+ Delta_noisy_vect(i+6) = Delta_;
+ }
+
+ for(int i=0; i<3; i++) //for noise dtheta, it is in SO3, need to work on quaternions
+ {
+ //fist we need to reset some stuff
+ Eigen::Vector3d dtheta = Eigen::Vector3d::Zero();
+
+ Delta_ = _Delta0;
+ new (&Dq_out_) Map<Quaterniond>(Delta_.data() + 3);
+ dtheta(i) += _Delta_noise(i+3); //introduce perturbation
+ Dq_out_ = Dq_out_ * v2q(dtheta);
+ Delta_noisy_vect(i+3) = Delta_;
+ }
+
+ Imu_jac_deltas jac_deltas(_Delta0, delta0, Delta_noisy_vect, delta_noisy_vect, jacobian_delta_preint0, jacobian_delta0);
+ return jac_deltas;
+
+}
+
+} // namespace wolf
+
+#endif // PROCESSOR_Imu_UNITTESTER_H