diff --git a/CMakeLists.txt b/CMakeLists.txt
index 1c86e754c1565eeb626442ea535c4b000aa40069..27580e0d52c0c02c288e9f434b88ca6c1d5f148f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -76,8 +76,8 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON)
FIND_PACKAGE(wolfcore REQUIRED CONFIG)
FIND_PACKAGE(Eigen3 3.3 REQUIRED CONFIG)
-# ============ config.h ============
-set(_WOLF_ROOT_DIR ${CMAKE_SOURCE_DIR})
+# ============ CONFIG.H ============
+set(_WOLF_CODE_DIR ${CMAKE_SOURCE_DIR})
# variable used to compile the config.h.in file
string(TOUPPER ${PROJECT_NAME} PROJECT_NAME_UPPER)
@@ -102,6 +102,9 @@ SET(HDRS_CAPTURE
include/${PROJECT_NAME}/capture/capture_compass.h
include/${PROJECT_NAME}/capture/capture_imu.h
)
+SET(HDRS_COMMON
+ include/${PROJECT_NAME}/common/imu.h
+ )
SET(HDRS_FACTOR
include/${PROJECT_NAME}/factor/factor_compass_3d.h
include/${PROJECT_NAME}/factor/factor_imu.h
diff --git a/demos/demo_imuDock.cpp b/demos/demo_imuDock.cpp
index fb43f89e9c4ff04c28655f189ddf8652811541a5..17939f7895e87c28c43e0495eb6785fb2fe28291 100644
--- a/demos/demo_imuDock.cpp
+++ b/demos/demo_imuDock.cpp
@@ -120,7 +120,7 @@ int main(int argc, char** argv)
Eigen::Vector7d expected_KF1_pose((Eigen::Vector7d()<<0,0,0,0,0,0,1).finished()), expected_KF2_pose((Eigen::Vector7d()<<0,-0.06,0,0,0,0,11).finished());
//#################################################### SETTING PROBLEM
- std::string wolf_root = _WOLF_ROOT_DIR;
+ std::string wolf_root = _WOLF_CODE_DIR;
// ___Create the WOLF Problem + define origin of the problem___
ProblemPtr problem = Problem::create("PQVBB 3D");
diff --git a/demos/demo_imuDock_autoKFs.cpp b/demos/demo_imuDock_autoKFs.cpp
index 1a19f9a69ccbff392d24da878ba192ac8208a396..3c38803e1ab593969173c3c29015d1c0b173b993 100644
--- a/demos/demo_imuDock_autoKFs.cpp
+++ b/demos/demo_imuDock_autoKFs.cpp
@@ -129,7 +129,7 @@ int main(int argc, char** argv)
std::array<double, 50> lastms_imuData;
#endif
//#################################################### SETTING PROBLEM
- std::string wolf_root = _WOLF_ROOT_DIR;
+ std::string wolf_root = _WOLF_CODE_DIR;
// ___Create the WOLF Problem + define origin of the problem___
ProblemPtr problem = Problem::create("PQVBB 3D");
diff --git a/demos/demo_imuPlateform_Offline.cpp b/demos/demo_imuPlateform_Offline.cpp
index 15824f14945898469940106039aeff70407dda28..8f953ad35b9713978d9bfb0bdb5723dd9e79bbec 100644
--- a/demos/demo_imuPlateform_Offline.cpp
+++ b/demos/demo_imuPlateform_Offline.cpp
@@ -86,7 +86,7 @@ int main(int argc, char** argv)
#endif
//===================================================== SETTING PROBLEM
- std::string wolf_root = _WOLF_ROOT_DIR;
+ std::string wolf_root = _WOLF_CODE_DIR;
// WOLF PROBLEM
ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
diff --git a/demos/demo_imu_constrained0.cpp b/demos/demo_imu_constrained0.cpp
index 973e8bbb021d1b40a5b10bd5dc175348fe54d612..eecd4f850ad55e3e723a458997b406515319bb38 100644
--- a/demos/demo_imu_constrained0.cpp
+++ b/demos/demo_imu_constrained0.cpp
@@ -101,7 +101,7 @@ int main(int argc, char** argv)
#endif
//===================================================== SETTING PROBLEM
- std::string wolf_root = _WOLF_ROOT_DIR;
+ std::string wolf_root = _WOLF_CODE_DIR;
// WOLF PROBLEM
ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
diff --git a/include/imu/capture/capture_imu.h b/include/imu/capture/capture_imu.h
index 799b596ae493221da3489cc8fe44a056d6eecf7a..006b45af28cd164fdd61441fc1d5871204326da9 100644
--- a/include/imu/capture/capture_imu.h
+++ b/include/imu/capture/capture_imu.h
@@ -19,8 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#ifndef CAPTURE_IMU_H
-#define CAPTURE_IMU_H
+#pragma once
//Wolf includes
#include "imu/math/imu_tools.h"
@@ -50,6 +49,4 @@ class CaptureImu : public CaptureMotion
~CaptureImu() override;
};
-} // namespace wolf
-
-#endif // CAPTURE_Imu_H
+} // namespace wolf
\ No newline at end of file
diff --git a/include/imu/common/imu.h b/include/imu/common/imu.h
new file mode 100644
index 0000000000000000000000000000000000000000..25ea836ea74255fded84d1caef2335bbecfc4e15
--- /dev/null
+++ b/include/imu/common/imu.h
@@ -0,0 +1,34 @@
+//--------LICENSE_START--------
+//
+// Copyright (C) 2020,2021,2022,2023 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
+// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
+// All rights reserved.
+//
+// This file is part of WOLF
+// WOLF is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program. If not, see <http://www.gnu.org/licenses/>.
+//
+//--------LICENSE_END--------
+#pragma once
+
+// Enable project-specific definitions and macros
+#include "imu/internal/config.h"
+#include <core/common/wolf.h>
+
+namespace wolf
+{
+
+// Folder schema Registry
+WOLF_REGISTER_FOLDER(_WOLF_IMU_SCHEMA_DIR);
+
+}
diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index 53cd537933b0f045bdecc2fadb754f51a40cd4c9..36ab4e97ddcfde043c879b3485e936031bdda426 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu.h
@@ -1,8 +1,8 @@
//--------LICENSE_START--------
//
-// Copyright (C) 2020,2021,2022,2023 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
+// Copyright (C) 2020,2021,2022,2023 Institut de Robòtica i Informà tica
+// Industrial, CSIC-UPC. Authors: Joan Solà Ortega (jsola@iri.upc.edu) All
+// rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
@@ -19,8 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#ifndef FACTOR_IMU_THETA_H_
-#define FACTOR_IMU_THETA_H_
+#pragma once
// Wolf includes
#include "imu/feature/feature_imu.h"
@@ -32,47 +31,45 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorImu);
// class
class FactorImu : public FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>
{
public:
- FactorImu(const FeatureImuPtr& _ftr_ptr,
- const CaptureImuPtr& _capture_origin_ptr,
- const ProcessorBasePtr& _processor_ptr,
+ FactorImu(const FeatureImuPtr & _ftr_ptr,
+ const CaptureImuPtr & _capture_origin_ptr,
+ const ProcessorBasePtr &_processor_ptr,
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
~FactorImu() override = default;
- /** \brief : compute the residual from the state blocks being iterated by the solver.
+ /** \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;)
+ -> 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,
- T* _res) const;
-
- /** \brief Estimation error given the states in the wolf tree
- *
- */
- Eigen::Vector9d error();
-
- /** \brief : compute the residual from the state blocks being iterated by the solver. (same as operator())
+ 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,
+ T * _res) const;
+
+ /** \brief : compute the residual from the state blocks being iterated by the
+ solver. (same as operator())
-> 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;)
+ -> weights the result with the covariance of the noise (residual =
+ sqrt_info_matrix * err;)
* params :
* Vector3d _p1 : position in imu frame
* Vector4d _q1 : orientation quaternion in imu frame
@@ -82,18 +79,19 @@ class FactorImu : public FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>
* Vector3d _p2 : position in current frame
* Vector4d _q2 : orientation quaternion in current frame
* Vector3d _v2 : velocity in current frame
- * Matrix<9,1, double> _res : to retrieve residuals (POV) O is rotation vector... NOT A QUATERNION
+ * Matrix<9,1, double> _res : to retrieve residuals (POV) O is rotation
+ vector... NOT A QUATERNION
*/
template <typename D1, typename D2, typename D3>
- bool residual(const Eigen::MatrixBase<D1>& _p1,
- const Eigen::QuaternionBase<D2>& _q1,
- const Eigen::MatrixBase<D1>& _v1,
- const Eigen::MatrixBase<D1>& _ab1,
- const Eigen::MatrixBase<D1>& _wb1,
- const Eigen::MatrixBase<D1>& _p2,
- const Eigen::QuaternionBase<D2>& _q2,
- const Eigen::MatrixBase<D1>& _v2,
- Eigen::MatrixBase<D3>& _res) const;
+ bool residual(const Eigen::MatrixBase<D1> & _p1,
+ const Eigen::QuaternionBase<D2> &_q1,
+ const Eigen::MatrixBase<D1> & _v1,
+ const Eigen::MatrixBase<D1> & _ab1,
+ const Eigen::MatrixBase<D1> & _wb1,
+ const Eigen::MatrixBase<D1> & _p2,
+ const Eigen::QuaternionBase<D2> &_q2,
+ const Eigen::MatrixBase<D1> & _v2,
+ Eigen::MatrixBase<D3> & _res) const;
/** Function expectation(...)
* params :
@@ -109,16 +107,16 @@ class FactorImu : public FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>
* _ve : expected velocity delta
*/
template <typename D1, typename D2, typename D3, typename D4>
- void expectation(const Eigen::MatrixBase<D1>& _p1,
- const Eigen::QuaternionBase<D2>& _q1,
- const Eigen::MatrixBase<D1>& _v1,
- const Eigen::MatrixBase<D1>& _p2,
- const Eigen::QuaternionBase<D2>& _q2,
- const Eigen::MatrixBase<D1>& _v2,
+ void expectation(const Eigen::MatrixBase<D1> & _p1,
+ const Eigen::QuaternionBase<D2> &_q1,
+ const Eigen::MatrixBase<D1> & _v1,
+ const Eigen::MatrixBase<D1> & _p2,
+ const Eigen::QuaternionBase<D2> &_q2,
+ const Eigen::MatrixBase<D1> & _v2,
typename D1::Scalar _dt,
- Eigen::MatrixBase<D3>& _pe,
- Eigen::QuaternionBase<D4>& _qe,
- Eigen::MatrixBase<D3>& _ve) const;
+ Eigen::MatrixBase<D3> & _pe,
+ Eigen::QuaternionBase<D4> & _qe,
+ Eigen::MatrixBase<D3> & _ve) const;
/** \brief : return the expected delta given the state blocks in the wolf tree
*/
@@ -150,9 +148,9 @@ class FactorImu : public FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>
///////////////////// IMPLEMENTAITON ////////////////////////////
-inline FactorImu::FactorImu(const FeatureImuPtr& _ftr_ptr,
- const CaptureImuPtr& _cap_origin_ptr,
- const ProcessorBasePtr& _processor_ptr,
+inline FactorImu::FactorImu(const FeatureImuPtr & _ftr_ptr,
+ const CaptureImuPtr & _cap_origin_ptr,
+ const ProcessorBasePtr &_processor_ptr,
bool _apply_loss_function,
FactorStatus _status)
: FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>( // ...
@@ -191,105 +189,86 @@ inline FactorImu::FactorImu(const FeatureImuPtr& _ftr_ptr,
}
template <typename T>
-inline bool FactorImu::operator()(const T* const _p1,
- const T* const _q1,
- const T* const _v1,
- const T* const _b1,
- const T* const _p2,
- const T* const _q2,
- const T* const _v2,
- T* _res) const
+inline bool FactorImu::operator()(const T *const _p1,
+ const T *const _q1,
+ const T *const _v1,
+ const T *const _b1,
+ const T *const _p2,
+ const T *const _q2,
+ const T *const _v2,
+ T * _res) const
{
using namespace Eigen;
// MAPS
- Map<const Matrix<T, 3, 1> > p1(_p1);
- Map<const Quaternion<T> > q1(_q1);
- Map<const Matrix<T, 3, 1> > v1(_v1);
- Map<const Matrix<T, 3, 1> > ab1(_b1);
- Map<const Matrix<T, 3, 1> > wb1(_b1 + 3);
+ Map<const Matrix<T, 3, 1>> p1(_p1);
+ Map<const Quaternion<T>> q1(_q1);
+ Map<const Matrix<T, 3, 1>> v1(_v1);
+ Map<const Matrix<T, 3, 1>> ab1(_b1);
+ Map<const Matrix<T, 3, 1>> wb1(_b1 + 3);
- Map<const Matrix<T, 3, 1> > p2(_p2);
- Map<const Quaternion<T> > q2(_q2);
- Map<const Matrix<T, 3, 1> > v2(_v2);
+ Map<const Matrix<T, 3, 1>> p2(_p2);
+ Map<const Quaternion<T>> q2(_q2);
+ Map<const Matrix<T, 3, 1>> v2(_v2);
- Map<Matrix<T, 9, 1> > res(_res);
+ Map<Matrix<T, 9, 1>> res(_res);
residual(p1, q1, v1, ab1, wb1, p2, q2, v2, res);
return true;
}
-Eigen::Vector9d FactorImu::error()
-{
- Vector6d bias = getCaptureOther()->getSensorIntrinsic()->getState();
-
- Map<const Vector3d> acc_bias(bias.data());
- Map<const Vector3d> gyro_bias(bias.data() + 3);
-
- Eigen::Vector10d delta_exp = expectation();
-
- Eigen::Vector10d delta_preint = getMeasurement();
-
- Eigen::Vector9d delta_step;
-
- delta_step.head<3>() = dDp_dab_ * (acc_bias - acc_bias_preint_) + dDp_dwb_ * (gyro_bias - gyro_bias_preint_);
- delta_step.segment<3>(3) = dDq_dwb_ * (gyro_bias - gyro_bias_preint_);
- delta_step.tail<3>() = dDv_dab_ * (acc_bias - acc_bias_preint_) + dDv_dwb_ * (gyro_bias - gyro_bias_preint_);
-
- Eigen::VectorXd delta_corr = imu::plus(delta_preint, delta_step);
-
- Eigen::Vector9d err = imu::diff(delta_exp, delta_corr);
-
- return err;
-}
-
template <typename D1, typename D2, typename D3>
-inline bool FactorImu::residual(const Eigen::MatrixBase<D1>& _p1,
- const Eigen::QuaternionBase<D2>& _q1,
- const Eigen::MatrixBase<D1>& _v1,
- const Eigen::MatrixBase<D1>& _ab1,
- const Eigen::MatrixBase<D1>& _wb1,
- const Eigen::MatrixBase<D1>& _p2,
- const Eigen::QuaternionBase<D2>& _q2,
- const Eigen::MatrixBase<D1>& _v2,
- Eigen::MatrixBase<D3>& _res) const
+inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
+ const Eigen::QuaternionBase<D2> &_q1,
+ const Eigen::MatrixBase<D1> & _v1,
+ const Eigen::MatrixBase<D1> & _ab1,
+ const Eigen::MatrixBase<D1> & _wb1,
+ const Eigen::MatrixBase<D1> & _p2,
+ const Eigen::QuaternionBase<D2> &_q2,
+ const Eigen::MatrixBase<D1> & _v2,
+ Eigen::MatrixBase<D3> & _res) const
{
/* Help for the Imu residual function
*
* Notations:
- * D_* : a motion in the Delta manifold -- implemented as 10-vectors with [Dp, Dq, Dv]
- * T_* : a motion difference in the Tangent space to the manifold -- implemented as 9-vectors with [Dp, Do, Dv]
- * b* : a bias
- * J* : a Jacobian matrix
+ * D_* : a motion in the Delta manifold --
+ * implemented as 10-vectors with [Dp, Dq, Dv] T_* : a motion difference in
+ * the Tangent space to the manifold -- implemented as 9-vectors with [Dp,
+ * Do, Dv] b* : a bias J* : a Jacobian matrix
*
* We use the following functions from imu_tools.h:
- * D = betweenStates(x1,x2,dt) : obtain a Delta from two states separated dt=t2-t1
- * D2 = plus(D1,T) : plus operator, D2 = D1 (+) T
- * T = diff(D1,D2) : minus operator, T = D2 (-) D1
+ * D = betweenStates(x1,x2,dt) : obtain a Delta from two states separated
+ * dt=t2-t1 D2 = plus(D1,T) : blocl-plus operator, D2 = D1 <+> T
+ * T = diff(D1,D2) : block-minus operator, T = D2 <-> D1
*
* Two methods are possible (select with #define below this note) :
*
* Computations common to methods 1 and 2:
* D_exp = betweenStates(x1,x2,dt) // Predict delta from states
- * T_step = J_preint * (b - b_preint) // compute the delta correction step due to bias change
+ * T_step = J_preint * (b - b_preint) // compute the delta correction step
+ * due to bias change
*
* Method 1:
- * D_corr = plus(D_preint, T_step) // correct the pre-integrated delta with correction step due to bias
- * change T_err = diff(D_exp, D_corr) // compare against expected delta res = W.sqrt * T_err
+ * D_corr = plus(D_preint, T_step) // correct the pre-integrated delta
+ * with correction step due to bias change T_err = diff(D_exp, D_corr) //
+ * compare against expected delta res = W.sqrt * T_err
*
* results in:
- * res = W.sqrt * ( diff ( D_exp, plus(D_preint, J_preint * (b - b_preint) ) ) )
+ * res = W.sqrt * ( diff ( D_exp, plus(D_preint, J_preint * (b -
+ * b_preint) ) ) )
*
* Method 2:
- * T_diff = diff(D_preint, D_exp) // compare pre-integrated against expected delta
- * T_err = T_diff - T_step // the difference should match the correction step due to bias change
- * res = W.sqrt * err
+ * T_diff = diff(D_preint, D_exp) // compare pre-integrated against
+ * expected delta T_err = T_diff - T_step // the difference should
+ * match the correction step due to bias change res = W.sqrt * err
*
* results in :
- * res = W.sqrt * ( ( diff ( D_preint , D_exp ) ) - J_preint * (b - b_preint) )
+ * res = W.sqrt * ( ( diff ( D_preint , D_exp ) ) - J_preint * (b -
+ * b_preint) )
*
- * NOTE: See optimization report at the end of this file for comparisons of both methods.
+ * NOTE: See optimization report at the end of this file for comparisons of
+ * both methods.
*/
#define METHOD_1 // if commented, then METHOD_2 will be applied
@@ -305,21 +284,21 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1>& _p1,
imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, dt_, dp_exp, dq_exp, dv_exp);
- // 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias * (bias_current - bias_preint)
+ // 2. Corrected integrated delta: delta_corr = delta_preint (+) J_bias *
+ // (bias_current - bias_preint)
- // 2.a. Compute the delta step in tangent space: step = J_bias * (bias - bias_preint)
- Eigen::Matrix<T, 9, 1> d_step;
+#ifdef METHOD_1 // method 1
+
+ // 2.a. Compute the delta step in tangent space: step = J_bias * (bias -
+ // bias_preint)
+ Eigen::Matrix<T, 3, 1> dp_step, do_step, dv_step;
d_step.block(0, 0, 3, 1) = dDp_dab_ * (_ab1 - acc_bias_preint_) + dDp_dwb_ * (_wb1 - gyro_bias_preint_);
- d_step.block(3, 0, 3, 1) = dDq_dwb_ * (_wb1 - gyro_bias_preint_);
+ d_step.block(3, 0, 3, 1) = /* it happens that dDq_dab = 0 ! */ dDq_dwb_ * (_wb1 - gyro_bias_preint_);
d_step.block(6, 0, 3, 1) = dDv_dab_ * (_ab1 - acc_bias_preint_) + dDv_dwb_ * (_wb1 - gyro_bias_preint_);
-#ifdef METHOD_1 // method 1
- Eigen::Matrix<T, 3, 1> dp_step = d_step.block(0, 0, 3, 1);
- Eigen::Matrix<T, 3, 1> do_step = d_step.block(3, 0, 3, 1);
- Eigen::Matrix<T, 3, 1> dv_step = d_step.block(6, 0, 3, 1);
-
- // 2.b. Add the correction step to the preintegrated delta: delta_cor = delta_preint (+) step
+ // 2.b. Add the correction step to the preintegrated delta: delta_cor =
+ // delta_preint (+) step
Eigen::Matrix<T, 3, 1> dp_correct;
Eigen::Quaternion<T> dq_correct;
Eigen::Matrix<T, 3, 1> dv_correct;
@@ -335,11 +314,12 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1>& _p1,
dv_correct);
// 3. Delta error in minimal form: D_err = diff(D_exp , D_corr)
- // Note the Dt here is zero because it's the delta-time between the same time stamps!
- Eigen::Matrix<T, 9, 1> d_error;
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_error(d_error.data());
- Eigen::Map<Eigen::Matrix<T, 3, 1> > do_error(d_error.data() + 3);
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dv_error(d_error.data() + 6);
+ // Note the Dt here is zero because it's the delta-time between the same time
+ // stamps!
+ Eigen::Matrix<T, 9, 1> d_error;
+ Eigen::Map<Eigen::Matrix<T, 3, 1>> dp_error(d_error.data());
+ Eigen::Map<Eigen::Matrix<T, 3, 1>> do_error(d_error.data() + 3);
+ Eigen::Map<Eigen::Matrix<T, 3, 1>> dv_error(d_error.data() + 6);
Eigen::Matrix<T, 3, 3> J_do_dq1, J_do_dq2;
Eigen::Matrix<T, 9, 9> J_err_corr;
@@ -361,31 +341,32 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1>& _p1,
#else // method 2
- Eigen::Matrix<T, 9, 1> d_diff;
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dp_diff(d_diff.data());
- Eigen::Map<Eigen::Matrix<T, 3, 1> > do_diff(d_diff.data() + 3);
- Eigen::Map<Eigen::Matrix<T, 3, 1> > dv_diff(d_diff.data() + 6);
+Eigen::Matrix<T, 9, 1> d_diff;
+Eigen::Map<Eigen::Matrix<T, 3, 1>> dp_diff(d_diff.data());
+Eigen::Map<Eigen::Matrix<T, 3, 1>> do_diff(d_diff.data() + 3);
+Eigen::Map<Eigen::Matrix<T, 3, 1>> dv_diff(d_diff.data() + 6);
- imu::diff(dp_preint_.cast<T>(),
- dq_preint_.cast<T>(),
- dv_preint_.cast<T>(),
- dp_exp,
- dq_exp,
- dv_exp,
- dp_diff,
- do_diff,
- dv_diff);
+imu::diff(dp_preint_.cast<T>(),
+ dq_preint_.cast<T>(),
+ dv_preint_.cast<T>(),
+ dp_exp,
+ dq_exp,
+ dv_exp,
+ dp_diff,
+ do_diff,
+ dv_diff);
- Eigen::Matrix<T, 9, 1> d_error;
- d_error << d_diff - d_step;
+Eigen::Matrix<T, 9, 1> d_error;
+d_error << d_diff - d_step;
- // 4. Residuals are the weighted errors
- _res = getMeasurementSquareRootInformationUpper() * d_error;
+// 4. Residuals are the weighted errors
+_res = getMeasurementSquareRootInformationUpper() * d_error;
#endif
//////////////////////////////////////////////////////////////////////////////////////////////
- ///////////////////////////////// PRINT VALUES ///////////////////////////////////
+ ///////////////////////////////// PRINT VALUES
+ //////////////////////////////////////
#if 0
if (sizeof(T) == sizeof(double))
{
@@ -463,10 +444,10 @@ inline Eigen::VectorXd FactorImu::expectation() const
const Eigen::Vector3d frame_previous_vel = (frm_previous->getV()->getState());
// Define results vector and Map bits over it
- Eigen::Matrix<double, 10, 1> exp;
- Eigen::Map<Eigen::Matrix<double, 3, 1> > pe(exp.data());
- Eigen::Map<Eigen::Quaternion<double> > qe(exp.data() + 3);
- Eigen::Map<Eigen::Matrix<double, 3, 1> > ve(exp.data() + 7);
+ Eigen::Matrix<double, 10, 1> exp;
+ Eigen::Map<Eigen::Matrix<double, 3, 1>> pe(exp.data());
+ Eigen::Map<Eigen::Quaternion<double>> qe(exp.data() + 3);
+ Eigen::Map<Eigen::Matrix<double, 3, 1>> ve(exp.data() + 7);
expectation(frame_previous_pos,
frame_previous_ori,
@@ -483,79 +464,91 @@ inline Eigen::VectorXd FactorImu::expectation() const
}
template <typename D1, typename D2, typename D3, typename D4>
-inline void FactorImu::expectation(const Eigen::MatrixBase<D1>& _p1,
- const Eigen::QuaternionBase<D2>& _q1,
- const Eigen::MatrixBase<D1>& _v1,
- const Eigen::MatrixBase<D1>& _p2,
- const Eigen::QuaternionBase<D2>& _q2,
- const Eigen::MatrixBase<D1>& _v2,
+inline void FactorImu::expectation(const Eigen::MatrixBase<D1> & _p1,
+ const Eigen::QuaternionBase<D2> &_q1,
+ const Eigen::MatrixBase<D1> & _v1,
+ const Eigen::MatrixBase<D1> & _p2,
+ const Eigen::QuaternionBase<D2> &_q2,
+ const Eigen::MatrixBase<D1> & _v2,
typename D1::Scalar _dt,
- Eigen::MatrixBase<D3>& _pe,
- Eigen::QuaternionBase<D4>& _qe,
- Eigen::MatrixBase<D3>& _ve) const
+ Eigen::MatrixBase<D3> & _pe,
+ Eigen::QuaternionBase<D4> & _qe,
+ Eigen::MatrixBase<D3> & _ve) const
{
imu::betweenStates(_p1, _q1, _v1, _p2, _q2, _v2, _dt, _pe, _qe, _ve);
}
} // namespace wolf
-#endif
-
/*
* Optimization results
* ================================================
*
* Using gtest_Imu.cpp
*
- * Conclusion: Residuals with method 1 and 2 are essentially identical, after exactly the same number of iterations.
+ * Conclusion: Residuals with method 1 and 2 are essentially identical, after
+exactly the same number of iterations.
*
- * You can verify this by looking at the 'Iterations' and 'Final costs' in the Ceres reports below.
+ * You can verify this by looking at the 'Iterations' and 'Final costs' in the
+Ceres reports below.
*
* With Method 1:
*
[ RUN ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
-[trace][10:58:16] [gtest_Imu.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final
-cost: 1.251480e-06, Termination: CONVERGENCE [trace][10:58:16] [gtest_Imu.cpp L490 : TestBody] w * DT (rather be lower
-than 1.57 approx) = 0.5 1 1.5 [ OK ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (53 ms) [ RUN ]
-Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 [trace][10:58:16] [gtest_Imu.cpp L564 : TestBody] Ceres Solver
-Report: Iterations: 16, Initial cost: 1.238969e+03, Final cost: 1.059256e-19, Termination: CONVERGENCE [ OK ]
+[trace][10:58:16] [gtest_Imu.cpp L488 : TestBody] Ceres Solver Report:
+Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251480e-06,
+Termination: CONVERGENCE [trace][10:58:16] [gtest_Imu.cpp L490 : TestBody] w *
+DT (rather be lower than 1.57 approx) = 0.5 1 1.5 [ OK ]
+Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (53 ms) [ RUN ]
+Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 [trace][10:58:16]
+[gtest_Imu.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial
+cost: 1.238969e+03, Final cost: 1.059256e-19, Termination: CONVERGENCE [ OK ]
Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (56 ms) [ RUN ]
-Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 [trace][10:58:16] [gtest_Imu.cpp L638 : TestBody] Ceres Solver
-Report: Iterations: 17, Initial cost: 4.769588e+03, Final cost: 3.767740e-19, Termination: CONVERGENCE [ OK ]
+Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 [trace][10:58:16]
+[gtest_Imu.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial
+cost: 4.769588e+03, Final cost: 3.767740e-19, Termination: CONVERGENCE [ OK ]
Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (50 ms)
[----------] 3 tests from Process_Factor_Imu (159 ms total)
[----------] 2 tests from Process_Factor_Imu_ODO
[ RUN ] Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
-[trace][10:58:16] [gtest_Imu.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03,
-Final cost: 1.867678e-22, Termination: CONVERGENCE [ OK ]
+[trace][10:58:16] [gtest_Imu.cpp L711 : TestBody] Ceres Solver Report:
+Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.867678e-22,
+Termination: CONVERGENCE [ OK ]
Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms) [ RUN ]
-Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 [trace][10:58:16] [gtest_Imu.cpp L783 : TestBody] Ceres
-Solver Report: Iterations: 16, Initial cost: 1.363681e+04, Final cost: 1.879880e-20, Termination: CONVERGENCE [ OK ]
+Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 [trace][10:58:16]
+[gtest_Imu.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial
+cost: 1.363681e+04, Final cost: 1.879880e-20, Termination: CONVERGENCE [ OK ]
Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (52 ms)
[----------] 2 tests from Process_Factor_Imu_ODO (120 ms total)
*
* With Method 2:
*
[ RUN ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
-[trace][11:15:43] [gtest_Imu.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final
-cost: 1.251479e-06, Termination: CONVERGENCE [trace][11:15:43] [gtest_Imu.cpp L490 : TestBody] w * DT (rather be lower
-than 1.57 approx) = 0.5 1 1.5 [ OK ] Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (37 ms) [ RUN ]
-Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 [trace][11:15:43] [gtest_Imu.cpp L564 : TestBody] Ceres Solver
-Report: Iterations: 16, Initial cost: 1.238985e+03, Final cost: 1.058935e-19, Termination: CONVERGENCE [ OK ]
+[trace][11:15:43] [gtest_Imu.cpp L488 : TestBody] Ceres Solver Report:
+Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251479e-06,
+Termination: CONVERGENCE [trace][11:15:43] [gtest_Imu.cpp L490 : TestBody] w *
+DT (rather be lower than 1.57 approx) = 0.5 1 1.5 [ OK ]
+Process_Factor_Imu.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (37 ms) [ RUN ]
+Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 [trace][11:15:43]
+[gtest_Imu.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial
+cost: 1.238985e+03, Final cost: 1.058935e-19, Termination: CONVERGENCE [ OK ]
Process_Factor_Imu.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (48 ms) [ RUN ]
-Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 [trace][11:15:43] [gtest_Imu.cpp L638 : TestBody] Ceres Solver
-Report: Iterations: 17, Initial cost: 4.769603e+03, Final cost: 3.762091e-19, Termination: CONVERGENCE [ OK ]
+Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 [trace][11:15:43]
+[gtest_Imu.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial
+cost: 4.769603e+03, Final cost: 3.762091e-19, Termination: CONVERGENCE [ OK ]
Process_Factor_Imu.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (47 ms)
[----------] 3 tests from Process_Factor_Imu (133 ms total)
[----------] 2 tests from Process_Factor_Imu_ODO
[ RUN ] Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
-[trace][11:15:43] [gtest_Imu.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03,
-Final cost: 1.855814e-22, Termination: CONVERGENCE [ OK ]
+[trace][11:15:43] [gtest_Imu.cpp L711 : TestBody] Ceres Solver Report:
+Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.855814e-22,
+Termination: CONVERGENCE [ OK ]
Process_Factor_Imu_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms) [ RUN ]
-Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 [trace][11:15:43] [gtest_Imu.cpp L783 : TestBody] Ceres
-Solver Report: Iterations: 16, Initial cost: 1.363675e+04, Final cost: 1.880084e-20, Termination: CONVERGENCE [ OK ]
+Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 [trace][11:15:43]
+[gtest_Imu.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial
+cost: 1.363675e+04, Final cost: 1.880084e-20, Termination: CONVERGENCE [ OK ]
Process_Factor_Imu_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (59 ms)
[----------] 2 tests from Process_Factor_Imu_ODO (127 ms total)
*
diff --git a/include/imu/math/imu2d_tools.h b/include/imu/math/imu2d_tools.h
index 379c94c10411a4c1fce1a972e1e33ab4952a4fdd..bbb860ef0e8a52b105ad8175e5bebd8b08d572fa 100644
--- a/include/imu/math/imu2d_tools.h
+++ b/include/imu/math/imu2d_tools.h
@@ -177,7 +177,6 @@ inline void compose(const MatrixBase<D1>& d1,
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);
diff --git a/include/imu/processor/processor_compass.h b/include/imu/processor/processor_compass.h
index 7f530b94399aec820243d79b712af4007f35b43d..f8d1d40caf021f741be7370630a56591627b3966 100644
--- a/include/imu/processor/processor_compass.h
+++ b/include/imu/processor/processor_compass.h
@@ -19,9 +19,9 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
-#ifndef INCLUDE_IMU_PROCESSOR_PROCESSORCOMPASS_H_
-#define INCLUDE_IMU_PROCESSOR_PROCESSORCOMPASS_H_
+#pragma once
+#include "imu/common/imu.h"
#include <core/processor/processor_base.h>
namespace wolf {
@@ -71,6 +71,4 @@ class ProcessorCompass : public ProcessorBase
bool voteForKeyFrame() const override { return false;};
};
-} /* namespace wolf */
-
-#endif /* INCLUDE_IMU_PROCESSOR_PROCESSORCOMPASS_H_ */
+} /* namespace wolf */
\ No newline at end of file
diff --git a/include/imu/sensor/sensor_compass.h b/include/imu/sensor/sensor_compass.h
index 9dfad0992cb68def6acd36b8d10e45ab90e7a850..9736cae31ce38b37258ffccffd6b907424ca3ac7 100644
--- a/include/imu/sensor/sensor_compass.h
+++ b/include/imu/sensor/sensor_compass.h
@@ -22,13 +22,13 @@
#ifndef SENSOR_SENSOR_COMPASS_H_
#define SENSOR_SENSOR_COMPASS_H_
-//wolf includes
+// wolf includes
#include <core/sensor/sensor_base.h>
#include <iostream>
-namespace wolf {
-
+namespace wolf
+{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorCompass);
struct ParamsSensorCompass : public ParamsSensorBase
@@ -36,16 +36,14 @@ struct ParamsSensorCompass : public ParamsSensorBase
double stdev_noise;
ParamsSensorCompass() = default;
- ParamsSensorCompass(const YAML::Node& _input_node)
- : ParamsSensorBase(_input_node)
+ ParamsSensorCompass(const YAML::Node& _input_node) : ParamsSensorBase(_input_node)
{
stdev_noise = _input_node["stdev_noise"].as<double>();
}
~ParamsSensorCompass() override = default;
std::string print() const override
{
- return ParamsSensorBase::print() + "\n"
- + "stdev_noise: " + toString(stdev_noise) + "\n";
+ return ParamsSensorBase::print() + "\n" + "stdev_noise: " + toString(stdev_noise) + "\n";
}
};
@@ -53,19 +51,18 @@ WOLF_PTR_TYPEDEFS(SensorCompass);
class SensorCompass : public SensorBase
{
- protected:
- ParamsSensorCompassPtr params_compass_;
+ protected:
+ ParamsSensorCompassPtr params_compass_;
- public:
- SensorCompass(ParamsSensorCompassPtr _params,
- const SpecStateSensorComposite& _priors);
+ public:
+ SensorCompass(ParamsSensorCompassPtr _params, const SpecStateSensorComposite& _priors);
- WOLF_SENSOR_CREATE(SensorCompass, ParamsSensorCompass);
+ WOLF_SENSOR_CREATE(SensorCompass, ParamsSensorCompass);
- ~SensorCompass() override;
- ParamsSensorCompassConstPtr getParams() const;
+ ~SensorCompass() override;
+ ParamsSensorCompassConstPtr getParams() const;
- Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
+ Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
};
inline ParamsSensorCompassConstPtr SensorCompass::getParams() const
diff --git a/internal/config.h.in b/internal/config.h.in
index 35f3468331656ba09a63ad7f26e5c0fb78c8d5bc..727b1bc74fa04ab302137551cab9eab93a51fa8d 100644
--- a/internal/config.h.in
+++ b/internal/config.h.in
@@ -24,13 +24,12 @@
// which will be added to the include path for compilation,
// and installed with the public wolf headers.
-#ifndef WOLF_INTERNAL_${PROJECT_NAME_UPPER}_CONFIG_H_
-#define WOLF_INTERNAL_${PROJECT_NAME_UPPER}_CONFIG_H_
+#pragma once
#cmakedefine _WOLF_DEBUG
#cmakedefine _WOLF_TRACE
-#define _WOLF_${PROJECT_NAME_UPPER}_ROOT_DIR "${_WOLF_ROOT_DIR}"
+#define _WOLF_${PROJECT_NAME_UPPER}_CODE_DIR "${_WOLF_CODE_DIR}"
-#endif /* WOLF_INTERNAL_CONFIG_H_ */
+#define _WOLF_${PROJECT_NAME_UPPER}_SCHEMA_DIR "${CMAKE_INSTALL_PREFIX}/${INCLUDE_INSTALL_DIR}/${PROJECT_NAME}/schema"
\ No newline at end of file
diff --git a/test/gtest_factor_compass_3d.cpp b/test/gtest_factor_compass_3d.cpp
index 053b1d5dcf64c7e08d8998b4c9357b80dc1ec9bd..9ee933008c80af9395185a691800b203b133ed2d 100644
--- a/test/gtest_factor_compass_3d.cpp
+++ b/test/gtest_factor_compass_3d.cpp
@@ -33,7 +33,7 @@
using namespace Eigen;
using namespace wolf;
-std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+std::string wolf_root = _WOLF_IMU_CODE_DIR;
int N_TESTS = 10;
diff --git a/test/gtest_factor_imu.cpp b/test/gtest_factor_imu.cpp
index 7f7ceb188966a309b3bcb393e1562109f06afefa..04740f89e326a4bc707e2dfc5b3ad656228b8a57 100644
--- a/test/gtest_factor_imu.cpp
+++ b/test/gtest_factor_imu.cpp
@@ -43,7 +43,7 @@ using namespace wolf;
using std::make_shared;
using std::static_pointer_cast;
-std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+std::string wolf_root = _WOLF_IMU_CODE_DIR;
/*
* This test is designed to test Imu biases in a particular case : perfect Imu, not moving.
diff --git a/test/gtest_factor_imu2d.cpp b/test/gtest_factor_imu2d.cpp
index 43051e45ea48a45836566f03489235d9181566ef..48289bda0027b4d8d9d2936f26c5078c83cca227 100644
--- a/test/gtest_factor_imu2d.cpp
+++ b/test/gtest_factor_imu2d.cpp
@@ -30,56 +30,53 @@
using namespace Eigen;
using namespace wolf;
-string wolf_root = _WOLF_IMU_ROOT_DIR;
-int N_TESTS = 10;
+string wolf_root = _WOLF_IMU_CODE_DIR;
+int N_TESTS = 10;
class FactorImu2d_test : public testing::Test
{
- public:
- Matrix6d data_cov;
- Matrix5d delta_cov;
- MatrixXd jacobian_bias = MatrixXd(5,3);
- Vector3d b0_preint;
- ProblemPtr problem_ptr;
- SolverCeresPtr solver;
- FrameBasePtr frm0, frm1;
- SensorBasePtr sensor;
- CaptureImuPtr cap0, cap1;
+ public:
+ Matrix6d data_cov;
+ Matrix5d delta_cov;
+ MatrixXd jacobian_bias = MatrixXd(5, 3);
+ Vector3d b0_preint;
+ ProblemPtr problem_ptr;
+ SolverCeresPtr solver;
+ FrameBasePtr frm0, frm1;
+ SensorBasePtr sensor;
+ CaptureImuPtr cap0, cap1;
virtual void SetUp()
{
- // Input odometry data and covariance
- data_cov = 0.1*Matrix6d::Identity();
- delta_cov = 0.1*Matrix5d::Identity();
-
- // Jacobian of the bias
- jacobian_bias << 1, 0, 0,
- 0, 1, 0,
- 0, 0, 1,
- 1, 0, 0,
- 0, 1, 0;
- //preintegration bias
- b0_preint = Vector3d::Zero();
-
- // Problem and solver
- problem_ptr = Problem::create("POV", 2);
- solver = std::make_shared<SolverCeres>(problem_ptr);
- solver->getSolverOptions().function_tolerance = 1e-8;
- solver->getSolverOptions().gradient_tolerance = 1e-8;
-
- // Two frames
- frm0 = problem_ptr->emplaceFrame(TimeStamp(0), "POV", Vector5d::Zero());
- frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "POV", Vector5d::Zero());
-
- // Imu2d sensor
- sensor = problem_ptr->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d.yaml", {wolf_root});
-
- //capture from frm1 to frm0
- cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
- cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
+ // Input odometry data and covariance
+ data_cov = 0.1 * Matrix6d::Identity();
+ delta_cov = 0.1 * Matrix5d::Identity();
+
+ // Jacobian of the bias
+ jacobian_bias << 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0;
+ // preintegration bias
+ b0_preint = Vector3d::Zero();
+
+ // Problem and solver
+ problem_ptr = Problem::create("POV", 2);
+ solver = std::make_shared<SolverCeres>(problem_ptr);
+ solver->getSolverOptions().function_tolerance = 1e-8;
+ solver->getSolverOptions().gradient_tolerance = 1e-8;
+
+ // Two frames
+ frm0 = problem_ptr->emplaceFrame(TimeStamp(0), "POV", Vector5d::Zero());
+ frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "POV", Vector5d::Zero());
+
+ // Imu2d sensor
+ sensor = problem_ptr->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d.yaml", {wolf_root});
+
+ // capture from frm1 to frm0
+ cap0 =
+ CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
+ cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
}
- virtual void TearDown(){}
+ virtual void TearDown() {}
};
TEST_F(FactorImu2d_test, check_tree)
@@ -89,399 +86,407 @@ TEST_F(FactorImu2d_test, check_tree)
TEST_F(FactorImu2d_test, bias_zero_solve_f1)
{
- for(int i = 0; i < N_TESTS; 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,"POV");
- frm1->setState(x1,"POV");
-
- // 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
- problem_ptr->print(4,1,1,1);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4,1,1,1);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
- ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
- //WOLF_INFO(frm1->getStateVector("POV"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < N_TESTS; 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, "POV");
+ frm1->setState(x1, "POV");
+
+ // 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
+ problem_ptr->print(4, 1, 1, 1);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem_ptr->print(4, 1, 1, 1);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
+ // WOLF_INFO(frm1->getStateVector("POV"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2d_test, bias_zero_solve_f0)
{
- for(int i = 0; i < N_TESTS; 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,"POV");
- frm1->setState(x1,"POV");
-
- // 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
- problem_ptr->print(4,1,1,1);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4,1,1,1);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
- ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
- //WOLF_INFO(frm1->getStateVector("POV"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < N_TESTS; 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, "POV");
+ frm1->setState(x1, "POV");
+
+ // 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
+ problem_ptr->print(4, 1, 1, 1);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem_ptr->print(4, 1, 1, 1);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
+ // WOLF_INFO(frm1->getStateVector("POV"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2d_test, bias_zero_solve_b1)
{
- for(int i = 0; i < N_TESTS; 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,"POV");
- frm1->setState(x1,"POV");
- 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
- problem_ptr->print(4,1,1,1);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4,1,1,1);
-
- EXPECT_POSE2d_APPROX(cap1->getStateVector("I"), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector("I"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < N_TESTS; 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, "POV");
+ frm1->setState(x1, "POV");
+ 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
+ problem_ptr->print(4, 1, 1, 1);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem_ptr->print(4, 1, 1, 1);
+
+ EXPECT_POSE2d_APPROX(cap1->getStateVector("I"), b0, 1e-6);
+ // WOLF_INFO(cap1->getStateVector("I"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2d_test, solve_b0)
{
- for(int i = 0; i < N_TESTS; 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,"POV");
- frm1->setState(x1,"POV");
- 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
- problem_ptr->print(4,1,1,1);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4,1,1,1);
-
- EXPECT_POSE2d_APPROX(cap0->getStateVector("I"), b0, 1e-6);
- //WOLF_INFO(cap0->getStateVector("I"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < N_TESTS; 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, "POV");
+ frm1->setState(x1, "POV");
+ 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
+ problem_ptr->print(4, 1, 1, 1);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem_ptr->print(4, 1, 1, 1);
+
+ EXPECT_POSE2d_APPROX(cap0->getStateVector("I"), b0, 1e-6);
+ // WOLF_INFO(cap0->getStateVector("I"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2d_test, solve_b1)
{
- for(int i = 0; i < N_TESTS; 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,"POV");
- frm1->setState(x1,"POV");
- 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
- problem_ptr->print(4,1,1,1);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4,1,1,1);
-
- EXPECT_POSE2d_APPROX(cap1->getStateVector("I"), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector("I"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < N_TESTS; 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, "POV");
+ frm1->setState(x1, "POV");
+ 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
+ problem_ptr->print(4, 1, 1, 1);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem_ptr->print(4, 1, 1, 1);
+
+ EXPECT_POSE2d_APPROX(cap1->getStateVector("I"), b0, 1e-6);
+ // WOLF_INFO(cap1->getStateVector("I"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2d_test, solve_f0)
{
- for(int i = 0; i < N_TESTS; 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,"POV");
- frm1->setState(x1,"POV");
- 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
- problem_ptr->print(4,1,1,1);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4,1,1,1);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
- ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
- //WOLF_INFO(frm0->getStateVector("POV"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < N_TESTS; 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, "POV");
+ frm1->setState(x1, "POV");
+ 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
+ problem_ptr->print(4, 1, 1, 1);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem_ptr->print(4, 1, 1, 1);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
+ // WOLF_INFO(frm0->getStateVector("POV"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2d_test, solve_f1)
{
- for(int i = 0; i < N_TESTS; 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,"POV");
- frm1->setState(x1,"POV");
- 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
- problem_ptr->print(4,1,1,1);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4,1,1,1);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
- ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
- //WOLF_INFO(frm0->getStateVector("POV"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < N_TESTS; 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, "POV");
+ frm1->setState(x1, "POV");
+ 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
+ problem_ptr->print(4, 1, 1, 1);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem_ptr->print(4, 1, 1, 1);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
+ // WOLF_INFO(frm0->getStateVector("POV"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2d_test, solve_f1_b1)
{
- for(int i = 0; i < N_TESTS; 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,"POV");
- frm1->setState(x1,"POV");
- 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
- problem_ptr->print(4,1,1,1);
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
- problem_ptr->print(4,1,1,1);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
- ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
- EXPECT_MATRIX_APPROX(cap1->getStateVector("I"), b0, 1e-6);
- //WOLF_INFO(frm0->getStateVector("POV"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < N_TESTS; 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, "POV");
+ frm1->setState(x1, "POV");
+ 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
+ problem_ptr->print(4, 1, 1, 1);
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+ problem_ptr->print(4, 1, 1, 1);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
+ EXPECT_MATRIX_APPROX(cap1->getStateVector("I"), b0, 1e-6);
+ // WOLF_INFO(frm0->getStateVector("POV"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_imu2d_with_gravity.cpp b/test/gtest_factor_imu2d_with_gravity.cpp
index 58fc90e55b014e776f784d35f0085389e8e1ed9c..4763816a2e067ac418dc790d714640d08cc44e16 100644
--- a/test/gtest_factor_imu2d_with_gravity.cpp
+++ b/test/gtest_factor_imu2d_with_gravity.cpp
@@ -1,8 +1,8 @@
//--------LICENSE_START--------
//
-// Copyright (C) 2020,2021,2022,2023 Institut de Robòtica i Informà tica Industrial, CSIC-UPC.
-// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
-// All rights reserved.
+// Copyright (C) 2020,2021,2022,2023 Institut de Robòtica i Informà tica
+// Industrial, CSIC-UPC. Authors: Joan Solà Ortega (jsola@iri.upc.edu) All
+// rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
@@ -22,64 +22,62 @@
#include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h>
-#include "imu/internal/config.h"
#include "imu/factor/factor_imu2d_with_gravity.h"
+#include "imu/internal/config.h"
#include "imu/math/imu2d_tools.h"
#include "imu/sensor/sensor_imu.h"
using namespace Eigen;
using namespace wolf;
-string wolf_root = _WOLF_IMU_ROOT_DIR;
-int N_TESTS = 10;
+string wolf_root = _WOLF_IMU_CODE_DIR;
+int N_TESTS = 10;
class FactorImu2dwithGravity_test : public testing::Test
{
- public:
- Matrix6d data_cov;
- Matrix5d delta_cov;
- MatrixXd jacobian_bias = MatrixXd(5,3);
- Vector3d b0_preint;
- ProblemPtr problem_ptr;
- SolverCeresPtr solver;
- FrameBasePtr frm0, frm1;
- SensorBasePtr sensor;
- CaptureImuPtr cap0, cap1;
+ public:
+ Matrix6d data_cov;
+ Matrix5d delta_cov;
+ MatrixXd jacobian_bias = MatrixXd(5, 3);
+ Vector3d b0_preint;
+ ProblemPtr problem_ptr;
+ SolverCeresPtr solver;
+ FrameBasePtr frm0, frm1;
+ SensorBasePtr sensor;
+ CaptureImuPtr cap0, cap1;
virtual void SetUp()
{
- // Input odometry data and covariance
- data_cov = 0.1*Matrix6d::Identity();
- delta_cov = 0.1*Matrix5d::Identity();
-
- // Jacobian of the bias
- jacobian_bias << 1, 0, 0,
- 0, 1, 0,
- 0, 0, 1,
- 1, 0, 0,
- 0, 1, 0;
- //preintegration bias
- b0_preint = Vector3d::Zero();
-
- // Problem and solver
- problem_ptr = Problem::create("POV", 2);
- solver = std::make_shared<SolverCeres>(problem_ptr);
- solver->getSolverOptions().function_tolerance = 1e-9;
- solver->getSolverOptions().gradient_tolerance = 1e-9;
-
- // Two frames
- frm0 = problem_ptr->emplaceFrame(TimeStamp(0), "POV", Vector5d::Zero());
- frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "POV", Vector5d::Zero());
-
- // Imu2d sensor
- sensor = problem_ptr->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d_with_gravity.yaml", {wolf_root});
-
- //capture from frm1 to frm0
- cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
- cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
+ // Input odometry data and covariance
+ data_cov = 0.1 * Matrix6d::Identity();
+ delta_cov = 0.1 * Matrix5d::Identity();
+
+ // Jacobian of the bias
+ jacobian_bias << 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0;
+ // preintegration bias
+ b0_preint = Vector3d::Zero();
+
+ // Problem and solver
+ problem_ptr = Problem::create("POV", 2);
+ solver = std::make_shared<SolverCeres>(problem_ptr);
+ solver->getSolverOptions().function_tolerance = 1e-9;
+ solver->getSolverOptions().gradient_tolerance = 1e-9;
+
+ // Two frames
+ frm0 = problem_ptr->emplaceFrame(TimeStamp(0), "POV", Vector5d::Zero());
+ frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "POV", Vector5d::Zero());
+
+ // Imu2d sensor
+ sensor = problem_ptr->installSensor(
+ "SensorImu", wolf_root + "/test/yaml/sensor_imu2d_with_gravity.yaml", {wolf_root});
+
+ // capture from frm1 to frm0
+ cap0 =
+ CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
+ cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
}
- virtual void TearDown(){}
+ virtual void TearDown() {}
};
TEST_F(FactorImu2dwithGravity_test, check_tree)
@@ -89,540 +87,550 @@ TEST_F(FactorImu2dwithGravity_test, check_tree)
TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_f1)
{
- for(int i = 0; i < N_TESTS; i++){
- //WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0,"POV");
- frm1->setState(x1,"POV");
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and biases, perturb frm1
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm1->perturb(0.01);
-
- // solve
- std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
- //std::cout << report << std::endl;
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
- ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
- //WOLF_INFO(frm1->getStateVector("POV"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < N_TESTS; i++)
+ {
+ // WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0, "POV");
+ frm1->setState(x1, "POV");
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and biases, perturb frm1
+ frm0->fix();
+ cap0->fix();
+ frm1->unfix();
+ cap1->fix();
+ sensor->getStateBlock('G')->fix();
+ frm1->perturb(0.01);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ // std::cout << report << std::endl;
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
+ // WOLF_INFO(frm1->getStateVector("POV"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_f0)
{
- for(int i = 0; i < N_TESTS; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0, "POV");
- frm1->setState(x1, "POV");
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1 and biases, perturb frm0
- frm0->unfix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm0->perturb(0.01);
-
- // solve
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm0->getStateVector("POV"), x0, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < N_TESTS; i++)
+ {
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0, "POV");
+ frm1->setState(x1, "POV");
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1 and biases, perturb frm0
+ frm0->unfix();
+ cap0->fix();
+ frm1->fix();
+ cap1->fix();
+ sensor->getStateBlock('G')->fix();
+ frm0->perturb(0.01);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector("POV"), x0, 1e-6);
+ // WOLF_INFO(frm1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2dWithGravity, bias_zero_solve_b1)
{
- for(int i = 0; i < N_TESTS; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- //0 Initial bias
- Vector3d b0 = Vector3d::Zero();
-
- // Set states
- frm0->setState(x0,"POV");
- frm1->setState(x1,"POV");
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
- frm0->fix();
- cap0->fix();
- frm1->fix();
- sensor->getStateBlock('G')->fix();
- cap1->unfix();
- cap1->perturb(0.01);
-
- // solve to estimate bias at cap1
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < N_TESTS; i++)
+ {
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // 0 Initial bias
+ Vector3d b0 = Vector3d::Zero();
+
+ // Set states
+ frm0->setState(x0, "POV");
+ frm1->setState(x1, "POV");
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
+ frm0->fix();
+ cap0->fix();
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+ cap1->unfix();
+ cap1->perturb(0.01);
+
+ // solve to estimate bias at cap1
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
+ // WOLF_INFO(cap1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2dWithGravity, solve_b0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0,"POV");
- frm1->setState(x1,"POV");
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->unfix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- cap0->perturb(0.1);
-
- // solve
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap0->getStateVector("I"), b0, 1e-6);
- //WOLF_INFO(cap0->getStateVector("I"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0, "POV");
+ frm1->setState(x1, "POV");
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ frm0->fix();
+ cap0->unfix();
+ frm1->fix();
+ cap1->fix();
+ sensor->getStateBlock('G')->fix();
+ cap0->perturb(0.1);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(cap0->getStateVector("I"), b0, 1e-6);
+ // WOLF_INFO(cap0->getStateVector("I"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2dwithGravity_test, solve_b1)
{
- for(int i = 0; i < N_TESTS; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- //0 Initial bias
- Vector3d b0 = Vector3d::Random();
-
- // Set states
- frm0->setState(x0,"POV");
- frm1->setState(x1,"POV");
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and frm1, unfix bias, perturb cap1
- frm0->fix();
- cap0->fix();
- frm1->fix();
- sensor->getStateBlock('G')->fix();
- cap1->unfix();
- cap1->perturb(0.01);
-
- // solve to estimate bias at cap1
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap1->getStateVector("I"), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector("I"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < N_TESTS; i++)
+ {
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // 0 Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Set states
+ frm0->setState(x0, "POV");
+ frm1->setState(x1, "POV");
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and frm1, unfix bias, perturb cap1
+ frm0->fix();
+ cap0->fix();
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+ cap1->unfix();
+ cap1->perturb(0.01);
+
+ // solve to estimate bias at cap1
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(cap1->getStateVector("I"), b0, 1e-6);
+ // WOLF_INFO(cap1->getStateVector("I"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2dwithGravity_test, solve_f0)
{
- for(int i = 0; i < N_TESTS; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0,"POV");
- frm1->setState(x1,"POV");
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->unfix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm0->perturb(0.1);
-
- // solve
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
- ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
- //WOLF_INFO(frm0->getStateVector("POV"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < N_TESTS; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0, "POV");
+ frm1->setState(x1, "POV");
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ frm0->unfix();
+ cap0->fix();
+ frm1->fix();
+ cap1->fix();
+ sensor->getStateBlock('G')->fix();
+ frm0->perturb(0.1);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
+ // WOLF_INFO(frm0->getStateVector("POV"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2dwithGravity_test, solve_f1)
{
- for(int i = 0; i < N_TESTS; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0,"POV");
- frm1->setState(x1,"POV");
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm1->perturb(0.1);
-
- // solve
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
- ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
- //WOLF_INFO(frm0->getStateVector("POV"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < N_TESTS; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0, "POV");
+ frm1->setState(x1, "POV");
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ frm0->fix();
+ cap0->fix();
+ frm1->unfix();
+ cap1->fix();
+ sensor->getStateBlock('G')->fix();
+ frm1->perturb(0.1);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
+ // WOLF_INFO(frm0->getStateVector("POV"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2dwithGravity_test, solve_f1_b1)
{
- for(int i = 0; i < N_TESTS; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0,"POV");
- frm1->setState(x1,"POV");
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->fix();
- sensor->getStateBlock('G')->fix();
- frm1->unfix();
- cap1->unfix();
- frm1->perturb(0.1);
- cap1->perturb(0.1);
-
- // solve
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
- ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
- ASSERT_MATRIX_APPROX(cap1->getStateVector("I"), b0, 1e-6);
- //WOLF_INFO(frm0->getStateVector("POV"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < N_TESTS; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0, "POV");
+ frm1->setState(x1, "POV");
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1, perturb frm0
+ frm0->fix();
+ cap0->fix();
+ sensor->getStateBlock('G')->fix();
+ frm1->unfix();
+ cap1->unfix();
+ frm1->perturb(0.1);
+ cap1->perturb(0.1);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector("PO"), x1.head(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm1->getStateVector("V"), x1.tail(2), 1e-6);
+ ASSERT_MATRIX_APPROX(cap1->getStateVector("I"), b0, 1e-6);
+ // WOLF_INFO(frm0->getStateVector("POV"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2dwithGravity_test, bias_zero_solve_G)
{
- for(int i = 0; i < N_TESTS; i++){
- //WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- //Vector2d g = Vector2d::Random()*5;
- Vector2d g = Vector2d::Zero();
-
- //Zero bias
- Vector3d b0 = Vector3d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0,"POV");
- frm1->setState(x1,"POV");
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frames and biases, perturb g
- frm0->fix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->unfix();
- sensor->getStateBlock('G')->perturb(1);
-
- // solve
- std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
-
- ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
- //WOLF_INFO(frm1->getStateVector("POV"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < N_TESTS; i++)
+ {
+ // WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ // Vector2d g = Vector2d::Random()*5;
+ Vector2d g = Vector2d::Zero();
+
+ // Zero bias
+ Vector3d b0 = Vector3d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0, "POV");
+ frm1->setState(x1, "POV");
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frames and biases, perturb g
+ frm0->fix();
+ cap0->fix();
+ frm1->fix();
+ cap1->fix();
+ sensor->getStateBlock('G')->unfix();
+ sensor->getStateBlock('G')->perturb(1);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
+ // WOLF_INFO(frm1->getStateVector("POV"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
TEST_F(FactorImu2dwithGravity_test, solve_G)
{
- for(int i = 0; i < N_TESTS; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0,"POV");
- frm1->setState(x1,"POV");
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frames and captures, perturb g
- frm0->fix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->unfix();
- sensor->getStateBlock('G')->perturb(1);
-
- // solve
- std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
- //WOLF_INFO(cap0->getStateVector("POV"));
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < N_TESTS; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0, "POV");
+ frm1->setState(x1, "POV");
+ cap0->getStateBlock('I')->setState(b0);
+ cap1->getStateBlock('I')->setState(b0);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frames and captures, perturb g
+ frm0->fix();
+ cap0->fix();
+ frm1->fix();
+ cap1->fix();
+ sensor->getStateBlock('G')->unfix();
+ sensor->getStateBlock('G')->perturb(1);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
+ // WOLF_INFO(cap0->getStateVector("POV"));
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
int main(int argc, char **argv)
diff --git a/test/gtest_imu.cpp b/test/gtest_imu.cpp
index 43beb4b56e791638100b421781e6ca29a41c7f50..64824c27585bd721b0ca5d6eddc3e53a3b07c01f 100644
--- a/test/gtest_imu.cpp
+++ b/test/gtest_imu.cpp
@@ -49,8 +49,8 @@ using std::make_shared;
using std::static_pointer_cast;
using std::string;
-string wolf_imu_root = _WOLF_IMU_ROOT_DIR;
-string wolf_root = _WOLF_ROOT_DIR;
+string wolf_imu_root = _WOLF_IMU_CODE_DIR;
+string wolf_root = _WOLF_CODE_DIR;
class Process_Factor_Imu : public testing::Test
{
diff --git a/test/gtest_imu2d_static_init.cpp b/test/gtest_imu2d_static_init.cpp
index d569b313b8b1d3b5570a4e660d014ff64f052a2e..6971696d06bee95c196b2be7b20d95f655aacf16 100644
--- a/test/gtest_imu2d_static_init.cpp
+++ b/test/gtest_imu2d_static_init.cpp
@@ -36,7 +36,7 @@ using namespace Eigen;
using namespace wolf;
using std::make_shared;
-std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+std::string wolf_root = _WOLF_IMU_CODE_DIR;
class ProcessorImu2dStaticInitTest : public testing::Test
{
diff --git a/test/gtest_imu_mocap_fusion.cpp b/test/gtest_imu_mocap_fusion.cpp
index 93f58ec761827d72336c16bc51d0f033cc429a6c..b5cc40a72589f754b775e2d041bc7aafc03cbf7e 100644
--- a/test/gtest_imu_mocap_fusion.cpp
+++ b/test/gtest_imu_mocap_fusion.cpp
@@ -47,7 +47,7 @@ using namespace wolf;
const Vector3d zero3 = Vector3d::Zero();
const double dt = 0.001;
-std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+std::string wolf_root = _WOLF_IMU_CODE_DIR;
class ImuMocapFusion_Test : public testing::Test
{
diff --git a/test/gtest_imu_static_init.cpp b/test/gtest_imu_static_init.cpp
index 95af12f496101b9cc003331a06e0dde1e5eec206..27fb343176edfb532a82be3199e5a3ae6b2f5f27 100644
--- a/test/gtest_imu_static_init.cpp
+++ b/test/gtest_imu_static_init.cpp
@@ -73,7 +73,7 @@ class ProcessorImuStaticInitTest : public testing::Test
using std::static_pointer_cast;
using namespace wolf::Constants;
- std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+ std::string wolf_root = _WOLF_IMU_CODE_DIR;
// Wolf problem
problem_ptr_ = Problem::create("POV", 3);
diff --git a/test/gtest_processor_imu.cpp b/test/gtest_processor_imu.cpp
index d8af4bca720076f847b47fd1fe0ba8ed16ac456a..d612c3f80649583dea1784faa84f6aec7768ca7c 100644
--- a/test/gtest_processor_imu.cpp
+++ b/test/gtest_processor_imu.cpp
@@ -38,7 +38,7 @@ using std::shared_ptr;
using std::make_shared;
using std::static_pointer_cast;
-std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+std::string wolf_root = _WOLF_IMU_CODE_DIR;
SpecComposite problem_prior_{{'P', SpecState("StatePoint3d",
Vector3d::Zero(),
diff --git a/test/gtest_processor_imu2d.cpp b/test/gtest_processor_imu2d.cpp
index 3f1b6e6ffbe0bd87a4b59fcc58a129e9cd001d8f..c9fac472bb6b2b20227b59949e6a2fa34a685386 100644
--- a/test/gtest_processor_imu2d.cpp
+++ b/test/gtest_processor_imu2d.cpp
@@ -37,7 +37,7 @@ using namespace wolf;
using std::shared_ptr;
using std::make_shared;
using std::static_pointer_cast;
-std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+std::string wolf_root = _WOLF_IMU_CODE_DIR;
class ProcessorImu2dTest : public testing::Test
{
@@ -93,7 +93,7 @@ TEST(ProcessorImu2d_constructors, ALL)
ASSERT_EQ(prc1->getAngleTurned(), param_ptr->angle_turned);
//Factory constructor with pointers
- std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+ std::string wolf_root = _WOLF_IMU_CODE_DIR;
ProblemPtr problem = Problem::create("POV", 2);
std::cout << "creating sensor_ptr" << std::endl;
SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d.yaml", {wolf_root});
diff --git a/test/gtest_processor_imu2d_with_gravity.cpp b/test/gtest_processor_imu2d_with_gravity.cpp
index 68412d83dea59e5f48fcf724f90e61f6811d8d8f..58e61f671840a79c98c84c17e962c1b9f5e610db 100644
--- a/test/gtest_processor_imu2d_with_gravity.cpp
+++ b/test/gtest_processor_imu2d_with_gravity.cpp
@@ -64,7 +64,7 @@ class ProcessorImu2dTest : public testing::Test
using std::static_pointer_cast;
using namespace wolf::Constants;
- std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+ std::string wolf_root = _WOLF_IMU_CODE_DIR;
// Wolf problem
problem = Problem::create("POV", 2);
diff --git a/test/gtest_sensor_compass.cpp b/test/gtest_sensor_compass.cpp
index 97904d59688ecb06acc9ebd33f1f620329cf36e4..8b3be46b4bdf52e40c8dbc64df9945bd14893f97 100644
--- a/test/gtest_sensor_compass.cpp
+++ b/test/gtest_sensor_compass.cpp
@@ -32,7 +32,7 @@ using namespace wolf;
using namespace Eigen;
using namespace yaml_schema_cpp;
-std::string wolf_root = _WOLF_IMU_ROOT_DIR;
+std::string wolf_root = _WOLF_IMU_CODE_DIR;
class sensor_compass_test : public testing::Test