Skip to content
Snippets Groups Projects

Resolve "Remove bias drift from FactorImu"

Merged Resolve "Remove bias drift from FactorImu"
14-remove-bias-drift-from-factorimu into devel
Merged Joan Solà Ortega requested to merge 14-remove-bias-drift-from-factorimu into devel
Compare
and
12 files
+ 909
− 1062
Compare changes
  • Side-by-side
  • Inline
Files
12
include/imu/factor/factor_imu.h
+ 7
− 43
@@ -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 ///////////////////////////////////