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;
}