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

Merged in feature/AHRS_Polish (pull request #43) 

Substantially cleaned up AHRS factor
release/4.3a0
Frank Dellaert 2014-11-24 17:38:45 +01:00
parent a9090fd7af 15bcb70260
commit f567f5a5dd
10 changed files with 510 additions and 407 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
.cproject
View File

@ -21,7 +21,7 @@
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="-j8" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder buildPath="${ProjDirPath}/build" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="4" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<option id="macosx.cpp.link.option.paths.451252615" name="Library search path (-L)" superClass="macosx.cpp.link.option.paths"/>
@ -178,16 +178,8 @@
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="gtsam.null.1344331286" name="gtsam"/>
</storageModule>
<storageModule moduleId="refreshScope" versionNumber="2">
<configuration configurationName="Timing">
<resource resourceType="PROJECT" workspacePath="/gtsam"/>
</configuration>
<configuration configurationName="fast">
<resource resourceType="PROJECT" workspacePath="/gtsam"/>
</configuration>
<configuration configurationName="MacOSX GCC">
<resource resourceType="PROJECT" workspacePath="/gtsam"/>
</configuration>
<storageModule moduleId="refreshScope" versionNumber="1">
<resource resourceType="PROJECT" workspacePath="/gtsam"/>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>

4
.project
View File

@ -23,7 +23,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildArguments</key>
<value>-j5</value>
<value>-j4</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>
@ -63,7 +63,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
<value>false</value>
<value>true</value>
</dictionary>
</arguments>
</buildCommand>

2
gtsam.h
View File

@ -2414,7 +2414,7 @@ class ImuFactorPreintegratedMeasurements {
Matrix delVdelBiasAcc() const;
Matrix delVdelBiasOmega() const;
Matrix delRdelBiasOmega() const;
Matrix PreintMeasCov() const;
Matrix preintMeasCov() const;
// Standard Interface

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

@ -0,0 +1,287 @@
/* ----------------------------------------------------------------------------
* 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>
/* External or standard includes */
#include <ostream>
namespace gtsam {
//------------------------------------------------------------------------------
// Inner class PreintegratedMeasurements
//------------------------------------------------------------------------------
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements(
const Vector3& bias, const Matrix3& measuredOmegaCovariance) :
biasHat_(bias), deltaTij_(0.0) {
measurementCovariance_ << measuredOmegaCovariance;
delRdelBiasOmega_.setZero();
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
biasHat_(Vector3()), 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;
std::cout << "biasHat [" << biasHat_.transpose() << "]" << std::endl;
deltaRij_.print(" deltaRij ");
std::cout << " measurementCovariance [" << measurementCovariance_ << " ]"
<< std::endl;
std::cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << std::endl;
}
//------------------------------------------------------------------------------
bool AHRSFactor::PreintegratedMeasurements::equals(
const PreintegratedMeasurements& other, double tol) const {
return equal_with_abs_tol(biasHat_, other.biasHat_, tol)
&& equal_with_abs_tol(measurementCovariance_,
other.measurementCovariance_, tol)
&& deltaRij_.equals(other.deltaRij_, tol)
&& std::fabs(deltaTij_ - other.deltaTij_) < tol
&& equal_with_abs_tol(delRdelBiasOmega_, other.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 = 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);
const Matrix3 incrRt = incrR.transpose();
// Right Jacobian computed at theta_incr
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr);
// Update Jacobians
// ---------------------------------------------------------------------------
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
// Update preintegrated measurements covariance
// ---------------------------------------------------------------------------
const Vector3 theta_i = Rot3::Logmap(deltaRij_); // Parameterization of so(3)
const Matrix3 Jr_theta_i = Rot3::rightJacobianExpMapSO3inverse(theta_i);
Rot3 Rot_j = deltaRij_ * incrR;
const Vector3 theta_j = Rot3::Logmap(Rot_j); // 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;
// Update preintegrated measurements
// ---------------------------------------------------------------------------
deltaRij_ = deltaRij_ * incrR;
deltaTij_ += deltaT;
}
//------------------------------------------------------------------------------
Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias,
boost::optional<Matrix&> H) const {
const Vector3 biasOmegaIncr = bias - biasHat_;
Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr;
const Rot3 deltaRij_biascorrected = deltaRij_.retract(
delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP);
const Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
if (H) {
const Matrix3 Jrinv_theta_bc = //
Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
const Matrix3 Jr_JbiasOmegaIncr = //
Rot3::rightJacobianExpMapSO3(delRdelBiasOmega_biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
}
return theta_biascorrected;
}
//------------------------------------------------------------------------------
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() :
preintegratedMeasurements_(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), preintegratedMeasurements_(
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 std::string& s,
const KeyFormatter& keyFormatter) const {
std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ",";
preintegratedMeasurements_.print(" preintegrated measurements:");
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]"
<< std::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)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, 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& rot_i, const Rot3& rot_j,
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 = //
preintegratedMeasurements_.predict(bias, H3);
// Coriolis term
const Vector3 coriolis = //
preintegratedMeasurements_.integrateCoriolis(rot_i, omegaCoriolis_);
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 = rot_i.between(rot_j);
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 * skewSymmetric(coriolis);
(*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

414
gtsam/navigation/AHRSFactor.h
View File

@ -11,166 +11,111 @@
/**
* @file AHRSFactor.h
* @author Krunal Chande, Luca Carlone
* @author Krunal Chande
* @author Luca Carlone
* @author Frank Dellaert
* @date July 2014
**/
#pragma once
/* GTSAM includes */
#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 */
#include <ostream>
#include <gtsam/geometry/Pose3.h>
namespace gtsam {
class AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, imuBias::ConstantBias> {
class AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
public:
/** Struct to store results of preintegrating IMU measurements. Can be build
* incrementally so as to avoid costly integration at time of factor construction. */
/** CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates)
* and the corresponding covariance matrix. The measurements are then used to build the Preintegrated AHRS factor*/
/**
* CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope
* measurements (rotation rates) and the corresponding covariance matrix.
* The measurements are then used to build the Preintegrated AHRS factor.
* Can be built incrementally so as to avoid costly integration at time of
* factor construction.
*/
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)
Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer
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*)
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);
/// Default constructor
PreintegratedMeasurements();
/**
* Default constructor, initialize with no measurements
* @param bias Current estimate of acceleration and rotation rate biases
* @param measuredOmegaCovariance Covariance matrix of measured angular rate
*/
PreintegratedMeasurements(const Vector3& bias,
const Matrix3& measuredOmegaCovariance);
/// print
void print(const std::string& s = "Preintegrated Measurements: ") const;
/// equals
bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const;
const Matrix3& measurementCovariance() const {
return measurementCovariance_;
}
Matrix3 deltaRij() const {
return deltaRij_.matrix();
}
double deltaTij() const {
return deltaTij_;
}
Vector3 biasHat() const {
return biasHat_;
}
const Matrix3& delRdelBiasOmega() const {
return delRdelBiasOmega_;
}
const Matrix3& preintMeasCov() const {
return preintMeasCov_;
}
PreintegratedMeasurements() :
biasHat_(imuBias::ConstantBias()), measurementCovariance_(Matrix::Zero(3,3)), deltaTij_(0.0),
delRdelBiasOmega_(Matrix3::Zero()), PreintMeasCov_(Matrix::Zero(3,3)) {}
/// TODO: Document
void resetIntegration();
/** 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;
/**
* Add a single Gyroscope measurement to the preintegration.
* @param measureOmedga Measured angular velocity (in body frame)
* @param deltaT Time step
* @param body_P_sensor Optional sensor frame
*/
void integrateMeasurement(const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor = boost::none);
/// Predict bias-corrected incremental rotation
/// TODO: The matrix Hbias is the derivative of predict? Unit-test?
Vector3 predict(const Vector3& bias, boost::optional<Matrix&> H =
boost::none) const;
/// Integrate coriolis correction in body frame rot_i
Vector3 integrateCoriolis(const Rot3& rot_i,
const Vector3& omegaCoriolis) const {
return rot_i.transpose() * omegaCoriolis * deltaTij_;
}
/** 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);
}
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);
}
/** Add a single Gyroscope measurement to the preintegration. */
void integrateMeasurement(
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);
}
/* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
// This function is only used for test purposes
// (compare numerical derivatives wrt analytic ones)
static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
@ -185,12 +130,12 @@ public:
private:
typedef AHRSFactor This;
typedef NoiseModelFactor3<Rot3, Rot3, imuBias::ConstantBias> Base;
typedef NoiseModelFactor3<Rot3, Rot3, Vector3> Base;
PreintegratedMeasurements preintegratedMeasurements_;
Vector3 gravity_;
Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
boost::optional<Pose3> body_P_sensor_;///< The pose of the sensor in the body frame
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
public:
@ -202,184 +147,57 @@ public:
#endif
/** Default constructor - only use for serialization */
AHRSFactor() :
preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero()) {}
AHRSFactor();
AHRSFactor(
Key rot_i, ///< previous rot key
Key rot_j, ///< current rot key
Key bias,///< previous bias key
const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated measurements
const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame
boost::optional<const Pose3&> body_P_sensor = boost::none ///< The Pose of the sensor frame in the body frame
) :
Base(
noiseModel::Gaussian::Covariance(
preintegratedMeasurements.PreintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_(
preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
body_P_sensor) {
/**
* Constructor
* @param rot_i previous rot key
* @param rot_j current rot key
* @param bias previous bias key
* @param preintegratedMeasurements preintegrated measurements
* @param omegaCoriolis rotation rate of the inertial frame
* @param body_P_sensor Optional pose of the sensor frame in the body frame
*/
AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none);
virtual ~AHRSFactor() {
}
virtual ~AHRSFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(
new This(*this)
)
);
}
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
/** implement functions needed for Testable */
/** print */
/// print
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3())
<< ",";
preintegratedMeasurements_.print(" preintegrated measurements:");
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]"
<< std::endl;
this->noiseModel_->print(" noise model: ");
if (this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
DefaultKeyFormatter) const;
/** equals */
virtual bool equals(const NonlinearFactor& expected,
double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, 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_)));
}
/** Access the preintegrated measurements. */
/// equals
virtual bool equals(const NonlinearFactor&, double tol = 1e-9) const;
/// Access the preintegrated measurements.
const PreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_;
}
const Vector3& omegaCoriolis() const {
return omegaCoriolis_;
}
/** implement functions needed to derive from Factor */
/** vector of errors */
/// vector of errors
Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
const imuBias::ConstantBias& bias,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const
{
const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::none) const;
double deltaTij = preintegratedMeasurements_.deltaTij_;
Vector3 biasOmegaIncr = bias.gyroscope()
- preintegratedMeasurements_.biasHat_.gyroscope();
// We compute factor's Jacobians
/* ---------------------------------------------------------------------------------------------------- */
Rot3 deltaRij_biascorrected =
preintegratedMeasurements_.deltaRij_.retract(
preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr,
Rot3::EXPMAP);
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected
- rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij; // Coriolis term
Rot3 deltaRij_biascorrected_corioliscorrected = Rot3::Expmap(
theta_biascorrected_corioliscorrected);
Rot3 fRhat = deltaRij_biascorrected_corioliscorrected.between(
rot_i.between(rot_j));
Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(
theta_biascorrected_corioliscorrected);
Matrix3 Jtheta = -Jr_theta_bcc
* skewSymmetric(rot_i.inverse().matrix() * omegaCoriolis_ * deltaTij);
Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(
Rot3::Logmap(fRhat));
if (H1) {
H1->resize(3, 3);
(*H1) << // dfR/dRi
Jrinv_fRhat
* (-rot_j.between(rot_i).matrix()
- fRhat.inverse().matrix() * Jtheta);
}
if(H2) {
H2->resize(3,3);
(*H2) <<
// dfR/dPosej
Jrinv_fRhat * ( Matrix3::Identity() );
}
if (H3) {
Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(
theta_biascorrected);
Matrix3 Jr_JbiasOmegaIncr = Rot3::rightJacobianExpMapSO3(
preintegratedMeasurements_.delRdelBiasOmega_ * biasOmegaIncr);
Matrix3 JbiasOmega = Jr_theta_bcc * Jrinv_theta_bc
* Jr_JbiasOmegaIncr * preintegratedMeasurements_.delRdelBiasOmega_;
H3->resize(3, 6);
(*H3) <<
// dfR/dBias
Matrix::Zero(3,3),
Jrinv_fRhat * (-fRhat.inverse().matrix() * JbiasOmega);
}
Vector3 fR = Rot3::Logmap(fRhat);
Vector r(3);
r << fR;
return r;
}
/** predicted states from IMU */
static Rot3 predict(const Rot3& rot_i,
const imuBias::ConstantBias& bias,
/// predicted states from IMU
static Rot3 predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none
) {
const double& deltaTij = preintegratedMeasurements.deltaTij_;
// const Vector3 biasAccIncr = bias.accelerometer()
- preintegratedMeasurements.biasHat_.accelerometer();
const Vector3 biasOmegaIncr = bias.gyroscope()
- preintegratedMeasurements.biasHat_.gyroscope();
const Rot3 Rot_i = rot_i;
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
const Rot3 deltaRij_biascorrected =
preintegratedMeasurements.deltaRij_.retract(
preintegratedMeasurements.delRdelBiasOmega_ * biasOmegaIncr,
Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)
Vector3 theta_biascorrected = Rot3::Logmap(deltaRij_biascorrected);
Vector3 theta_biascorrected_corioliscorrected = theta_biascorrected
- Rot_i.inverse().matrix() * omegaCoriolis * deltaTij; // Coriolis term
const Rot3 deltaRij_biascorrected_corioliscorrected = Rot3::Expmap(
theta_biascorrected_corioliscorrected);
// const Rot3 Rot_j =
return (Rot_i.compose(deltaRij_biascorrected_corioliscorrected));
}
boost::optional<const Pose3&> body_P_sensor = boost::none);
private:
@ -388,12 +206,16 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
& boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(preintegratedMeasurements_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
}; // AHRSFactor
};
// AHRSFactor
typedef AHRSFactor::PreintegratedMeasurements AHRSFactorPreintegratedMeasurements;
} //namespace gtsam

