diff --git a/include/imu/factor/factor_imu.h b/include/imu/factor/factor_imu.h
index 9e752656f10de383513a406538da1f399c2ec1a7..49f5043359eaa42042c6e7ffd899781735472867 100644
--- a/include/imu/factor/factor_imu.h
+++ b/include/imu/factor/factor_imu.h
@@ -36,7 +36,7 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(FactorImu);
//class
-class FactorImu : public FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>
+class FactorImu : public FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>
{
public:
FactorImu(const FeatureImuPtr& _ftr_ptr,
@@ -61,7 +61,6 @@ class FactorImu : public FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>
const T* const _p2,
const T* const _o2,
const T* const _v2,
- const T* const _b2,
T* _res) const;
/** \brief Estimation error given the states in the wolf tree
@@ -95,8 +94,6 @@ class FactorImu : public FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>
const Eigen::MatrixBase<D1> & _p2,
const Eigen::QuaternionBase<D2> & _q2,
const Eigen::MatrixBase<D1> & _v2,
- const Eigen::MatrixBase<D1> & _ab2,
- const Eigen::MatrixBase<D1> & _wb2,
Eigen::MatrixBase<D3> & _res) const;
/** Function expectation(...)
@@ -150,23 +147,7 @@ class FactorImu : public FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>
/// Metrics
const double dt_; ///< delta-time and delta-time-squared between keyframes
- const double ab_rate_stdev_, wb_rate_stdev_; //stdev for standard_deviation (= sqrt(variance))
- /** bias covariance and bias residuals
- *
- * continuous bias covariance matrix for accelerometer would then be A_r = diag(ab_stdev_^2, ab_stdev_^2, ab_stdev_^2)
- * To compute bias residuals, we will need to do (sqrt(A_r)).inverse() * ab_error
- *
- * In our case, we introduce time 'distance' in the computation of this residual (SEE FORSTER17), thus we have to use the discret covariance matrix
- * discrete bias covariance matrix for accelerometer : A_r_dt = dt_ * A_r
- * taking the square root for bias residuals : sqrt_A_r_dt = sqrt(dt_ * A_r) = sqrt(dt_) * sqrt(A_r)
- * then with the inverse : sqrt_A_r_dt_inv = (sqrt(dt_ * A_r)).inverse() = (1/sqrt(dt_)) * sqrt(A_r).inverse()
- *
- * same logic for gyroscope bias
- */
- const Eigen::Matrix3d sqrt_A_r_dt_inv;
- const Eigen::Matrix3d sqrt_W_r_dt_inv;
-
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
@@ -177,8 +158,8 @@ inline FactorImu::FactorImu(const FeatureImuPtr& _ftr_ptr,
const CaptureImuPtr& _cap_origin_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
- FactorStatus _status) :
- FactorAutodiff<FactorImu, 15, 3, 4, 3, 6, 3, 4, 3, 6>( // ...
+ FactorStatus _status) :
+ FactorAutodiff<FactorImu, 9, 3, 4, 3, 6, 3, 4, 3>( // ...
"FactorImu",
TOP_MOTION,
_ftr_ptr,
@@ -195,8 +176,7 @@ inline FactorImu::FactorImu(const FeatureImuPtr& _ftr_ptr,
_cap_origin_ptr->getSensorIntrinsic(),
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
- _ftr_ptr->getFrame()->getV(),
- _ftr_ptr->getCapture()->getSensorIntrinsic()),
+ _ftr_ptr->getFrame()->getV()),
processor_imu_(std::static_pointer_cast<ProcessorImu>(_processor_ptr)),
dp_preint_(_ftr_ptr->getMeasurement().head<3>()), // dp, dv, dq at preintegration time
dq_preint_(_ftr_ptr->getMeasurement().data()+3),
@@ -208,11 +188,7 @@ inline FactorImu::FactorImu(const FeatureImuPtr& _ftr_ptr,
dDp_dwb_(_ftr_ptr->getJacobianBias().block(0,3,3,3)),
dDv_dwb_(_ftr_ptr->getJacobianBias().block(6,3,3,3)),
dDq_dwb_(_ftr_ptr->getJacobianBias().block(3,3,3,3)),
- dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
- ab_rate_stdev_(std::static_pointer_cast<SensorImu>(_ftr_ptr->getCapture()->getSensor())->getAbRateStdev()),
- wb_rate_stdev_(std::static_pointer_cast<SensorImu>(_ftr_ptr->getCapture()->getSensor())->getWbRateStdev()),
- sqrt_A_r_dt_inv((Eigen::Matrix3d::Identity() * ab_rate_stdev_ * sqrt(dt_)).inverse()),
- sqrt_W_r_dt_inv((Eigen::Matrix3d::Identity() * wb_rate_stdev_ * sqrt(dt_)).inverse())
+ dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp())
{
//
}
@@ -225,7 +201,6 @@ inline bool FactorImu::operator ()(const T* const _p1,
const T* const _p2,
const T* const _q2,
const T* const _v2,
- const T* const _b2,
T* _res) const
{
using namespace Eigen;
@@ -240,12 +215,10 @@ inline bool FactorImu::operator ()(const T* const _p1,
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> > ab2(_b2);
- Map<const Matrix<T,3,1> > wb2(_b2 + 3);
Map<Matrix<T,15,1> > res(_res);
- residual(p1, q1, v1, ab1, wb1, p2, q2, v2, ab2, wb2, res);
+ residual(p1, q1, v1, ab1, wb1, p2, q2, v2, res);
return true;
}
@@ -264,7 +237,7 @@ Eigen::Vector9d FactorImu::error()
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.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);
@@ -283,8 +256,6 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
const Eigen::MatrixBase<D1> & _p2,
const Eigen::QuaternionBase<D2> & _q2,
const Eigen::MatrixBase<D1> & _v2,
- const Eigen::MatrixBase<D1> & _ab2,
- const Eigen::MatrixBase<D1> & _wb2,
Eigen::MatrixBase<D3> & _res) const
{
@@ -405,13 +376,6 @@ inline bool FactorImu::residual(const Eigen::MatrixBase<D1> & _p1,
#endif
- // Errors between biases
- Eigen::Matrix<T,3,1> ab_error(_ab1 - _ab2); // KF1.bias - KF2.bias
- Eigen::Matrix<T,3,1> wb_error(_wb1 - _wb2);
-
- // 4. Residuals are the weighted errors
- _res.segment(9,3) = sqrt_A_r_dt_inv.cast<T>() * ab_error;
- _res.tail(3) = sqrt_W_r_dt_inv.cast<T>() * wb_error;
//////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////// PRINT VALUES ///////////////////////////////////
diff --git a/include/imu/factor/factor_imu2d.h b/include/imu/factor/factor_imu2d.h
index 11b87fee551371907a9ac38883d7c0f2c15fb5a2..394dceecbb2d239585066818bc6b93e4ff53d559 100644
--- a/include/imu/factor/factor_imu2d.h
+++ b/include/imu/factor/factor_imu2d.h
@@ -37,7 +37,7 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(FactorImu2d);
//class
-class FactorImu2d : public FactorAutodiff<FactorImu2d, 8, 2, 1, 2, 3, 2, 1, 2, 3>
+class FactorImu2d : public FactorAutodiff<FactorImu2d, 5, 2, 1, 2, 3, 2, 1, 2>
{
public:
FactorImu2d(const FeatureImu2dPtr& _ftr_ptr,
@@ -62,8 +62,8 @@ class FactorImu2d : public FactorAutodiff<FactorImu2d, 8, 2, 1, 2, 3, 2, 1, 2, 3
const T* const _p2,
const T* const _o2,
const T* const _v2,
- const T* const _b2,
T* _res) const;
+
Eigen::Matrix3d getBiasDriftSquareRootInformationUpper(const FeatureImu2dPtr& _ftr_ptr)
{
Eigen::Matrix3d res = Eigen::Matrix3d::Identity();
@@ -91,20 +91,7 @@ class FactorImu2d : public FactorAutodiff<FactorImu2d, 8, 2, 1, 2, 3, 2, 1, 2, 3
/// Metrics
const double dt_; ///< delta-time and delta-time-squared between keyframes
- /** bias covariance and bias residuals
- *
- * continuous bias covariance matrix for accelerometer would then be A_r = diag(ab_stdev_^2, ab_stdev_^2, ab_stdev_^2)
- * To compute bias residuals, we will need to do (sqrt(A_r)).inverse() * ab_error
- *
- * In our case, we introduce time 'distance' in the computation of this residual (SEE FORSTER17), thus we have to use the discret covariance matrix
- * discrete bias covariance matrix for accelerometer : A_r_dt = dt_ * A_r
- * taking the square root for bias residuals : sqrt_A_r_dt = sqrt(dt_ * A_r) = sqrt(dt_) * sqrt(A_r)
- * then with the inverse : sqrt_A_r_dt_inv = (sqrt(dt_ * A_r)).inverse() = (1/sqrt(dt_)) * sqrt(A_r).inverse()
- *
- * same logic for gyroscope bias
- */
const Eigen::Matrix5d sqrt_delta_preint_inv_;
- const Eigen::Matrix3d sqrt_bias_drift_dt_inv_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -117,7 +104,7 @@ inline FactorImu2d::FactorImu2d(const FeatureImu2dPtr& _ftr_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
- FactorAutodiff<FactorImu2d, 8, 2, 1, 2, 3, 2, 1, 2, 3>( // ...
+ FactorAutodiff<FactorImu2d, 5, 2, 1, 2, 3, 2, 1, 2>( // ...
"FactorImu2d",
TOP_MOTION,
_ftr_ptr,
@@ -134,15 +121,13 @@ inline FactorImu2d::FactorImu2d(const FeatureImu2dPtr& _ftr_ptr,
_cap_origin_ptr->getSensorIntrinsic(),
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
- _ftr_ptr->getFrame()->getV(),
- _ftr_ptr->getCapture()->getSensorIntrinsic()),
+ _ftr_ptr->getFrame()->getV()),
processor_imu2d_(std::static_pointer_cast<ProcessorImu2d>(_processor_ptr)),
delta_preint_(_ftr_ptr->getMeasurement()), // dp, dth, dv at preintegration time
bias_preint_(_ftr_ptr->getBiasPreint()), // state biases at preintegration time
jacobian_bias_(_ftr_ptr->getJacobianBias()), // Jacs of dp dv dq wrt biases
dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
- sqrt_delta_preint_inv_(_ftr_ptr->getMeasurementSquareRootInformationUpper()),
- sqrt_bias_drift_dt_inv_(getBiasDriftSquareRootInformationUpper(_ftr_ptr))
+ sqrt_delta_preint_inv_(_ftr_ptr->getMeasurementSquareRootInformationUpper())
{
//
}
@@ -155,7 +140,6 @@ inline bool FactorImu2d::operator ()(const T* const _p1,
const T* const _p2,
const T* const _th2,
const T* const _v2,
- const T* const _b2,
T* _res) const
{
using namespace Eigen;
@@ -169,9 +153,8 @@ inline bool FactorImu2d::operator ()(const T* const _p1,
Map<const Matrix<T,2,1> > p2(_p2);
const T& th2 = *_th2;
Map<const Matrix<T,2,1> > v2(_v2);
- Map<const Matrix<T,3,1> > b2(_b2);
- Map<Matrix<T,8,1> > res(_res);
+ Map<Matrix<T,5,1> > res(_res);
//residual
/*
@@ -191,12 +174,8 @@ inline bool FactorImu2d::operator ()(const T* const _p1,
Matrix<T, 5, 1> delta_error = delta_corr - delta_predicted;
delta_error(2) = pi2pi(delta_error(2));
- res.template head<5>() = sqrt_delta_preint_inv_*delta_error;
+ res = sqrt_delta_preint_inv_ * delta_error;
-
- //bias drift
- res.template tail<3>() = sqrt_bias_drift_dt_inv_*(b2 -b1);
-
return true;
}
diff --git a/include/imu/factor/factor_imu2d_with_gravity.h b/include/imu/factor/factor_imu2d_with_gravity.h
index 681eb46c285d45c0e8e33674e37b850ca21890fb..77fa8a50b0aa5e5db81fca43ca1355006c5ed7a6 100644
--- a/include/imu/factor/factor_imu2d_with_gravity.h
+++ b/include/imu/factor/factor_imu2d_with_gravity.h
@@ -36,7 +36,7 @@ namespace wolf {
WOLF_PTR_TYPEDEFS(FactorImu2dWithGravity);
//class
-class FactorImu2dWithGravity : public FactorAutodiff<FactorImu2dWithGravity, 8, 2, 1, 2, 3, 2, 1, 2, 3, 2>
+class FactorImu2dWithGravity : public FactorAutodiff<FactorImu2dWithGravity, 5, 2, 1, 2, 3, 2, 1, 2, 2>
{
public:
FactorImu2dWithGravity(const FeatureImu2dPtr& _ftr_ptr,
@@ -61,9 +61,9 @@ class FactorImu2dWithGravity : public FactorAutodiff<FactorImu2dWithGravity, 8,
const T* const _p2,
const T* const _o2,
const T* const _v2,
- const T* const _b2,
const T* const _g,
T* _res) const;
+
Eigen::Matrix3d getBiasDriftSquareRootInformationUpper(const FeatureImu2dPtr& _ftr_ptr)
{
Eigen::Matrix3d res = Eigen::Matrix3d::Identity();
@@ -91,20 +91,7 @@ class FactorImu2dWithGravity : public FactorAutodiff<FactorImu2dWithGravity, 8,
/// Metrics
const double dt_; ///< delta-time and delta-time-squared between keyframes
- /** bias covariance and bias residuals
- *
- * continuous bias covariance matrix for accelerometer would then be A_r = diag(ab_stdev_^2, ab_stdev_^2, ab_stdev_^2)
- * To compute bias residuals, we will need to do (sqrt(A_r)).inverse() * ab_error
- *
- * In our case, we introduce time 'distance' in the computation of this residual (SEE FORSTER17), thus we have to use the discret covariance matrix
- * discrete bias covariance matrix for accelerometer : A_r_dt = dt_ * A_r
- * taking the square root for bias residuals : sqrt_A_r_dt = sqrt(dt_ * A_r) = sqrt(dt_) * sqrt(A_r)
- * then with the inverse : sqrt_A_r_dt_inv = (sqrt(dt_ * A_r)).inverse() = (1/sqrt(dt_)) * sqrt(A_r).inverse()
- *
- * same logic for gyroscope bias
- */
const Eigen::Matrix5d sqrt_delta_preint_inv_;
- const Eigen::Matrix3d sqrt_bias_drift_dt_inv_;
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -117,7 +104,7 @@ inline FactorImu2dWithGravity::FactorImu2dWithGravity(const FeatureImu2dPtr&
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
- FactorAutodiff<FactorImu2dWithGravity, 8, 2, 1, 2, 3, 2, 1, 2, 3, 2>( // ...
+ FactorAutodiff<FactorImu2dWithGravity, 5, 2, 1, 2, 3, 2, 1, 2, 2>( // ...
"FactorImu2dWithGravity",
TOP_MOTION,
_ftr_ptr,
@@ -135,15 +122,13 @@ inline FactorImu2dWithGravity::FactorImu2dWithGravity(const FeatureImu2dPtr&
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
_ftr_ptr->getFrame()->getV(),
- _ftr_ptr->getCapture()->getSensorIntrinsic(),
_cap_origin_ptr->getSensor()->getStateBlock('G')),
processor_imu2d_(std::static_pointer_cast<ProcessorImu2d>(_processor_ptr)),
delta_preint_(_ftr_ptr->getMeasurement()), // dp, dth, dv at preintegration time
bias_preint_(_ftr_ptr->getBiasPreint()), // state biases at preintegration time
jacobian_bias_(_ftr_ptr->getJacobianBias()), // Jacs of dp dv dq wrt biases
dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
- sqrt_delta_preint_inv_(_ftr_ptr->getMeasurementSquareRootInformationUpper()),
- sqrt_bias_drift_dt_inv_(getBiasDriftSquareRootInformationUpper(_ftr_ptr))
+ sqrt_delta_preint_inv_(_ftr_ptr->getMeasurementSquareRootInformationUpper())
{
//
}
@@ -156,7 +141,6 @@ inline bool FactorImu2dWithGravity::operator ()(const T* const _p1,
const T* const _p2,
const T* const _th2,
const T* const _v2,
- const T* const _b2,
const T* const _g,
T* _res) const
{
@@ -171,10 +155,9 @@ inline bool FactorImu2dWithGravity::operator ()(const T* const _p1,
Map<const Matrix<T,2,1> > p2(_p2);
const T& th2 = *_th2;
Map<const Matrix<T,2,1> > v2(_v2);
- Map<const Matrix<T,3,1> > b2(_b2);
Map<const Matrix<T,2,1> > g(_g);
- Map<Matrix<T,8,1> > res(_res);
+ Map<Matrix<T,5,1> > res(_res);
//residual
/*
@@ -194,12 +177,8 @@ inline bool FactorImu2dWithGravity::operator ()(const T* const _p1,
Matrix<T, 5, 1> delta_error = imu2d::diff(delta_predicted, delta_corr);
delta_error(2) = pi2pi(delta_error(2));
- res.template head<5>() = sqrt_delta_preint_inv_*delta_error;
+ res = sqrt_delta_preint_inv_*delta_error;
-
- //bias drift
- res.template tail<3>() = sqrt_bias_drift_dt_inv_*(b2 -b1);
-
return true;
}
diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h
index 1c03c1b41371b0fdbc412fdb3391c063c7f90c5c..0fb04fd3fe13363407da78b03d3b9bb7d05a081e 100644
--- a/include/imu/processor/processor_imu.h
+++ b/include/imu/processor/processor_imu.h
@@ -22,9 +22,12 @@
#ifndef PROCESSOR_IMU_H
#define PROCESSOR_IMU_H
-// Wolf
+// Wolf Imu
+#include "imu/sensor/sensor_imu.h"
#include "imu/capture/capture_imu.h"
#include "imu/feature/feature_imu.h"
+// Wolf core
+
#include <core/processor/processor_motion.h>
namespace wolf {
@@ -82,7 +85,7 @@ class ProcessorImu : public ProcessorMotion{
ProcessorImu(ParamsProcessorImuPtr _params_motion_Imu);
~ProcessorImu() override;
WOLF_PROCESSOR_CREATE(ProcessorImu, ParamsProcessorImu);
- void configure(SensorBasePtr _sensor) override { };
+ void configure(SensorBasePtr _sensor) override;
void preProcess() override;
@@ -147,8 +150,10 @@ class ProcessorImu : public ProcessorMotion{
bool recomputeStates() const;
protected:
- ParamsProcessorImuPtr params_motion_Imu_;
- std::list<FactorBasePtr> bootstrap_factor_list_; ///< List of all IMU factors created while IMU is bootstrapping
+ ParamsProcessorImuPtr params_motion_Imu_;
+ std::list<FactorBasePtr> bootstrap_factor_list_; ///< List of all IMU factors created while IMU is bootstrapping
+ SensorImuPtr sensor_imu_;
+ Matrix6d imu_drift_cov_;
};
}
diff --git a/include/imu/processor/processor_imu2d.h b/include/imu/processor/processor_imu2d.h
index 44f418140aec1b066a8a07600e4ec535d6c1f01f..f8f4a547838bc7e13db224f915cb8d8934ea7975 100644
--- a/include/imu/processor/processor_imu2d.h
+++ b/include/imu/processor/processor_imu2d.h
@@ -23,6 +23,7 @@
#define PROCESSOR_IMU2D_H
// Wolf
+#include "imu/sensor/sensor_imu2d.h"
#include "imu/capture/capture_imu.h"
#include "imu/feature/feature_imu.h"
#include <core/processor/processor_motion.h>
@@ -51,7 +52,7 @@ class ProcessorImu2d : public ProcessorMotion{
public:
ProcessorImu2d(ParamsProcessorImu2dPtr _params_motion_Imu);
~ProcessorImu2d() override;
- void configure(SensorBasePtr _sensor) override { };
+ void configure(SensorBasePtr _sensor) override;
WOLF_PROCESSOR_CREATE(ProcessorImu2d, ParamsProcessorImu2d);
void preProcess() override;
@@ -97,6 +98,8 @@ class ProcessorImu2d : public ProcessorMotion{
protected:
ParamsProcessorImu2dPtr params_motion_Imu_;
+ SensorImu2dPtr sensor_imu2d_;
+ Matrix3d imu_drift_cov_;
};
}
diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h
index 0b9cf145a65d88704296ff7f0dcf54069356595c..7e3e2b38bf38ebf29e5f0957b2735434421f1704 100644
--- a/include/imu/sensor/sensor_imu.h
+++ b/include/imu/sensor/sensor_imu.h
@@ -95,12 +95,12 @@ class SensorImu : public SensorBase
SensorImu(const Eigen::VectorXd& _extrinsics, ParamsSensorImuPtr _params);
WOLF_SENSOR_CREATE(SensorImu, ParamsSensorImu, 7);
- double getGyroNoise() const;
double getAccelNoise() const;
- double getWbInitialStdev() const;
+ double getGyroNoise() const;
double getAbInitialStdev() const;
- double getWbRateStdev() const;
+ double getWbInitialStdev() const;
double getAbRateStdev() const;
+ double getWbRateStdev() const;
~SensorImu() override;
diff --git a/src/capture/capture_imu.cpp b/src/capture/capture_imu.cpp
index 14a604c9261613207b5f73c1a899bcf6f782136d..7f38375ad2335533033fb4352a3871c65a6007b2 100644
--- a/src/capture/capture_imu.cpp
+++ b/src/capture/capture_imu.cpp
@@ -41,9 +41,11 @@ CaptureImu::CaptureImu(const TimeStamp& _init_ts,
_capture_origin_ptr,
nullptr,
nullptr,
- (_sensor_ptr->getProblem()->getDim() == 2)
- ? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(Vector3d::Zero(), false))
- : std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(Vector6d::Zero(), false)))
+ (_sensor_ptr->isStateBlockDynamic('I'))
+ ? ((_sensor_ptr->getProblem()->getDim() == 2)
+ ? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(Vector3d::Zero(), false))
+ : std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(Vector6d::Zero(), false)))
+ : nullptr)
{
//
}
@@ -63,11 +65,14 @@ CaptureImu::CaptureImu(const TimeStamp& _init_ts,
_capture_origin_ptr,
nullptr,
nullptr,
- (_bias.size() == 3)
- ? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(Vector3d::Zero(), false))
- : std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(Vector6d::Zero(), false)))
+ (_sensor_ptr->isStateBlockDynamic('I'))
+ ? ((_sensor_ptr->getProblem()->getDim() == 2)
+ ? std::static_pointer_cast<StateBlock>(std::make_shared<StateParams3>(_bias, false))
+ : std::static_pointer_cast<StateBlock>(std::make_shared<StateParams6>(_bias, false)))
+ : nullptr)
{
assert((_bias.size() == 3) or (_bias.size() == 6));
+ WOLF_WARN_COND(_sensor_ptr->isStateBlockDynamic('I'), "Sensor bias was provided but bias is static in sensor. Bias discarded.");
}
CaptureImu::~CaptureImu()
diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index b0b5c56497cadc61f0c83e0735be58952dc2dabe..c4cba6b04638efb09da580494f56755caaf4d1f9 100644
--- a/src/processor/processor_imu.cpp
+++ b/src/processor/processor_imu.cpp
@@ -26,6 +26,7 @@
// wolf
#include <core/state_block/state_composite.h>
+#include <core/factor/factor_block_difference.h>
namespace wolf {
@@ -106,25 +107,77 @@ void ProcessorImu::setCalibration (const CaptureBasePtr _capture, const VectorXd
_capture->getSensorIntrinsic()->setState(_calibration);
}
+void ProcessorImu::configure(SensorBasePtr _sensor)
+{
+ sensor_imu_ = std::static_pointer_cast<SensorImu>(_sensor);
+
+ auto acc_drift_std = sensor_imu_->getAbRateStdev();
+ auto gyro_drift_std = sensor_imu_->getAbRateStdev();
+
+ ArrayXd imu_drift_sigmas(6);
+ imu_drift_sigmas << acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std;
+ imu_drift_cov_ = imu_drift_sigmas.square().matrix().asDiagonal();
+
+}
void ProcessorImu::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
{
- auto feature = FeatureBase::emplace<FeatureImu>(_capture_own,
- _capture_own->getBuffer().back().delta_integr_,
- _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
- _capture_own->getCalibrationPreint(),
- _capture_own->getBuffer().back().jacobian_calib_);
+ // 1. Feature and factor Imu
+ //---------------------------
+
+ auto ftr_imu = FeatureBase::emplace<FeatureImu>(
+ _capture_own, _capture_own->getBuffer().back().delta_integr_,
+ _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_,
+ _capture_own->getCalibrationPreint(), _capture_own->getBuffer().back().jacobian_calib_);
CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin);
- FeatureImuPtr ftr_imu = std::static_pointer_cast<FeatureImu>(feature);
- auto fac_imu = FactorBase::emplace<FactorImu>(feature, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
+ auto fac_imu =
+ FactorBase::emplace<FactorImu>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), params_->apply_loss_function);
- if (bootstrapping_)
+ if (bootstrapping_)
{
fac_imu->setStatus(FAC_INACTIVE);
bootstrap_factor_list_.push_back(fac_imu);
}
+ // 2. Feature and factor bias -- IMU bias drift for acc and gyro
+ //---------------------------------------------------------------
+ // - This factor only if IMU biases are Dynamic (check sensor->isStateBlockDynamic('I'))
+ // - feature has zero measurement size 6, with the cov of the bias drift size 6x6
+ // - factor relates bias(last capture) and bias(origin capture)
+ if (getSensor()->isStateBlockDynamic('I'))
+ {
+ const auto& sb_imubias_own = _capture_own->getStateBlock('I');
+ const auto& sb_imubias_origin = _capture_origin->getStateBlock('I');
+ if (sb_imubias_own != sb_imubias_origin) // make sure it's two different state blocks! -- just in case
+ {
+ auto dt = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp();
+ auto ftr_bias =
+ FeatureBase::emplace<FeatureBase>(_capture_own, "FeatureBase",
+ Vector6d::Zero(), // mean IMU drift is zero
+ imu_drift_cov_ * dt); // IMU drift cov specified in continuous time
+ auto fac_bias =
+ FactorBase::emplace<FactorBlockDifference>(ftr_bias, ftr_bias,
+ sb_imubias_own, // IMU bias block at t=own
+ sb_imubias_origin, // IMU bias block at t=origin
+ nullptr, // frame other
+ _capture_origin, // origin capture
+ nullptr, // feature other
+ nullptr, // landmark other
+ 0, // take all of first state block
+ -1, // take all of first state block
+ 0, // take all of first second block
+ -1, // take all of first second block
+ shared_from_this(), // this processor
+ params_->apply_loss_function); // loss function
+
+ if (bootstrapping_)
+ {
+ fac_bias->setStatus(FAC_INACTIVE);
+ bootstrap_factor_list_.push_back(fac_bias);
+ }
+ }
+ }
}
void ProcessorImu::computeCurrentDelta(const Eigen::VectorXd& _data,
@@ -391,9 +444,12 @@ VectorXd ProcessorImu::bootstrapDelta() const
for (const auto& fac : bootstrap_factor_list_)
// here, we take advantage of the list of IMU factors to recover all deltas
{
- dt = fac->getCapture()->getTimeStamp() - fac->getCaptureOther()->getTimeStamp();
- const auto& delta = fac->getFeature()->getMeasurement(); // In FeatImu, delta = measurement
- delta_int = imu::compose(delta_int, delta, dt);
+ if (std::dynamic_pointer_cast<FactorImu>(fac) != nullptr)
+ {
+ dt = fac->getCapture()->getTimeStamp() - fac->getCaptureOther()->getTimeStamp();
+ const auto& delta = fac->getFeature()->getMeasurement(); // In FeatImu, delta = measurement
+ delta_int = imu::compose(delta_int, delta, dt);
+ }
}
// now compose with delta in last_ptr_
dt = last_ptr_->getBuffer().back().ts_ - origin_ptr_->getTimeStamp();
@@ -413,16 +469,19 @@ bool ProcessorImu::recomputeStates() const
WOLF_DEBUG("Recomputing IMU keyframe states...");
for (const auto& fac : bootstrap_factor_list_)
{
- const auto& ftr = fac->getFeature();
- const auto& cap = std::static_pointer_cast<CaptureMotion>(ftr->getCapture());
- const auto& frm = cap->getFrame();
- const auto& cap_origin = cap->getOriginCapture();
- const auto& frm_origin = cap_origin->getFrame();
- const auto& delta = VectorComposite(ftr->getMeasurement(), "POV", {3, 4, 3});
- const auto& x_origin = frm_origin->getState();
- auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp();
- auto x = imu::composeOverState(x_origin, delta, dt);
- frm->setState(x);
+ if (std::dynamic_pointer_cast<FactorImu>(fac) != nullptr)
+ {
+ const auto& ftr = fac->getFeature();
+ const auto& cap = std::static_pointer_cast<CaptureMotion>(ftr->getCapture());
+ const auto& frm = cap->getFrame();
+ const auto& cap_origin = cap->getOriginCapture();
+ const auto& frm_origin = cap_origin->getFrame();
+ const auto& delta = VectorComposite(ftr->getMeasurement(), "POV", {3, 4, 3});
+ const auto& x_origin = frm_origin->getState();
+ auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp();
+ auto x = imu::composeOverState(x_origin, delta, dt);
+ frm->setState(x);
+ }
}
return true;
}
diff --git a/src/processor/processor_imu2d.cpp b/src/processor/processor_imu2d.cpp
index 23fbbd222c60e28df1eb36ba9742c7efb83b6604..10c6f25d5bd616737c4d7b0f65ceb2759aa64999 100644
--- a/src/processor/processor_imu2d.cpp
+++ b/src/processor/processor_imu2d.cpp
@@ -27,12 +27,14 @@
*/
// imu
#include "imu/processor/processor_imu2d.h"
+#include "imu/sensor/sensor_imu2d.h"
#include "imu/factor/factor_imu2d.h"
#include "imu/factor/factor_imu2d_with_gravity.h"
#include "imu/math/imu2d_tools.h"
// wolf
#include <core/state_block/state_composite.h>
+#include <core/factor/factor_block_difference.h>
namespace wolf {
@@ -113,6 +115,17 @@ namespace wolf {
{
_capture->getSensorIntrinsic()->setState(_calibration);
}
+void ProcessorImu2d::configure(SensorBasePtr _sensor)
+{
+ sensor_imu2d_ = std::static_pointer_cast<SensorImu2d>(_sensor);
+
+ auto acc_drift_std = sensor_imu2d_->getAbRateStdev();
+ auto gyro_drift_std = sensor_imu2d_->getWbRateStdev();
+
+ Array3d imu_drift_sigmas(acc_drift_std, acc_drift_std, gyro_drift_std);
+ imu_drift_cov_ = imu_drift_sigmas.square().matrix().asDiagonal();
+
+}
void ProcessorImu2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own)
{
@@ -130,6 +143,35 @@ namespace wolf {
else
FactorBase::emplace<FactorImu2dWithGravity>(ftr_imu, ftr_imu, cap_imu, shared_from_this(),
params_->apply_loss_function);
+
+ if (getSensor()->isStateBlockDynamic('I'))
+ {
+ const auto& sb_imubias_own = _capture_own->getStateBlock('I');
+ const auto& sb_imubias_origin = _capture_origin->getStateBlock('I');
+ if (sb_imubias_own != sb_imubias_origin) // make sure it's two different state blocks! -- just in case
+ {
+ auto dt = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp();
+ auto ftr_bias = FeatureBase::emplace<FeatureBase>(
+ _capture_own, "FeatureBase",
+ Vector3d::Zero(), // mean IMU drift is zero
+ imu_drift_cov_ * dt); // IMU drift cov specified in continuous time
+ auto fac_bias =
+ FactorBase::emplace<FactorBlockDifference>(ftr_bias, ftr_bias,
+ sb_imubias_own, // IMU bias block at t=own
+ sb_imubias_origin, // IMU bias block at t=origin
+ nullptr, // frame other
+ _capture_origin, // origin capture
+ nullptr, // feature other
+ nullptr, // landmark other
+ 0, // take all of first state block
+ -1, // take all of first state block
+ 0, // take all of first second block
+ -1, // take all of first second block
+ shared_from_this(), // this processor
+ params_->apply_loss_function); // loss function
+
+ }
+ }
}
void ProcessorImu2d::computeCurrentDelta(const Eigen::VectorXd& _data,
diff --git a/src/sensor/sensor_imu2d.cpp b/src/sensor/sensor_imu2d.cpp
index ca6130465acdac333330b3c97c656ac12a038778..3fe55da10b1fb5d9d303d5e2b1862085f42c815f 100644
--- a/src/sensor/sensor_imu2d.cpp
+++ b/src/sensor/sensor_imu2d.cpp
@@ -37,10 +37,10 @@ SensorImu2d::SensorImu2d(const Eigen::VectorXd& _extrinsics, const ParamsSensorI
std::make_shared<StatePoint2d>(_extrinsics.head(2), true, false),
std::make_shared<StateAngle>(_extrinsics(2), true, false),
std::make_shared<StateParams3>(Vector3d::Zero(3), false),
- (Eigen::Vector3d() << _params.a_noise, _params.a_noise, _params.w_noise).finished(),
+ Eigen::Vector3d(_params.a_noise, _params.a_noise, _params.w_noise),
false,
false,
- true),
+ false),
a_noise(_params.a_noise),
w_noise(_params.w_noise),
ab_initial_stdev(_params.ab_initial_stdev),
diff --git a/test/gtest_factor_imu2d.cpp b/test/gtest_factor_imu2d.cpp
index 7dd71060608bd87f8b3188f9827f213d181322e6..6aafb68474cf2c95f12544b18f50bc0e809b284d 100644
--- a/test/gtest_factor_imu2d.cpp
+++ b/test/gtest_factor_imu2d.cpp
@@ -29,418 +29,328 @@
using namespace Eigen;
using namespace wolf;
-// Input odometry data and covariance
-Matrix6d data_cov = 0.1*Matrix6d::Identity();
-Matrix5d delta_cov = 0.1*Matrix5d::Identity();
-
-// Jacobian of the bias
-MatrixXd jacobian_bias((MatrixXd(5,3) << 1, 0, 0,
- 0, 1, 0,
- 0, 0, 1,
- 1, 0, 0,
- 0, 1, 0 ).finished());
-//preintegration bias
-Vector3d b0_preint = Vector3d::Zero();
-
-// Problem and solver
-ProblemPtr problem_ptr = Problem::create("POV", 2);
-SolverCeres solver(problem_ptr);
-
-// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector5d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector5d::Zero());
-
-// Imu2d sensor
-SensorBasePtr sensor = std::make_shared<SensorImu2d>( Vector3d::Zero(), ParamsSensorImu2d() ); // default params: see sensor_imu2d.h
-
-//capture from frm1 to frm0
-auto cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
-auto cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
-
-TEST(FactorImu2d, check_tree)
+class FactorImu2d_test : public testing::Test
{
- ASSERT_TRUE(problem_ptr->check(0));
-}
-
-TEST(FactorImu2d, bias_zero_solve_f1)
+ public:
+ ProblemPtr problem;
+ SolverCeresPtr solver;
+ SensorImu2dPtr sensor;
+ FrameBasePtr frm0, frm1;
+ CaptureImuPtr cap0, cap1;
+ Matrix6d data_cov;
+ Matrix5d delta_cov;
+ MatrixXd jacobian_bias;
+ Vector3d b0_preint;
+
+ void SetUp() override
+ {
+ // Input odometry data and covariance
+ data_cov = 0.1 * Matrix6d::Identity();
+ delta_cov = 0.1 * Matrix5d::Identity();
+
+ // Jacobian of the bias
+ jacobian_bias = ((MatrixXd(5, 3) << 1, 0, 0, //
+ 0, 1, 0, //
+ 0, 0, 1, //
+ 1, 0, 0, //
+ 0, 1, 0).finished());
+ // preintegration bias
+ b0_preint = Vector3d::Zero();
+
+ // Problem and solver
+ problem = Problem::create("POV", 2);
+ solver = std::make_shared<SolverCeres>(problem);
+
+ // sensor
+ sensor = SensorBase::emplace<SensorImu2d>(problem->getHardware(), Vector3d::Zero(), ParamsSensorImu2d());
+ sensor->setStateBlockDynamic('I', false);
+
+ // Two frames
+ frm0 = problem->emplaceFrame(TimeStamp(0), Vector5d::Zero());
+ frm1 = problem->emplaceFrame(TimeStamp(1), Vector5d::Zero());
+
+ // capture from frm1 to frm0
+ cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, nullptr);
+ cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, cap0);
+ }
+};
+
+TEST_F(FactorImu2d_test, problem_check)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and biases, perturb frm1
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->fix();
- frm1->perturb(0.01);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ ASSERT_TRUE(problem->check(0));
}
-TEST(FactorImu2d, bias_zero_solve_f0)
+TEST_F(FactorImu2d_test, bias_zero_solve_f1)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and biases, perturb frm1
- frm0->unfix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- frm0->perturb(0.01);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and biases, perturb frm1
+ sensor->fix();
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(0.01);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2d, bias_zero_solve_b1)
+TEST_F(FactorImu2d_test, bias_zero_solve_f0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- //0 Initial bias
- Vector3d b0 = Vector3d::Zero();
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
- frm0->fix();
- cap0->fix();
- frm1->fix();
- cap1->unfix();
- cap1->perturb(0.01);
-
- // solve to estimate bias at cap1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and biases, perturb frm1
+ sensor->fix();
+ frm0->unfix();
+ frm0->perturb(0.01);
+ frm1->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2d, solve_b0)
+TEST_F(FactorImu2d_test, bias_zero_solve)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->unfix();
- frm1->fix();
- cap1->fix();
- cap0->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(cap0->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
-}
+ for (int i = 0; i < 50; i++)
+ {
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
-TEST(FactorImu2d, solve_b1)
-{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- //0 Initial bias
- Vector3d b0 = Vector3d::Random();
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and frm1, unfix bias, perturb cap1
- frm0->fix();
- cap0->fix();
- frm1->fix();
- cap1->unfix();
- cap1->perturb(0.01);
-
- // solve to estimate bias at cap1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // 0 Initial bias
+ Vector3d b0 = Vector3d::Zero();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->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
+ sensor->getStateBlock('I')->unfix();
+ sensor->getStateBlock('I')->perturb(0.01);
+ frm0->fix();
+ frm1->fix();
+
+ // solve to estimate bias at cap1
+ auto report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2d, solve_f0)
+TEST_F(FactorImu2d_test, solve_b0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->unfix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- frm0->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->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
+ sensor->getStateBlock('I')->unfix();
+ sensor->getStateBlock('I')->perturb(0.1);
+ frm0->fix();
+ frm1->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2d, solve_f1)
+TEST_F(FactorImu2d_test, solve_f0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->fix();
- frm1->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->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
+ sensor->fix();
+ frm0->unfix();
+ frm0->perturb(0.1);
+ frm1->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2d, solve_f1_b1)
+TEST_F(FactorImu2d_test, solve_f1)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::compose(x0, delta, 1);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2d>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->unfix();
- frm1->perturb(0.1);
- cap1->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::compose(x0, delta, 1);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->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
+ sensor->fix();
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(0.1);
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "FactorImu2d.no_bias_fixed"; // Test only this one
+ // ::testing::GTEST_FLAG(filter) = "FactorImu2d_test.solve_f1_b1"; // Test only this one
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_factor_imu2d_with_gravity.cpp b/test/gtest_factor_imu2d_with_gravity.cpp
index 931fcd3db298b8aa27e4f0a9edf189fbc98f9bb5..51ee79d6ee34e460d02c6022a016828d1bdfd627 100644
--- a/test/gtest_factor_imu2d_with_gravity.cpp
+++ b/test/gtest_factor_imu2d_with_gravity.cpp
@@ -29,572 +29,473 @@
using namespace Eigen;
using namespace wolf;
-// Input odometry data and covariance
-Matrix6d data_cov = 0.1*Matrix6d::Identity();
-Matrix5d delta_cov = 0.1*Matrix5d::Identity();
-
-// Jacobian of the bias
-MatrixXd jacobian_bias((MatrixXd(5,3) << 1, 0, 0,
- 0, 1, 0,
- 0, 0, 1,
- 1, 0, 0,
- 0, 1, 0 ).finished());
-//preintegration bias
-Vector3d b0_preint = Vector3d::Zero();
-
-// Problem and solver
-ProblemPtr problem_ptr = Problem::create("POV", 2);
-SolverCeres solver(problem_ptr);
-
-// Two frames
-FrameBasePtr frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector5d::Zero());
-FrameBasePtr frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector5d::Zero());
-
-// Imu2d sensor
-ParamsSensorImu2dPtr sensorparams = std::make_shared<ParamsSensorImu2d>(false);
-SensorBasePtr sensor = SensorBase::emplace<SensorImu2d>(problem_ptr->getHardware(), Vector3d::Zero(), *sensorparams );
-
-//capture from frm1 to frm0
-auto cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr);
-auto cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), cap0);
-
-TEST(FactorImu2dWithGravity, check_tree)
+class FactorImu2dWithGravity_test : public testing::Test
{
- ASSERT_TRUE(problem_ptr->check(0));
-}
-
-TEST(FactorImu2dWithGravity, bias_zero_solve_f1)
+ public:
+ ProblemPtr problem;
+ SolverCeresPtr solver;
+ SensorImu2dPtr sensor;
+ FrameBasePtr frm0, frm1;
+ CaptureImuPtr cap0, cap1;
+ Matrix6d data_cov;
+ Matrix5d delta_cov;
+ MatrixXd jacobian_bias;
+ Vector3d b0_preint;
+
+ void SetUp() override
+ {
+ // Input odometry data and covariance
+ data_cov = 0.1 * Matrix6d::Identity();
+ delta_cov = 0.1 * Matrix5d::Identity();
+
+ // Jacobian of the bias
+ jacobian_bias = ((MatrixXd(5, 3) << 1, 0, 0, //
+ 0, 1, 0, //
+ 0, 0, 1, //
+ 1, 0, 0, //
+ 0, 1, 0)
+ .finished());
+ // preintegration bias
+ b0_preint = Vector3d::Zero();
+
+ // Problem and solver
+ problem = Problem::create("POV", 2);
+ solver = std::make_shared<SolverCeres>(problem);
+
+ // sensor
+ ParamsSensorImu2dPtr sensorparams = std::make_shared<ParamsSensorImu2d>(false);
+ sensor = SensorBase::emplace<SensorImu2d>(problem->getHardware(), Vector3d::Zero(), sensorparams);
+ sensor->setStateBlockDynamic('I', false);
+
+ // Two frames
+ frm0 = problem->emplaceFrame(TimeStamp(0), Vector5d::Zero());
+ frm1 = problem->emplaceFrame(TimeStamp(1), Vector5d::Zero());
+
+ // capture from frm1 to frm0
+ cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, nullptr);
+ cap1 = CaptureBase::emplace<CaptureImu>(frm1, 1, sensor, Vector6d::Zero(), data_cov, cap0);
+
+ problem->print(4,1,1,1);
+ }
+};
+
+TEST_F(FactorImu2dWithGravity_test, check_tree)
{
- for(int i = 0; i < 50; i++){
- //WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and biases, perturb frm1
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm1->perturb(0.01);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
- //std::cout << report << std::endl;
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ ASSERT_TRUE(problem->check(0));
}
-TEST(FactorImu2dWithGravity, bias_zero_solve_f0)
+TEST_F(FactorImu2dWithGravity_test, bias_zero_solve_f1)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1 and biases, perturb frm0
- frm0->unfix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm0->perturb(0.01);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm0 and biases, perturb frm1
+ sensor->fix();
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(0.01);
+ sensor->getStateBlock('G')->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+ // std::cout << report << std::endl;
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+ // WOLF_INFO(frm1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, bias_zero_solve_b1)
+TEST_F(FactorImu2dWithGravity_test, bias_zero_solve_f0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- //0 Initial bias
- Vector3d b0 = Vector3d::Zero();
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0, frm1 and cap0, unfix cap1 (bias), perturb cap1
- frm0->fix();
- cap0->fix();
- frm1->fix();
- sensor->getStateBlock('G')->fix();
- cap1->unfix();
- cap1->perturb(0.01);
-
- // solve to estimate bias at cap1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->getStateBlock('G')->setState(g);
+
+ // feature & factor with delta measurement
+ auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
+ FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
+
+ // Fix frm1 and biases, perturb frm0
+ sensor->fix();
+ frm0->unfix();
+ frm0->perturb(0.01);
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+ // WOLF_INFO(frm1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_b0)
+TEST_F(FactorImu2dWithGravity_test, bias_zero_solve)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->unfix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- cap0->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap0->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // 0 Initial bias
+ Vector3d b0 = Vector3d::Zero();
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->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
+ sensor->getStateBlock('I')->unfix();
+ sensor->getStateBlock('I')->perturb(0.01);
+ frm0->fix();
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+
+ // solve to estimate bias at cap1
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_b1)
+TEST_F(FactorImu2dWithGravity_test, solve_b0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- //0 Initial bias
- Vector3d b0 = Vector3d::Random();
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm0 and frm1, unfix bias, perturb cap1
- frm0->fix();
- cap0->fix();
- frm1->fix();
- sensor->getStateBlock('G')->fix();
- cap1->unfix();
- cap1->perturb(0.01);
-
- // solve to estimate bias at cap1
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(cap1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->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
+ sensor->getStateBlock('I')->unfix();
+ sensor->getStateBlock('I')->perturb(0.1);
+ frm0->fix();
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('I')->getState(), b0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_f0)
+TEST_F(FactorImu2dWithGravity_test, solve_f0)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->unfix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm0->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
-}
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->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
+ sensor->fix();
+ frm0->unfix();
+ frm0->perturb(0.1);
+ frm1->fix();
+ sensor->getStateBlock('G')->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm0->getStateVector(), x0, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_f1)
+TEST_F(FactorImu2dWithGravity_test, solve_f1)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->fix();
- frm1->unfix();
- cap1->fix();
- sensor->getStateBlock('G')->fix();
- frm1->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->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
+ sensor->fix();
+ frm0->fix();
+ frm1->unfix();
+ frm1->perturb(0.1);
+ sensor->getStateBlock('G')->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_f1_b1)
-{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frm1, perturb frm0
- frm0->fix();
- cap0->fix();
- sensor->getStateBlock('G')->fix();
- frm1->unfix();
- cap1->unfix();
- frm1->perturb(0.1);
- cap1->perturb(0.1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_POSE2d_APPROX(frm1->getStateVector(), x1, 1e-6);
- ASSERT_MATRIX_APPROX(cap1->getStateVector(), b0, 1e-6);
- //WOLF_INFO(frm0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
-}
-TEST(FactorImu2dWithGravity, bias_zero_solve_G)
+TEST_F(FactorImu2dWithGravity_test, bias_zero_solve_G)
{
- for(int i = 0; i < 50; i++){
- //WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
- // Random delta
- Vector5d delta = Vector5d::Random() * 10; //delta *= 0;
- delta(2) = pi2pi(delta(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10; //x0 *= 0;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- //Vector2d g = Vector2d::Random()*5;
- Vector2d g = Vector2d::Zero();
-
- //Zero bias
- Vector3d b0 = Vector3d::Zero();
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta, delta_cov, b0_preint, jacobian_bias, cap1);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frames and biases, perturb g
- frm0->fix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->unfix();
- sensor->getStateBlock('G')->perturb(1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::FULL);
-
- ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
- //WOLF_INFO(frm1->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // WOLF_INFO("gravity is orthogonal: ", sensorparams->orthogonal_gravity);
+ // Random delta
+ Vector5d delta = Vector5d::Random() * 10; // delta *= 0;
+ delta(2) = pi2pi(delta(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10; // x0 *= 0;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ // Vector2d g = Vector2d::Random()*5;
+ Vector2d g = Vector2d::Zero();
+
+ // Zero bias
+ Vector3d b0 = Vector3d::Zero();
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->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
+ sensor->fix();
+ sensor->getStateBlock('G')->unfix();
+ sensor->getStateBlock('G')->perturb(1);
+ frm0->fix();
+ frm1->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
+ // WOLF_INFO(frm1->getStateVector());
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
-TEST(FactorImu2dWithGravity, solve_G)
+TEST_F(FactorImu2dWithGravity_test, solve_G)
{
- for(int i = 0; i < 50; i++){
- // Random delta
- Vector5d delta_biased = Vector5d::Random() * 10;
- delta_biased(2) = pi2pi(delta_biased(2));
-
- // Random initial pose
- Vector5d x0 = Vector5d::Random() * 10;
- x0(2) = pi2pi(x0(2));
-
- //Random gravity
- Vector2d g = Vector2d::Random()*5;
- //Vector2d g = Vector2d::Zero();
-
- //Random Initial bias
- Vector3d b0 = Vector3d::Random();
-
- //Corrected delta
- Vector5d delta_step = jacobian_bias*(b0-b0_preint);
- Vector5d delta = imu2d::plus(delta_biased, delta_step);
-
- // x1 groundtruth
- Vector5d x1;
- x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
-
- // Set states
- frm0->setState(x0);
- frm1->setState(x1);
- cap0->getStateBlock('I')->setState(b0);
- cap1->getStateBlock('I')->setState(b0);
- sensor->getStateBlock('G')->setState(g);
-
- // feature & factor with delta measurement
- auto fea1 = FeatureBase::emplace<FeatureImu2d>(cap1, delta_biased, delta_cov, b0_preint, jacobian_bias);
- FactorBase::emplace<FactorImu2dWithGravity>(fea1, fea1, cap0, nullptr, false);
-
- // Fix frames and captures, perturb g
- frm0->fix();
- cap0->fix();
- frm1->fix();
- cap1->fix();
- sensor->getStateBlock('G')->unfix();
- sensor->getStateBlock('G')->perturb(1);
-
- // solve
- std::string report = solver.solve(SolverManager::ReportVerbosity::BRIEF);
-
- ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
- //WOLF_INFO(cap0->getStateVector());
-
- // remove feature (and factor) for the next loop
- fea1->remove();
- }
+ for (int i = 0; i < 50; i++)
+ {
+ // Random delta
+ Vector5d delta_biased = Vector5d::Random() * 10;
+ delta_biased(2) = pi2pi(delta_biased(2));
+
+ // Random initial pose
+ Vector5d x0 = Vector5d::Random() * 10;
+ x0(2) = pi2pi(x0(2));
+
+ // Random gravity
+ Vector2d g = Vector2d::Random() * 5;
+ // Vector2d g = Vector2d::Zero();
+
+ // Random Initial bias
+ Vector3d b0 = Vector3d::Random();
+
+ // Corrected delta
+ Vector5d delta_step = jacobian_bias * (b0 - b0_preint);
+ Vector5d delta = imu2d::plus(delta_biased, delta_step);
+
+ // x1 groundtruth
+ Vector5d x1;
+ x1 = imu2d::composeOverStateWithGravity(x0, delta, 1, g);
+
+ // Set states
+ frm0->setState(x0);
+ frm1->setState(x1);
+ sensor->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
+ sensor->fix();
+ sensor->getStateBlock('G')->unfix();
+ sensor->getStateBlock('G')->perturb(1);
+ frm0->fix();
+ frm1->fix();
+
+ // solve
+ std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
+
+ ASSERT_MATRIX_APPROX(sensor->getStateBlock('G')->getState(), g, 1e-6);
+
+ // remove feature (and factor) for the next loop
+ fea1->remove();
+ }
}
int main(int argc, char **argv)