Skip to content
Snippets Groups Projects
Commit 61ba84de authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Remove bias drift from Imu2d factors

parent 004b759f
No related branches found
No related tags found
2 merge requests!54devel->main,!53Resolve "Remove bias drift from FactorImu"
Hide whitespace changes
Inline Side-by-side
include/imu/factor/factor_imu2d.h
+ 7
− 28
View file @ 61ba84de
......@@ -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;
}
......
include/imu/factor/factor_imu2d_with_gravity.h
+ 6
− 27
View file @ 61ba84de
......@@ -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;
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment