From 9e004ff81f8c6705c9973a63da8883a4626a5c92 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 8 May 2025 17:58:06 +0200
Subject: [PATCH] working on imu bootstrap
---
src/processor/processor_imu_3d.cpp | 61 +++++++++++++++++++++---------
1 file changed, 43 insertions(+), 18 deletions(-)
diff --git a/src/processor/processor_imu_3d.cpp b/src/processor/processor_imu_3d.cpp
index dcb1fc58e..82ac9bd25 100644
--- a/src/processor/processor_imu_3d.cpp
+++ b/src/processor/processor_imu_3d.cpp
@@ -100,13 +100,13 @@ bool ProcessorImu3d::voteForKeyFrame() const
return false;
}
-CaptureMotionPtr ProcessorImu3d::emplaceCapture(const FrameBasePtr & _frame_own,
- const SensorBasePtr & _sensor,
- const TimeStamp & _ts,
- const VectorXd & _data,
- const MatrixXd & _data_cov,
- const VectorXd & _calib,
- const VectorXd & _calib_preint,
+CaptureMotionPtr ProcessorImu3d::emplaceCapture(const FrameBasePtr &_frame_own,
+ const SensorBasePtr &_sensor,
+ const TimeStamp &_ts,
+ const VectorXd &_data,
+ const MatrixXd &_data_cov,
+ const VectorXd &_calib,
+ const VectorXd &_calib_preint,
const CaptureBasePtr &_capture_origin)
{
auto cap_motion = std::static_pointer_cast<CaptureMotion>(
@@ -178,9 +178,9 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data,
const Eigen::MatrixXd &_data_cov,
const Eigen::VectorXd &_calib,
const double _dt,
- Eigen::VectorXd & _delta,
- Eigen::MatrixXd & _delta_cov,
- Eigen::MatrixXd & _jac_delta_calib) const
+ Eigen::VectorXd &_delta,
+ Eigen::MatrixXd &_delta_cov,
+ Eigen::MatrixXd &_jac_delta_calib) const
{
assert(_data.size() == data_size_ && "Wrong data size!");
@@ -219,7 +219,7 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data,
void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
const Eigen::VectorXd &_delta,
const double _dt,
- Eigen::VectorXd & _delta_preint_plus_delta) const
+ Eigen::VectorXd &_delta_preint_plus_delta) const
{
/* MATHS according to Sola-16
* Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2 = Dp + Dv*dt + Dq*dp if dp =
@@ -236,7 +236,7 @@ void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
void ProcessorImu3d::statePlusDelta(const VectorComposite &_x,
const Eigen::VectorXd &_delta,
const double _dt,
- VectorComposite & _x_plus_delta) const
+ VectorComposite &_x_plus_delta) const
{
assert(_delta.size() == 10 && "Wrong _delta vector size");
assert(_dt >= 0 && "Time interval _dt is negative!");
@@ -256,9 +256,9 @@ void ProcessorImu3d::statePlusDelta(const VectorComposite &_x,
void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
const Eigen::VectorXd &_delta,
const double _dt,
- Eigen::VectorXd & _delta_preint_plus_delta,
- Eigen::MatrixXd & _jacobian_delta_preint,
- Eigen::MatrixXd & _jacobian_delta) const
+ Eigen::VectorXd &_delta_preint_plus_delta,
+ Eigen::MatrixXd &_jacobian_delta_preint,
+ Eigen::MatrixXd &_jacobian_delta) const
{
/*
* Expression of the delta integration step, D' = D (+) d:
@@ -385,6 +385,32 @@ void ProcessorImu3d::bootstrap()
// Transform problem to new reference
getProblem()->transform(transfo_w_l);
+ // Compute bias initialization
+ Vector6d bias_init;
+ // Gyro bias is directly the gyro readings (ideally average, implented for the last capture)
+ bias_init.tail<3>() = last_ptr_->getBuffer().back().data_.tail<3>();
+ // Acc bias is not observable, computing a warm start
+ // Bias that provide exactly 'g' at the estimated orientation
+ bias_init.head<3>() = Vector3d::Zero(); // last_ptr_->getBuffer().back().data_.head<3>() - q_w_l * g_w;
+ WOLF_DEBUG("////////////////////////////////////////////////////");
+ WOLF_DEBUG("q_l_s: ", q_l_s.coeffs().transpose());
+ WOLF_DEBUG("g_l: ", g_l.transpose());
+ WOLF_DEBUG("q_w_l * g_w: ", (q_w_l * g_w).transpose());
+ WOLF_DEBUG("last accel: ", last_ptr_->getBuffer().back().data_.head<3>().transpose());
+ WOLF_DEBUG("bias init: ", bias_init.transpose());
+
+ // Set biases to the IMU
+ // static bias case
+ if (getSensor()->isStateBlockDynamic('I')) first_capture->getSensorIntrinsic()->setState(bias_init);
+ // dynamic bias case
+ else
+ // set for all captures in the bootstrap
+ for (auto &capture : bootstrap_factor_list_)
+ {
+ auto cap_imu = std::static_pointer_cast<CaptureImu>(capture->getCapture());
+ cap_imu->getSensorIntrinsic()->setState(bias_init);
+ }
+
// Activate factors that were inactive during bootstrap
while (not bootstrap_factor_list_.empty())
{
@@ -395,7 +421,6 @@ void ProcessorImu3d::bootstrap()
// Clear bootstrapping flag. This marks the end of the bootstrapping process
bootstrapping_ = false;
}
-
break;
case BootstrapMethod::BOOTSTRAP_G:
@@ -535,8 +560,8 @@ bool ProcessorImu3d::recomputeStates() const
const auto &cap_origin = cap->getOriginCapture();
const auto &frm_origin = cap_origin->getFrame();
const auto &delta = VectorComposite({{'P', ftr->getMeasurement().head<3>()},
- {'O', ftr->getMeasurement().segment<4>(3)},
- {'V', ftr->getMeasurement().tail<3>()}});
+ {'O', ftr->getMeasurement().segment<4>(3)},
+ {'V', ftr->getMeasurement().tail<3>()}});
const auto &x_origin = frm_origin->getState();
auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp();
auto x = imu::composeOverState(x_origin, delta, dt);
--
GitLab