261 lines
9.9 KiB
C++
261 lines
9.9 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 AHRSFactor.cpp
|
|
* @author Krunal Chande
|
|
* @author Luca Carlone
|
|
* @author Frank Dellaert
|
|
* @date July 2014
|
|
**/
|
|
|
|
#include <gtsam/navigation/AHRSFactor.h>
|
|
#include <iostream>
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
|
|
//------------------------------------------------------------------------------
|
|
// Inner class PreintegratedMeasurements
|
|
//------------------------------------------------------------------------------
|
|
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements(
|
|
const Vector3& bias, const Matrix3& measuredOmegaCovariance) :
|
|
biasHat_(bias) {
|
|
measurementCovariance_ << measuredOmegaCovariance;
|
|
preintMeasCov_.setZero();
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
|
|
biasHat_(Vector3()) {
|
|
measurementCovariance_.setZero();
|
|
preintMeasCov_.setZero();
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void AHRSFactor::PreintegratedMeasurements::print(const string& s) const {
|
|
PreintegratedRotation::print(s);
|
|
cout << "biasHat [" << biasHat_.transpose() << "]" << endl;
|
|
cout << " measurementCovariance [" << measurementCovariance_ << " ]" << endl;
|
|
cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
bool AHRSFactor::PreintegratedMeasurements::equals(
|
|
const PreintegratedMeasurements& other, double tol) const {
|
|
return PreintegratedRotation::equals(other, tol)
|
|
&& equal_with_abs_tol(biasHat_, other.biasHat_, tol)
|
|
&& equal_with_abs_tol(measurementCovariance_,
|
|
other.measurementCovariance_, tol);
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void AHRSFactor::PreintegratedMeasurements::resetIntegration() {
|
|
PreintegratedRotation::resetIntegration();
|
|
preintMeasCov_.setZero();
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
|
|
const Vector3& measuredOmega, double deltaT,
|
|
boost::optional<const Pose3&> body_P_sensor) {
|
|
|
|
// NOTE: order is important here because each update uses old values.
|
|
// First we compensate the measurements for the bias
|
|
Vector3 correctedOmega = measuredOmega - biasHat_;
|
|
|
|
// Then compensate for sensor-body displacement: we express the quantities
|
|
// (originally in the IMU frame) into the body frame
|
|
if (body_P_sensor) {
|
|
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
|
|
// rotation rate vector in the body frame
|
|
correctedOmega = body_R_sensor * correctedOmega;
|
|
}
|
|
|
|
// rotation vector describing rotation increment computed from the
|
|
// current rotation rate measurement
|
|
const Vector3 theta_incr = correctedOmega * deltaT;
|
|
|
|
// rotation increment computed from the current rotation rate measurement
|
|
const Rot3 incrR = Rot3::Expmap(theta_incr); // expensive !!
|
|
const Matrix3 incrRt = incrR.transpose();
|
|
|
|
// Right Jacobian computed at theta_incr
|
|
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr);
|
|
|
|
// Update Jacobian
|
|
update_delRdelBiasOmega(Jr_theta_incr, incrR, deltaT);
|
|
|
|
// Update preintegrated measurements covariance
|
|
const Vector3 theta_i = thetaRij(); // super-expensive, Parameterization of so(3)
|
|
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i);
|
|
|
|
// Update rotation and deltaTij. TODO Frank moved from end of this function !!!
|
|
updateIntegratedRotationAndDeltaT(incrR, deltaT);
|
|
|
|
const Vector3 theta_j = thetaRij(); // super-expensive, , Parameterization of so(3)
|
|
const Matrix3 Jrinv_theta_j = Rot3::rightJacobianExpMapSO3inverse(theta_j);
|
|
|
|
// Update preintegrated measurements covariance: as in [2] we consider a first
|
|
// order propagation that can be seen as a prediction phase in an EKF framework
|
|
Matrix3 H_angles_angles = Jrinv_theta_j * incrRt * Jr_theta_i;
|
|
// analytic expression corresponding to the following numerical derivative
|
|
// Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>
|
|
// (boost::bind(&DeltaAngles, correctedOmega, deltaT, _1), thetaij);
|
|
|
|
// overall Jacobian wrpt preintegrated measurements (df/dx)
|
|
const Matrix3& F = H_angles_angles;
|
|
|
|
// first order uncertainty propagation
|
|
// the deltaT allows to pass from continuous time noise to discrete time noise
|
|
preintMeasCov_ = F * preintMeasCov_ * F.transpose()
|
|
+ measurementCovariance_ * deltaT;
|
|
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias,
|
|
boost::optional<Matrix&> H) const {
|
|
const Vector3 biasOmegaIncr = bias - biasHat_;
|
|
return biascorrectedThetaRij(biasOmegaIncr, H);
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles(
|
|
const Vector& msr_gyro_t, const double msr_dt,
|
|
const Vector3& delta_angles) {
|
|
|
|
// Note: all delta terms refer to an IMU\sensor system at t0
|
|
|
|
// Calculate the corrected measurements using the Bias object
|
|
Vector body_t_omega_body = msr_gyro_t;
|
|
|
|
Rot3 R_t_to_t0 = Rot3::Expmap(delta_angles);
|
|
|
|
R_t_to_t0 = R_t_to_t0 * Rot3::Expmap(body_t_omega_body * msr_dt);
|
|
return Rot3::Logmap(R_t_to_t0);
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
// AHRSFactor methods
|
|
//------------------------------------------------------------------------------
|
|
AHRSFactor::AHRSFactor() :
|
|
_PIM_(Vector3(), Matrix3::Zero()) {
|
|
}
|
|
|
|
AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
|
|
const PreintegratedMeasurements& preintegratedMeasurements,
|
|
const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) :
|
|
Base(
|
|
noiseModel::Gaussian::Covariance(
|
|
preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), _PIM_(
|
|
preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
|
|
body_P_sensor) {
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const {
|
|
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
|
|
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
void AHRSFactor::print(const string& s,
|
|
const KeyFormatter& keyFormatter) const {
|
|
cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
|
|
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ",";
|
|
_PIM_.print(" preintegrated measurements:");
|
|
cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
|
|
noiseModel_->print(" noise model: ");
|
|
if (body_P_sensor_)
|
|
body_P_sensor_->print(" sensor pose in body frame: ");
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
|
|
const This *e = dynamic_cast<const This*>(&other);
|
|
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
|
|
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
|
|
&& ((!body_P_sensor_ && !e->body_P_sensor_)
|
|
|| (body_P_sensor_ && e->body_P_sensor_
|
|
&& body_P_sensor_->equals(*e->body_P_sensor_)));
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
|
|
const Vector3& bias, boost::optional<Matrix&> H1,
|
|
boost::optional<Matrix&> H2, boost::optional<Matrix&> H3) const {
|
|
|
|
// Do bias correction, if (H3) will contain 3*3 derivative used below
|
|
const Vector3 theta_biascorrected = _PIM_.predict(bias, H3);
|
|
|
|
// Coriolis term
|
|
const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_);
|
|
const Matrix3 coriolisHat = skewSymmetric(coriolis);
|
|
const Vector3 theta_corrected = theta_biascorrected - coriolis;
|
|
|
|
// Prediction
|
|
const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected);
|
|
|
|
// Get error between actual and prediction
|
|
const Rot3 actualRij = Ri.between(Rj);
|
|
const Rot3 fRhat = deltaRij_corrected.between(actualRij);
|
|
Vector3 fR = Rot3::Logmap(fRhat);
|
|
|
|
// Terms common to derivatives
|
|
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected);
|
|
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(fR);
|
|
|
|
if (H1) {
|
|
// dfR/dRi
|
|
H1->resize(3, 3);
|
|
Matrix3 Jtheta = -Jr_theta_bcc * coriolisHat;
|
|
(*H1)
|
|
<< Jrinv_fRhat * (-actualRij.transpose() - fRhat.transpose() * Jtheta);
|
|
}
|
|
|
|
if (H2) {
|
|
// dfR/dPosej
|
|
H2->resize(3, 3);
|
|
(*H2) << Jrinv_fRhat * Matrix3::Identity();
|
|
}
|
|
|
|
if (H3) {
|
|
// dfR/dBias, note H3 contains derivative of predict
|
|
const Matrix3 JbiasOmega = Jr_theta_bcc * (*H3);
|
|
H3->resize(3, 3);
|
|
(*H3) << Jrinv_fRhat * (-fRhat.transpose() * JbiasOmega);
|
|
}
|
|
|
|
Vector error(3);
|
|
error << fR;
|
|
return error;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
|
|
const PreintegratedMeasurements preintegratedMeasurements,
|
|
const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) {
|
|
|
|
const Vector3 theta_biascorrected = preintegratedMeasurements.predict(bias);
|
|
|
|
// Coriolis term
|
|
const Vector3 coriolis = //
|
|
preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis);
|
|
|
|
const Vector3 theta_corrected = theta_biascorrected - coriolis;
|
|
const Rot3 deltaRij_corrected = Rot3::Expmap(theta_corrected);
|
|
|
|
return rot_i.compose(deltaRij_corrected);
|
|
}
|
|
|
|
} //namespace gtsam
|