Skip to content
Snippets Groups Projects
Commit 9e004ff8 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

working on imu bootstrap

parent ce712f07
No related branches found
No related tags found
1 merge request!49Draft: Resolve "Adapt to new sensor constructors in core"
Hide whitespace changes
Inline Side-by-side
src/processor/processor_imu_3d.cpp
+ 43
− 18
View file @ 9e004ff8
...@@ -100,13 +100,13 @@ bool ProcessorImu3d::voteForKeyFrame() const ...@@ -100,13 +100,13 @@ bool ProcessorImu3d::voteForKeyFrame() const
return false; return false;
} }
CaptureMotionPtr ProcessorImu3d::emplaceCapture(const FrameBasePtr & _frame_own, CaptureMotionPtr ProcessorImu3d::emplaceCapture(const FrameBasePtr &_frame_own,
const SensorBasePtr & _sensor, const SensorBasePtr &_sensor,
const TimeStamp & _ts, const TimeStamp &_ts,
const VectorXd & _data, const VectorXd &_data,
const MatrixXd & _data_cov, const MatrixXd &_data_cov,
const VectorXd & _calib, const VectorXd &_calib,
const VectorXd & _calib_preint, const VectorXd &_calib_preint,
const CaptureBasePtr &_capture_origin) const CaptureBasePtr &_capture_origin)
{ {
auto cap_motion = std::static_pointer_cast<CaptureMotion>( auto cap_motion = std::static_pointer_cast<CaptureMotion>(
...@@ -178,9 +178,9 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data, ...@@ -178,9 +178,9 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data,
const Eigen::MatrixXd &_data_cov, const Eigen::MatrixXd &_data_cov,
const Eigen::VectorXd &_calib, const Eigen::VectorXd &_calib,
const double _dt, const double _dt,
Eigen::VectorXd & _delta, Eigen::VectorXd &_delta,
Eigen::MatrixXd & _delta_cov, Eigen::MatrixXd &_delta_cov,
Eigen::MatrixXd & _jac_delta_calib) const Eigen::MatrixXd &_jac_delta_calib) const
{ {
assert(_data.size() == data_size_ && "Wrong data size!"); assert(_data.size() == data_size_ && "Wrong data size!");
...@@ -219,7 +219,7 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data, ...@@ -219,7 +219,7 @@ void ProcessorImu3d::computeCurrentDelta(const Eigen::VectorXd &_data,
void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint, void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
const Eigen::VectorXd &_delta, const Eigen::VectorXd &_delta,
const double _dt, const double _dt,
Eigen::VectorXd & _delta_preint_plus_delta) const Eigen::VectorXd &_delta_preint_plus_delta) const
{ {
/* MATHS according to Sola-16 /* MATHS according to Sola-16
* Dp' = Dp + Dv*dt + 1/2*Dq*(a-a_b)*dt^2 = Dp + Dv*dt + Dq*dp if dp = * 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, ...@@ -236,7 +236,7 @@ void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
void ProcessorImu3d::statePlusDelta(const VectorComposite &_x, void ProcessorImu3d::statePlusDelta(const VectorComposite &_x,
const Eigen::VectorXd &_delta, const Eigen::VectorXd &_delta,
const double _dt, const double _dt,
VectorComposite & _x_plus_delta) const VectorComposite &_x_plus_delta) const
{ {
assert(_delta.size() == 10 && "Wrong _delta vector size"); assert(_delta.size() == 10 && "Wrong _delta vector size");
assert(_dt >= 0 && "Time interval _dt is negative!"); assert(_dt >= 0 && "Time interval _dt is negative!");
...@@ -256,9 +256,9 @@ void ProcessorImu3d::statePlusDelta(const VectorComposite &_x, ...@@ -256,9 +256,9 @@ void ProcessorImu3d::statePlusDelta(const VectorComposite &_x,
void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint, void ProcessorImu3d::deltaPlusDelta(const Eigen::VectorXd &_delta_preint,
const Eigen::VectorXd &_delta, const Eigen::VectorXd &_delta,
const double _dt, const double _dt,
Eigen::VectorXd & _delta_preint_plus_delta, Eigen::VectorXd &_delta_preint_plus_delta,
Eigen::MatrixXd & _jacobian_delta_preint, Eigen::MatrixXd &_jacobian_delta_preint,
Eigen::MatrixXd & _jacobian_delta) const Eigen::MatrixXd &_jacobian_delta) const
{ {
/* /*
* Expression of the delta integration step, D' = D (+) d: * Expression of the delta integration step, D' = D (+) d:
...@@ -385,6 +385,32 @@ void ProcessorImu3d::bootstrap() ...@@ -385,6 +385,32 @@ void ProcessorImu3d::bootstrap()
// Transform problem to new reference // Transform problem to new reference
getProblem()->transform(transfo_w_l); 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 // Activate factors that were inactive during bootstrap
while (not bootstrap_factor_list_.empty()) while (not bootstrap_factor_list_.empty())
{ {
...@@ -395,7 +421,6 @@ void ProcessorImu3d::bootstrap() ...@@ -395,7 +421,6 @@ void ProcessorImu3d::bootstrap()
// Clear bootstrapping flag. This marks the end of the bootstrapping process // Clear bootstrapping flag. This marks the end of the bootstrapping process
bootstrapping_ = false; bootstrapping_ = false;
} }
break; break;
case BootstrapMethod::BOOTSTRAP_G: case BootstrapMethod::BOOTSTRAP_G:
...@@ -535,8 +560,8 @@ bool ProcessorImu3d::recomputeStates() const ...@@ -535,8 +560,8 @@ bool ProcessorImu3d::recomputeStates() const
const auto &cap_origin = cap->getOriginCapture(); const auto &cap_origin = cap->getOriginCapture();
const auto &frm_origin = cap_origin->getFrame(); const auto &frm_origin = cap_origin->getFrame();
const auto &delta = VectorComposite({{'P', ftr->getMeasurement().head<3>()}, const auto &delta = VectorComposite({{'P', ftr->getMeasurement().head<3>()},
{'O', ftr->getMeasurement().segment<4>(3)}, {'O', ftr->getMeasurement().segment<4>(3)},
{'V', ftr->getMeasurement().tail<3>()}}); {'V', ftr->getMeasurement().tail<3>()}});
const auto &x_origin = frm_origin->getState(); const auto &x_origin = frm_origin->getState();
auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp(); auto dt = cap->getTimeStamp() - cap_origin->getTimeStamp();
auto x = imu::composeOverState(x_origin, delta, dt); auto x = imu::composeOverState(x_origin, delta, dt);
......
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