From 04ab480e8c1dde2bff58535ece28a38c71cd9b3f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 2 Sep 2022 08:39:48 +0200
Subject: [PATCH] Add second factor with bias drift
---
include/imu/processor/processor_imu2d.h | 5 ++-
src/processor/processor_imu2d.cpp | 42 +++++++++++++++++++++++++
2 files changed, 46 insertions(+), 1 deletion(-)
diff --git a/include/imu/processor/processor_imu2d.h b/include/imu/processor/processor_imu2d.h
index 44f418140..f8f4a5478 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/src/processor/processor_imu2d.cpp b/src/processor/processor_imu2d.cpp
index 23fbbd222..10c6f25d5 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,
--
GitLab