146 lines
4.8 KiB
C++
146 lines
4.8 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file PreintegratedRotation.cpp
|
|
* @author Luca Carlone
|
|
* @author Stephen Williams
|
|
* @author Richard Roberts
|
|
* @author Vadim Indelman
|
|
* @author David Jensen
|
|
* @author Frank Dellaert
|
|
**/
|
|
|
|
#include "PreintegratedRotation.h"
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
|
|
void PreintegratedRotationParams::print(const string& s) const {
|
|
cout << (s.empty() ? s : s + "\n") << endl;
|
|
cout << "gyroscopeCovariance:\n" << gyroscopeCovariance << endl;
|
|
if (omegaCoriolis)
|
|
cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl;
|
|
if (body_P_sensor) body_P_sensor->print("body_P_sensor");
|
|
}
|
|
|
|
bool PreintegratedRotationParams::equals(
|
|
const PreintegratedRotationParams& other, double tol) const {
|
|
if (body_P_sensor) {
|
|
if (!other.body_P_sensor
|
|
|| !assert_equal(*body_P_sensor, *other.body_P_sensor, tol))
|
|
return false;
|
|
}
|
|
if (omegaCoriolis) {
|
|
if (!other.omegaCoriolis
|
|
|| !equal_with_abs_tol(*omegaCoriolis, *other.omegaCoriolis, tol))
|
|
return false;
|
|
}
|
|
return equal_with_abs_tol(gyroscopeCovariance, other.gyroscopeCovariance, tol);
|
|
}
|
|
|
|
void PreintegratedRotation::resetIntegration() {
|
|
deltaTij_ = 0.0;
|
|
deltaRij_ = Rot3();
|
|
delRdelBiasOmega_ = Z_3x3;
|
|
}
|
|
|
|
void PreintegratedRotation::print(const string& s) const {
|
|
cout << s;
|
|
cout << " deltaTij [" << deltaTij_ << "]" << endl;
|
|
cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl;
|
|
}
|
|
|
|
bool PreintegratedRotation::equals(const PreintegratedRotation& other,
|
|
double tol) const {
|
|
return this->matchesParamsWith(other)
|
|
&& deltaRij_.equals(other.deltaRij_, tol)
|
|
&& std::abs(deltaTij_ - other.deltaTij_) < tol
|
|
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
|
|
}
|
|
|
|
namespace internal {
|
|
Rot3 IncrementalRotation::operator()(
|
|
const Vector3& bias, OptionalJacobian<3, 3> H_bias) const {
|
|
// First we compensate the measurements for the bias
|
|
Vector3 correctedOmega = measuredOmega - bias;
|
|
|
|
// Then compensate for sensor-body displacement: we express the quantities
|
|
// (originally in the IMU frame) into the body frame.
|
|
// Note that the rotate Jacobian is just body_P_sensor->rotation().matrix().
|
|
if (body_P_sensor) {
|
|
// rotation rate vector in the body frame
|
|
correctedOmega = body_P_sensor->rotation() * correctedOmega;
|
|
}
|
|
|
|
// rotation vector describing rotation increment computed from the
|
|
// current rotation rate measurement
|
|
const Vector3 integratedOmega = correctedOmega * deltaT;
|
|
Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias); // expensive !!
|
|
if (H_bias) {
|
|
*H_bias *= -deltaT; // Correct so accurately reflects bias derivative
|
|
if (body_P_sensor) *H_bias *= body_P_sensor->rotation().matrix();
|
|
}
|
|
return incrR;
|
|
}
|
|
} // namespace internal
|
|
|
|
void PreintegratedRotation::integrateGyroMeasurement(
|
|
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
|
|
OptionalJacobian<3, 3> F) {
|
|
Matrix3 H_bias;
|
|
internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
|
|
const Rot3 incrR = f(biasHat, H_bias);
|
|
|
|
// Update deltaTij and rotation
|
|
deltaTij_ += deltaT;
|
|
deltaRij_ = deltaRij_.compose(incrR, F);
|
|
|
|
// Update Jacobian
|
|
const Matrix3 incrRt = incrR.transpose();
|
|
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + H_bias;
|
|
}
|
|
|
|
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
|
|
void PreintegratedRotation::integrateMeasurement(
|
|
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
|
|
OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
|
|
OptionalJacobian<3, 3> F) {
|
|
integrateGyroMeasurement(measuredOmega, biasHat, deltaT, F);
|
|
|
|
// If asked, pass obsolete Jacobians as well
|
|
if (optional_D_incrR_integratedOmega) {
|
|
Matrix3 H_bias;
|
|
internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
|
|
const Rot3 incrR = f(biasHat, H_bias);
|
|
*optional_D_incrR_integratedOmega << H_bias / -deltaT;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
|
|
OptionalJacobian<3, 3> H) const {
|
|
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasOmegaIncr;
|
|
const Rot3 deltaRij_biascorrected = deltaRij_.expmap(biasInducedOmega,{}, H);
|
|
if (H)
|
|
(*H) *= delRdelBiasOmega_;
|
|
return deltaRij_biascorrected;
|
|
}
|
|
|
|
Vector3 PreintegratedRotation::integrateCoriolis(const Rot3& rot_i) const {
|
|
if (!p_->omegaCoriolis)
|
|
return Vector3::Zero();
|
|
return rot_i.transpose() * (*p_->omegaCoriolis) * deltaTij_;
|
|
}
|
|
|
|
} // namespace gtsam
|