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/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/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp
index b0b5c56497cadc61f0c83e0735be58952dc2dabe..45fcac1a5296a2a6edab149140fc57e7b9d4998e 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,