29 cout << (
s.empty() ?
s :
s +
"\n") << endl;
30 cout <<
"gyroscopeCovariance:\n[\n" << gyroscopeCovariance <<
"\n]" << endl;
32 cout <<
"omegaCoriolis = (" << omegaCoriolis->transpose() <<
")" << endl;
36 bool PreintegratedRotationParams::equals(
39 if (!
other.body_P_sensor
44 if (!
other.omegaCoriolis
51 void PreintegratedRotation::resetIntegration() {
54 delRdelBiasOmega_ = Z_3x3;
59 cout <<
" deltaTij [" << deltaTij_ <<
"]" << endl;
60 cout <<
" deltaRij.ypr = (" << deltaRij_.ypr().transpose() <<
")" << endl;
65 return this->matchesParamsWith(
other)
66 && deltaRij_.equals(
other.deltaRij_,
tol)
80 if (p_->body_P_sensor) {
81 Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix();
83 correctedOmega = body_R_sensor * correctedOmega;
89 return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega);
96 Matrix3 D_incrR_integratedOmega;
98 D_incrR_integratedOmega);
101 if (optional_D_incrR_integratedOmega) {
102 *optional_D_incrR_integratedOmega << D_incrR_integratedOmega;
107 deltaRij_ = deltaRij_.compose(incrR,
F);
110 const Matrix3 incrRt = incrR.
transpose();
111 delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
112 - D_incrR_integratedOmega *
deltaT;
115 Rot3 PreintegratedRotation::biascorrectedDeltaRij(
const Vector3& biasOmegaIncr,
117 const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasOmegaIncr;
118 const Rot3 deltaRij_biascorrected = deltaRij_.
expmap(biasInducedOmega,{},
H);
120 (*H) *= delRdelBiasOmega_;
121 return deltaRij_biascorrected;
124 Vector3 PreintegratedRotation::integrateCoriolis(
const Rot3& rot_i)
const {
125 if (!p_->omegaCoriolis)
126 return Vector3::Zero();
127 return rot_i.
transpose() * (*p_->omegaCoriolis) * deltaTij_;