Moved inner class to cpp file, and use fixed-size matrices now for faster math.
dellaert
parent
d00a5b19ab
commit
7683917558
|
|
@ -0,0 +1,159 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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
|
||||
* @date July 2014
|
||||
**/
|
||||
|
||||
#include <gtsam/navigation/AHRSFactor.h>
|
||||
|
||||
/* GTSAM includes */
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
|
||||
/* External or standard includes */
|
||||
#include <ostream>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
// Inner class PreintegratedMeasurements
|
||||
//------------------------------------------------------------------------------
|
||||
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias, const Matrix3& measuredOmegaCovariance) :
|
||||
biasHat_(bias), deltaTij_(0.0) {
|
||||
measurementCovariance_ << measuredOmegaCovariance;
|
||||
delRdelBiasOmega_.setZero();
|
||||
PreintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
|
||||
biasHat_(imuBias::ConstantBias()), deltaTij_(0.0) {
|
||||
measurementCovariance_.setZero();
|
||||
delRdelBiasOmega_.setZero();
|
||||
delRdelBiasOmega_.setZero();
|
||||
PreintMeasCov_.setZero();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const {
|
||||
std::cout << s << std::endl;
|
||||
biasHat_.print(" biasHat");
|
||||
deltaRij_.print(" deltaRij ");
|
||||
std::cout << " measurementCovariance [" << measurementCovariance_ << " ]"
|
||||
<< std::endl;
|
||||
std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
bool AHRSFactor::PreintegratedMeasurements::equals(
|
||||
const PreintegratedMeasurements& expected, double tol) const {
|
||||
return biasHat_.equals(expected.biasHat_, tol)
|
||||
&& equal_with_abs_tol(measurementCovariance_,
|
||||
expected.measurementCovariance_, tol)
|
||||
&& deltaRij_.equals(expected.deltaRij_, tol)
|
||||
&& std::fabs(deltaTij_ - expected.deltaTij_) < tol
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void AHRSFactor::PreintegratedMeasurements::resetIntegration() {
|
||||
deltaRij_ = Rot3();
|
||||
deltaTij_ = 0.0;
|
||||
delRdelBiasOmega_.setZero();
|
||||
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 = biasHat_.correctGyroscope(measuredOmega);
|
||||
|
||||
// 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 Rincr = Rot3::Expmap(theta_incr);
|
||||
|
||||
// Right jacobian computed at theta_incr
|
||||
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr);
|
||||
|
||||
// Update Jacobians
|
||||
// ---------------------------------------------------------------------------
|
||||
delRdelBiasOmega_ = Rincr.transpose() * delRdelBiasOmega_
|
||||
- Jr_theta_incr * deltaT;
|
||||
|
||||
// Update preintegrated measurements covariance
|
||||
// ---------------------------------------------------------------------------
|
||||
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
|
||||
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i);
|
||||
|
||||
Rot3 Rot_j = deltaRij_ * Rincr;
|
||||
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization 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 * Rincr.transpose() * Jr_theta_i;
|
||||
// analytic expression corresponding to the following numerical derivative
|
||||
// Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
|
||||
|
||||
// overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
Matrix3 F;
|
||||
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;
|
||||
|
||||
// Update preintegrated measurements
|
||||
// ---------------------------------------------------------------------------
|
||||
deltaRij_ = deltaRij_ * Rincr;
|
||||
deltaTij_ += deltaT;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector AHRSFactor::PreintegratedMeasurements::PreIntegrateIMUObservations_delta_angles(
|
||||
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);
|
||||
}
|
||||
|
||||
} //namespace gtsam
|
||||
|
|
@ -11,7 +11,9 @@
|
|||
|
||||
/**
|
||||
* @file AHRSFactor.h
|
||||
* @author Krunal Chande, Luca Carlone
|
||||
* @author Krunal Chande
|
||||
* @author Luca Carlone
|
||||
* @date July 2014
|
||||
**/
|
||||
|
||||
#pragma once
|
||||
|
|
@ -20,7 +22,6 @@
|
|||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/base/LieVector.h>
|
||||
#include <gtsam/base/debug.h>
|
||||
|
||||
/* External or standard includes */
|
||||
|
|
@ -37,138 +38,56 @@ public:
|
|||
/** CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates)
|
||||
* and the corresponding covariance matrix. The measurements are then used to build the Preintegrated AHRS factor*/
|
||||
class PreintegratedMeasurements {
|
||||
|
||||
friend class AHRSFactor;
|
||||
|
||||
protected:
|
||||
imuBias::ConstantBias biasHat_;///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer
|
||||
Matrix measurementCovariance_;///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3)
|
||||
Matrix3 measurementCovariance_;///< (Raw measurements uncertainty) Covariance of the vector [measuredOmega] in R^(3X3)
|
||||
|
||||
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
|
||||
double deltaTij_; ///< Time interval from i to j
|
||||
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
|
||||
Matrix PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
|
||||
public:
|
||||
/** Default constructor, initialize with no measurements */
|
||||
PreintegratedMeasurements(
|
||||
const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
|
||||
const Matrix3& measuredOmegaCovariance ///< Covariance matrix of measured angular rate
|
||||
) : biasHat_(bias), measurementCovariance_(3,3), deltaTij_(0.0),
|
||||
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(3,3) {
|
||||
measurementCovariance_ <<measuredOmegaCovariance;
|
||||
PreintMeasCov_ = Matrix::Zero(3,3);
|
||||
}
|
||||
Matrix3 PreintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
|
||||
|
||||
PreintegratedMeasurements() :
|
||||
biasHat_(imuBias::ConstantBias()), measurementCovariance_(Matrix::Zero(3,3)), deltaTij_(0.0),
|
||||
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(3,3)) {}
|
||||
public:
|
||||
|
||||
/// Default constructor
|
||||
PreintegratedMeasurements();
|
||||
|
||||
/// Default constructor, initialize with no measurements
|
||||
PreintegratedMeasurements(const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
|
||||
const Matrix3& measuredOmegaCovariance ///< Covariance matrix of measured angular rate
|
||||
);
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s = "Preintegrated Measurements: ") const {
|
||||
std::cout << s << std::endl;
|
||||
biasHat_.print(" biasHat");
|
||||
deltaRij_.print(" deltaRij ");
|
||||
std::cout << " measurementCovariance [" << measurementCovariance_ << " ]"
|
||||
<< std::endl;
|
||||
std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl;
|
||||
}
|
||||
void print(const std::string& s = "Preintegrated Measurements: ") const;
|
||||
|
||||
/** equals */
|
||||
bool equals(const PreintegratedMeasurements& expected,
|
||||
double tol = 1e-9) const {
|
||||
return biasHat_.equals(expected.biasHat_, tol)
|
||||
&& equal_with_abs_tol(measurementCovariance_,
|
||||
expected.measurementCovariance_, tol)
|
||||
&& deltaRij_.equals(expected.deltaRij_, tol)
|
||||
&& std::fabs(deltaTij_ - expected.deltaTij_) < tol
|
||||
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_,
|
||||
tol);
|
||||
}
|
||||
double tol = 1e-9) const;
|
||||
|
||||
Matrix measurementCovariance() const {return measurementCovariance_;}
|
||||
Matrix deltaRij() const {return deltaRij_.matrix();}
|
||||
double deltaTij() const {return deltaTij_;}
|
||||
Vector biasHat() const {return biasHat_.vector();}
|
||||
Matrix3 delRdelBiasOmega() {return delRdelBiasOmega_;}
|
||||
Matrix PreintMeasCov() {return PreintMeasCov_;}
|
||||
void resetIntegration() {
|
||||
deltaRij_ = Rot3();
|
||||
deltaTij_ = 0.0;
|
||||
delRdelBiasOmega_ = Matrix3::Zero();
|
||||
PreintMeasCov_ = Matrix::Zero(9, 9);
|
||||
}
|
||||
Matrix deltaRij() const {return deltaRij_.matrix();}
|
||||
double deltaTij() const {return deltaTij_;}
|
||||
Vector biasHat() const {return biasHat_.vector();}
|
||||
Matrix3 delRdelBiasOmega() const {return delRdelBiasOmega_;}
|
||||
Matrix PreintMeasCov() const {return PreintMeasCov_;}
|
||||
|
||||
/// TODO: Document
|
||||
void resetIntegration();
|
||||
|
||||
/** Add a single Gyroscope measurement to the preintegration. */
|
||||
void integrateMeasurement(
|
||||
const Vector3& measuredOmega, ///< Measured angular velocity (in body frame)
|
||||
const Vector3& measuredOmega, ///< Measured angular velocity (in body frame)
|
||||
double deltaT, ///< Time step
|
||||
boost::optional<const Pose3&> body_P_sensor = boost::none ///< Sensor frame
|
||||
) {
|
||||
);
|
||||
|
||||
// NOTE: order is important here because each update uses old values.
|
||||
// First we compensate the measurements for the bias
|
||||
Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega);
|
||||
|
||||
// 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();
|
||||
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
|
||||
// linear acceleration vector in the body frame
|
||||
}
|
||||
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
|
||||
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
|
||||
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
|
||||
|
||||
// Update Jacobians
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_
|
||||
- Jr_theta_incr * deltaT;
|
||||
|
||||
// Update preintegrated measurements covariance
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // parametrization of so(3)
|
||||
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i);
|
||||
|
||||
Rot3 Rot_j = deltaRij_ * Rincr;
|
||||
const Vector3 theta_j = Rot3::Logmap(Rot_j); // parametrization 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
|
||||
Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix()
|
||||
* Jr_theta_i;
|
||||
// analytic expression corresponding to the following numerical derivative
|
||||
// Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
|
||||
|
||||
// overall Jacobian wrt preintegrated measurements (df/dx)
|
||||
Matrix F(3, 3);
|
||||
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;
|
||||
|
||||
// Update preintegrated measurements
|
||||
/* ----------------------------------------------------------------------------------------------------------------------- */
|
||||
deltaRij_ = deltaRij_ * Rincr;
|
||||
deltaTij_ += deltaT;
|
||||
}
|
||||
|
||||
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
|
||||
// This function is only used for test purposes (compare numerical derivatives wrt analytic ones)
|
||||
static inline Vector PreIntegrateIMUObservations_delta_angles(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);
|
||||
}
|
||||
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
|
||||
static Vector PreIntegrateIMUObservations_delta_angles(
|
||||
const Vector& msr_gyro_t, const double msr_dt,
|
||||
const Vector3& delta_angles);
|
||||
|
||||
private:
|
||||
/** Serialization function */
|
||||
|
|
|
|||
Loading…
Reference in New Issue