12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Moved inner class to cpp file, and use fixed-size matrices now for faster math.

release/4.3a0
dellaert 2014-11-23 11:17:45 +01:00
parent d00a5b19ab
commit 7683917558
2 changed files with 191 additions and 113 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

159
gtsam/navigation/AHRSFactor.cpp Normal file
View File

@ -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

145
gtsam/navigation/AHRSFactor.h
View File

@ -11,7 +11,9 @@
/** /**
* @file AHRSFactor.h * @file AHRSFactor.h
* @author Krunal Chande, Luca Carlone * @author Krunal Chande
* @author Luca Carlone
* @date July 2014
**/ **/
#pragma once #pragma once
@ -20,7 +22,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
#include <gtsam/navigation/ImuBias.h> #include <gtsam/navigation/ImuBias.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
/* External or standard includes */ /* External or standard includes */
@ -37,138 +38,56 @@ public:
/** CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) /** CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates)
* and the corresponding covariance matrix. The measurements are then used to build the Preintegrated AHRS factor*/ * and the corresponding covariance matrix. The measurements are then used to build the Preintegrated AHRS factor*/
class PreintegratedMeasurements { class PreintegratedMeasurements {
friend class AHRSFactor; friend class AHRSFactor;
protected: protected:
imuBias::ConstantBias biasHat_;///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer 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) Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
double deltaTij_; ///< Time interval from i to j double deltaTij_; ///< Time interval from i to j
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias 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*) Matrix3 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);
}
PreintegratedMeasurements() : public:
biasHat_(imuBias::ConstantBias()), measurementCovariance_(Matrix::Zero(3,3)), deltaTij_(0.0),
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(3,3)) {} /// 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 */ /** print */
void print(const std::string& s = "Preintegrated Measurements: ") const { 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;
}
/** equals */ /** equals */
bool equals(const PreintegratedMeasurements& expected, bool equals(const PreintegratedMeasurements& expected,
double tol = 1e-9) const { 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);
}
Matrix measurementCovariance() const {return measurementCovariance_;} Matrix measurementCovariance() const {return measurementCovariance_;}
Matrix deltaRij() const {return deltaRij_.matrix();} Matrix deltaRij() const {return deltaRij_.matrix();}
double deltaTij() const {return deltaTij_;} double deltaTij() const {return deltaTij_;}
Vector biasHat() const {return biasHat_.vector();} Vector biasHat() const {return biasHat_.vector();}
Matrix3 delRdelBiasOmega() {return delRdelBiasOmega_;} Matrix3 delRdelBiasOmega() const {return delRdelBiasOmega_;}
Matrix PreintMeasCov() {return PreintMeasCov_;} Matrix PreintMeasCov() const {return PreintMeasCov_;}
void resetIntegration() {
deltaRij_ = Rot3(); /// TODO: Document
deltaTij_ = 0.0; void resetIntegration();
delRdelBiasOmega_ = Matrix3::Zero();
PreintMeasCov_ = Matrix::Zero(9, 9);
}
/** Add a single Gyroscope measurement to the preintegration. */ /** Add a single Gyroscope measurement to the preintegration. */
void integrateMeasurement( void integrateMeasurement(
const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) const Vector3& measuredOmega, ///< Measured angular velocity (in body frame)
double deltaT, ///< Time step double deltaT, ///< Time step
boost::optional<const Pose3&> body_P_sensor = boost::none ///< Sensor frame 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) // 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, static Vector PreIntegrateIMUObservations_delta_angles(
const Vector3& 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);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
private: private:
/** Serialization function */ /** Serialization function */