16
gtsam/navigation/ImuBias.h
View File

@ -57,8 +57,8 @@ namespace imuBias {
}
/** return the accelerometer and gyro biases in a single vector */
Vector vector() const {
Vector v(6);
Vector6 vector() const {
Vector6 v;
v << biasAcc_, biasGyro_;
return v;
}
@ -70,18 +70,18 @@ namespace imuBias {
const Vector3& gyroscope() const { return biasGyro_; }
/** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */
Vector correctAccelerometer(const Vector3& measurement, boost::optional<Matrix&> H=boost::none) const {
if (H){
H->resize(3,6);
Vector3 correctAccelerometer(const Vector3& measurement, boost::optional<Matrix&> H=boost::none) const {
if (H) {
H->resize(3, 6);
(*H) << -Matrix3::Identity(), Matrix3::Zero();
}
return measurement - biasAcc_;
}
/** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */
Vector correctGyroscope(const Vector3& measurement, boost::optional<Matrix&> H=boost::none) const {
if (H){
H->resize(3,6);
Vector3 correctGyroscope(const Vector3& measurement, boost::optional<Matrix&> H=boost::none) const {
if (H) {
H->resize(3, 6);
(*H) << Matrix3::Zero(), -Matrix3::Identity();
}
return measurement - biasGyro_;

2
gtsam/navigation/ImuFactor.h
View File

@ -147,7 +147,7 @@ struct PoseVelocity {
Matrix delVdelBiasAcc() const { return delVdelBiasAcc_;}
Matrix delVdelBiasOmega() const { return delVdelBiasOmega_;}
Matrix delRdelBiasOmega() const{ return delRdelBiasOmega_;}
Matrix PreintMeasCov() const { return PreintMeasCov_;}
Matrix preintMeasCov() const { return PreintMeasCov_;}
void resetIntegration(){

175
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -10,18 +10,18 @@
* -------------------------------------------------------------------------- */
/**
* @file testImuFactor.cpp
* @brief Unit test for ImuFactor
* @author Krunal Chande, Luca Carlone
* @file testAHRSFactor.cpp
* @brief Unit test for AHRSFactor
* @author Krunal Chande
* @author Luca Carlone
* @author Frank Dellaert
*/
#include <gtsam/navigation/AHRSFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/debug.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/bind.hpp>
@ -35,23 +35,22 @@ using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;
/* ************************************************************************* */
//******************************************************************************
namespace {
Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i,
const Rot3 rot_j, const imuBias::ConstantBias& bias) {
const Rot3 rot_j, const Vector3& bias) {
return factor.evaluateError(rot_i, rot_j, bias);
}
Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i,
const Rot3 rot_j, const imuBias::ConstantBias& bias) {
const Rot3 rot_j, const Vector3& bias) {
return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3));
}
AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredOmegas,
const Vector3& bias, const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) {
const Vector3& initialRotationRate = Vector3()) {
AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity());
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@ -64,12 +63,12 @@ AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
}
Rot3 evaluatePreintegratedMeasurementsRotation(
const imuBias::ConstantBias& bias,
const list<Vector3>& measuredOmegas,
const Vector3& bias, const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) {
return Rot3(evaluatePreintegratedMeasurements(bias, measuredOmegas,
deltaTs, initialRotationRate).deltaRij());
const Vector3& initialRotationRate = Vector3::Zero()) {
return Rot3(
evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
initialRotationRate).deltaRij());
}
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
@ -82,10 +81,10 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
}
}
/* ************************************************************************* */
//******************************************************************************
TEST( AHRSFactor, PreintegratedMeasurements ) {
// Linearization point
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases
Vector3 bias(0,0,0); ///< Current estimate of angular rate bias
// Measurements
Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
@ -117,7 +116,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
/* ************************************************************************* */
TEST(AHRSFactor, Error) {
// Linearization point
imuBias::ConstantBias bias; // Bias
Vector3 bias; // Bias
Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0));
@ -133,8 +132,7 @@ TEST(AHRSFactor, Error) {
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
// Create factor
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis,
false);
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, false);
Vector errorActual = factor.evaluateError(x1, x2, bias);
@ -148,7 +146,7 @@ TEST(AHRSFactor, Error) {
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, imuBias::ConstantBias>(
Matrix H3e = numericalDerivative11<Vector, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
// Check rotation Jacobians
@ -162,21 +160,24 @@ TEST(AHRSFactor, Error) {
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
// rotations
EXPECT(assert_equal(RH1e, H1a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
EXPECT(assert_equal(RH1e, H1a, 1e-5));
// 1e-5 needs to be added only when using quaternions for rotations
EXPECT(assert_equal(H2e, H2a, 1e-5));
// rotations
EXPECT(assert_equal(RH2e, H2a, 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
EXPECT(assert_equal(RH2e, H2a, 1e-5));
// 1e-5 needs to be added only when using quaternions for rotations
EXPECT(assert_equal(H3e, H3a, 1e-5)); // FIXME!! DOes not work. Different matrix dimensions.
EXPECT(assert_equal(H3e, H3a, 1e-5));
// FIXME!! DOes not work. Different matrix dimensions.
}
/* ************************************************************************* */
TEST(AHRSFactor, ErrorWithBiases) {
// Linearization point
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Vector3 bias(0, 0, 0.3);
Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
@ -187,28 +188,26 @@ TEST(AHRSFactor, ErrorWithBiases) {
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
double deltaT = 1.0;
AHRSFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero());
AHRSFactor::PreintegratedMeasurements pre_int_data(Vector3(0,0,0),
Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
// Create factor
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
SETDEBUG("ImuFactor evaluateError", false);
Vector errorActual = factor.evaluateError(x1, x2, bias);
SETDEBUG("ImuFactor evaluateError", false);
// Expected error
Vector errorExpected(3);
errorExpected << 0, 0, 0;
EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, imuBias::ConstantBias>(
Matrix H3e = numericalDerivative11<Vector, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
// Check rotation Jacobians
@ -216,7 +215,7 @@ TEST(AHRSFactor, ErrorWithBiases) {
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3, imuBias::ConstantBias>(
Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
// Actual Jacobians
@ -225,14 +224,13 @@ TEST(AHRSFactor, ErrorWithBiases) {
EXPECT(assert_equal(H1e, H1a));
EXPECT(assert_equal(H2e, H2a));
EXPECT(assert_equal(H3e, H3a));
EXPECT(assert_equal(H3e, H3a));
}
/* ************************************************************************* */
//******************************************************************************
TEST( AHRSFactor, PartialDerivativeExpmap ) {
// Linearization point
Vector3 biasOmega;
biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias
Vector3 biasOmega(0,0,0);
// Measurements
Vector3 measuredOmega;
@ -241,8 +239,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) {
// Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluateRotation, measuredOmega, _1, deltaT),
biasOmega);
boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega);
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3(
(measuredOmega - biasOmega) * deltaT);
@ -250,11 +247,12 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) {
Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
// Compare Jacobians
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3));
// 1e-3 needs to be added only when using quaternions for rotations
}
/* ************************************************************************* */
//******************************************************************************
TEST( AHRSFactor, PartialDerivativeLogmap ) {
// Linearization point
Vector3 thetahat;
@ -280,11 +278,10 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) {
}
/* ************************************************************************* */
//******************************************************************************
TEST( AHRSFactor, fistOrderExponential ) {
// Linearization point
Vector3 biasOmega;
biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias
Vector3 biasOmega(0,0,0);
// Measurements
Vector3 measuredOmega;
@ -313,10 +310,10 @@ TEST( AHRSFactor, fistOrderExponential ) {
EXPECT(assert_equal(expectedRot, actualRot));
}
/* ************************************************************************* */
//******************************************************************************
TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
// Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
Vector3 bias; ///< Current estimate of rotation rate bias
Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
@ -335,32 +332,29 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
// Actual preintegrated values
AHRSFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredOmegas,
deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0));
evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
Vector3(M_PI / 100.0, 0.0, 0.0));
// Compute numerical derivatives
Matrix expectedDelRdelBias =
numericalDerivative11<Rot3, imuBias::ConstantBias>(
numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
measuredOmegas, deltaTs,
Vector3(M_PI / 100.0, 0.0, 0.0)), bias);
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias);
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
// Compare Jacobians
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
EXPECT(
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(),
1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3));
// 1e-3 needs to be added only when using quaternions for rotations
}
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
/* ************************************************************************* */
//******************************************************************************
TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Vector3 bias(0, 0, 0.3);
Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
@ -376,8 +370,7 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
Point3(1, 0, 0));
AHRSFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
AHRSFactor::PreintegratedMeasurements pre_int_data(Vector3::Zero(),
Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
@ -390,7 +383,7 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, imuBias::ConstantBias>(
Matrix H3e = numericalDerivative11<Vector, Vector3>(
boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
// Check rotation Jacobians
@ -398,7 +391,7 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3, imuBias::ConstantBias>(
Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
// Actual Jacobians
@ -409,41 +402,52 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
EXPECT(assert_equal(H2e, H2a));
EXPECT(assert_equal(H3e, H3a));
}
/* ************************************************************************* */
//******************************************************************************
TEST (AHRSFactor, predictTest) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
Vector3 bias(0,0,0);
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.0, 0.0;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0;
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0.0, 0.0;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0;
double deltaT = 0.001;
AHRSFactor::PreintegratedMeasurements pre_int_data(bias,Matrix3::Zero());
for (int i = 0; i < 1000; ++i){
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero());
for (int i = 0; i < 1000; ++i) {
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
}
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
// Predict
Rot3 x;
Rot3 expectedRot = Rot3().ypr(M_PI/10,0,0);
Rot3 actualRot = factor.predict(x,bias, pre_int_data, omegaCoriolis);
Rot3 expectedRot = Rot3().ypr(M_PI / 10, 0, 0);
Rot3 actualRot = factor.predict(x, bias, pre_int_data, omegaCoriolis);
EXPECT(assert_equal(expectedRot, actualRot, 1e-6));
// AHRSFactor::PreintegratedMeasurements::predict
Matrix expectedH = numericalDerivative11<Vector3, Vector3>(
boost::bind(&AHRSFactor::PreintegratedMeasurements::predict,
&pre_int_data, _1, boost::none), bias);
// Actual Jacobians
Matrix H;
(void) pre_int_data.predict(bias,H);
EXPECT(assert_equal(expectedH, H));
}
/* ************************************************************************* */
//******************************************************************************
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Marginals.h>
TEST (AHRSFactor, graphTest) {
// linearization point
Rot3 x1(Rot3::RzRyRx(0, 0, 0));
Rot3 x2(Rot3::RzRyRx(0, M_PI/4, 0));
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
Rot3 x2(Rot3::RzRyRx(0, M_PI / 4, 0));
Vector3 bias(0,0,0);
// PreIntegrator
imuBias::ConstantBias biasHat(Vector3(0, 0, 0), Vector3(0, 0, 0));
Vector3 biasHat(0, 0, 0);
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
@ -452,18 +456,19 @@ TEST (AHRSFactor, graphTest) {
Matrix3::Identity());
// Pre-integrate measurements
Vector3 measuredAcc(0.0, 0.0, 0.0);
Vector3 measuredOmega(0, M_PI/20, 0);
Vector3 measuredOmega(0, M_PI / 20, 0);
double deltaT = 1;
// Create Factor
noiseModel::Base::shared_ptr model = noiseModel::Gaussian::Covariance(pre_int_data.PreintMeasCov());
noiseModel::Base::shared_ptr model = //
noiseModel::Gaussian::Covariance(pre_int_data.preintMeasCov());
NonlinearFactorGraph graph;
Values values;
for(size_t i = 0; i < 5; ++i) {
for (size_t i = 0; i < 5; ++i) {
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
}
// pre_int_data.print("Pre integrated measurementes");
// pre_int_data.print("Pre integrated measurementes");
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
values.insert(X(1), x1);
values.insert(X(2), x2);
@ -471,13 +476,13 @@ TEST (AHRSFactor, graphTest) {
graph.push_back(factor);
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
Rot3 expectedRot(Rot3::RzRyRx(0, M_PI/4, 0));
Rot3 expectedRot(Rot3::RzRyRx(0, M_PI / 4, 0));
EXPECT(assert_equal(expectedRot, result.at<Rot3>(X(2))));
}
/* ************************************************************************* */
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */
//******************************************************************************

1
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -271,7 +271,6 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity){
Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3;
Vector3 measuredAcc; measuredAcc << 0,1,-9.81;
double deltaT = 0.001;
double tol = 1e-6;
Matrix I6x6(6,6);
I6x6 = Matrix::Identity(6,6);

2
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -569,7 +569,6 @@ TEST(ImuFactor, PredictPositionAndVelocity){
Vector3 measuredOmega; measuredOmega << 0, 0, 0;//M_PI/10.0+0.3;
Vector3 measuredAcc; measuredAcc << 0,1,-9.81;
double deltaT = 0.001;
double tol = 1e-6;
Matrix I6x6(6,6);
I6x6 = Matrix::Identity(6,6);
@ -603,7 +602,6 @@ TEST(ImuFactor, PredictRotation) {
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10;//M_PI/10.0+0.3;
Vector3 measuredAcc; measuredAcc << 0,0,-9.81;
double deltaT = 0.001;
double tol = 1e-6;
Matrix I6x6(6,6);
I6x6 = Matrix::Identity(6,6);