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

Doxygen docs and naming convention PreintMeasCov_ -> preintMeasCov_

release/4.3a0
dellaert 2014-11-23 11:36:05 +01:00
parent 02ed59b65d
commit 5ab5e008ba
4 changed files with 99 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam.h
View File

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

22
gtsam/navigation/AHRSFactor.cpp
View File

@ -37,7 +37,7 @@ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements(
biasHat_(bias), deltaTij_(0.0) { biasHat_(bias), deltaTij_(0.0) {
measurementCovariance_ << measuredOmegaCovariance; measurementCovariance_ << measuredOmegaCovariance;
delRdelBiasOmega_.setZero(); delRdelBiasOmega_.setZero();
PreintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -46,7 +46,7 @@ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
measurementCovariance_.setZero(); measurementCovariance_.setZero();
delRdelBiasOmega_.setZero(); delRdelBiasOmega_.setZero();
delRdelBiasOmega_.setZero(); delRdelBiasOmega_.setZero();
PreintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -56,18 +56,18 @@ void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const {
deltaRij_.print(" deltaRij "); deltaRij_.print(" deltaRij ");
std::cout << " measurementCovariance [" << measurementCovariance_ << " ]" std::cout << " measurementCovariance [" << measurementCovariance_ << " ]"
<< std::endl; << std::endl;
std::cout << " PreintMeasCov [ " << PreintMeasCov_ << " ]" << std::endl; std::cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << std::endl;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool AHRSFactor::PreintegratedMeasurements::equals( bool AHRSFactor::PreintegratedMeasurements::equals(
const PreintegratedMeasurements& expected, double tol) const { const PreintegratedMeasurements& other, double tol) const {
return biasHat_.equals(expected.biasHat_, tol) return biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(measurementCovariance_, && equal_with_abs_tol(measurementCovariance_,
expected.measurementCovariance_, tol) other.measurementCovariance_, tol)
&& deltaRij_.equals(expected.deltaRij_, tol) && deltaRij_.equals(other.deltaRij_, tol)
&& std::fabs(deltaTij_ - expected.deltaTij_) < tol && std::fabs(deltaTij_ - other.deltaTij_) < tol
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_, tol); && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -75,7 +75,7 @@ void AHRSFactor::PreintegratedMeasurements::resetIntegration() {
deltaRij_ = Rot3(); deltaRij_ = Rot3();
deltaTij_ = 0.0; deltaTij_ = 0.0;
delRdelBiasOmega_.setZero(); delRdelBiasOmega_.setZero();
PreintMeasCov_.setZero(); preintMeasCov_.setZero();
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
@ -131,7 +131,7 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
// first order uncertainty propagation // first order uncertainty propagation
// the deltaT allows to pass from continuous time noise to discrete time noise // the deltaT allows to pass from continuous time noise to discrete time noise
PreintMeasCov_ = F * PreintMeasCov_ * F.transpose() preintMeasCov_ = F * preintMeasCov_ * F.transpose()
+ measurementCovariance_ * deltaT; + measurementCovariance_ * deltaT;
// Update preintegrated measurements // Update preintegrated measurements

64
gtsam/navigation/AHRSFactor.h
View File

@ -32,11 +32,13 @@ namespace gtsam {
class AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, imuBias::ConstantBias> { class AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, imuBias::ConstantBias> {
public: 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.
/** CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope measurements (rotation rates) * 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*/ * Can be built incrementally so as to avoid costly integration at time of
* factor construction.
*/
class PreintegratedMeasurements { class PreintegratedMeasurements {
friend class AHRSFactor; friend class AHRSFactor;
@ -48,48 +50,54 @@ public:
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
Matrix3 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: public:
/// Default constructor /// Default constructor
PreintegratedMeasurements(); PreintegratedMeasurements();
/// Default constructor, initialize with no measurements /**
PreintegratedMeasurements(const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases * Default constructor, initialize with no measurements
const Matrix3& measuredOmegaCovariance ///< Covariance matrix of measured angular rate * @param bias Current estimate of acceleration and rotation rate biases
); * @param measuredOmegaCovariance Covariance matrix of measured angular rate
*/
PreintegratedMeasurements(const imuBias::ConstantBias& bias,
const Matrix3& measuredOmegaCovariance);
/** print */ /// print
void print(const std::string& s = "Preintegrated Measurements: ") const; void print(const std::string& s = "Preintegrated Measurements: ") const;
/** equals */ /// equals
bool equals(const PreintegratedMeasurements& expected, bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const;
double tol = 1e-9) const;
Matrix measurementCovariance() const {return measurementCovariance_;} const Matrix3& measurementCovariance() const {return measurementCovariance_;}
Matrix deltaRij() const {return deltaRij_.matrix();} Matrix3 deltaRij() const {return deltaRij_.matrix();}
double deltaTij() const {return deltaTij_;} double deltaTij() const {return deltaTij_;}
Vector biasHat() const {return biasHat_.vector();} Vector6 biasHat() const {return biasHat_.vector();}
Matrix3 delRdelBiasOmega() const {return delRdelBiasOmega_;} const Matrix3& delRdelBiasOmega() const {return delRdelBiasOmega_;}
Matrix PreintMeasCov() const {return PreintMeasCov_;} const Matrix3& preintMeasCov() const {return preintMeasCov_;}
/// TODO: Document /// TODO: Document
void resetIntegration(); void resetIntegration();
/** Add a single Gyroscope measurement to the preintegration. */ /**
void integrateMeasurement( * Add a single Gyroscope measurement to the preintegration.
const Vector3& measuredOmega, ///< Measured angular velocity (in body frame) * @param measureOmedga Measured angular velocity (in body frame)
double deltaT, ///< Time step * @param deltaT Time step
boost::optional<const Pose3&> body_P_sensor = boost::none ///< Sensor frame * @param body_P_sensor Optional sensor frame
); */
void integrateMeasurement(const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor = boost::none);
// 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 Vector PreIntegrateIMUObservations_delta_angles( static Vector PreIntegrateIMUObservations_delta_angles(
const Vector& msr_gyro_t, const double msr_dt, const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles); const Vector3& delta_angles);
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
@ -134,7 +142,7 @@ public:
) : ) :
Base( Base(
noiseModel::Gaussian::Covariance( noiseModel::Gaussian::Covariance(
preintegratedMeasurements.PreintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_( preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), preintegratedMeasurements_(
preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_( preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
body_P_sensor) { body_P_sensor) {
} }

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

@ -48,8 +48,7 @@ Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i,
} }
AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias, const imuBias::ConstantBias& bias, const list<Vector3>& measuredOmegas,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs, const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) { const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) {
AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity()); AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity());
@ -64,12 +63,12 @@ AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
} }
Rot3 evaluatePreintegratedMeasurementsRotation( Rot3 evaluatePreintegratedMeasurementsRotation(
const imuBias::ConstantBias& bias, const imuBias::ConstantBias& bias, const list<Vector3>& measuredOmegas,
const list<Vector3>& measuredOmegas,
const list<double>& deltaTs, const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) { const Vector3& initialRotationRate = Vector3(0.0, 0.0, 0.0)) {
return Rot3(evaluatePreintegratedMeasurements(bias, measuredOmegas, return Rot3(
deltaTs, initialRotationRate).deltaRij()); evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
initialRotationRate).deltaRij());
} }
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
@ -82,8 +81,7 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( AHRSFactor, PreintegratedMeasurements ) {
TEST( AHRSFactor, PreintegratedMeasurements ) {
// Linearization point // Linearization point
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); ///< Current estimate of acceleration and angular rate biases
@ -114,8 +112,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( ImuFactor, Error ) {
TEST( ImuFactor, Error ) {
// Linearization point // Linearization point
imuBias::ConstantBias bias; // Bias imuBias::ConstantBias bias; // Bias
Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
@ -133,8 +130,7 @@ TEST( ImuFactor, Error ) {
pre_int_data.integrateMeasurement(measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredOmega, deltaT);
// Create factor // Create factor
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, false);
false);
Vector errorActual = factor.evaluateError(x1, x2, bias); Vector errorActual = factor.evaluateError(x1, x2, bias);
@ -162,18 +158,20 @@ TEST( ImuFactor, Error ) {
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a); (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
// rotations // 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)); EXPECT(assert_equal(H2e, H2a, 1e-5));
// rotations // 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( ImuFactor, ErrorWithBiases ) {
TEST( ImuFactor, ErrorWithBiases ) {
// Linearization point // Linearization point
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
@ -188,7 +186,8 @@ TEST( ImuFactor, ErrorWithBiases ) {
double deltaT = 1.0; double deltaT = 1.0;
AHRSFactor::PreintegratedMeasurements pre_int_data( AHRSFactor::PreintegratedMeasurements pre_int_data(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)), Matrix3::Zero()); imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredOmega, deltaT);
// Create factor // Create factor
@ -201,7 +200,7 @@ TEST( ImuFactor, ErrorWithBiases ) {
// Expected error // Expected error
Vector errorExpected(3); Vector errorExpected(3);
errorExpected << 0, 0, 0; errorExpected << 0, 0, 0;
EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians // Expected Jacobians
Matrix H1e = numericalDerivative11<Vector, Rot3>( Matrix H1e = numericalDerivative11<Vector, Rot3>(
@ -225,11 +224,10 @@ TEST( ImuFactor, ErrorWithBiases ) {
EXPECT(assert_equal(H1e, H1a)); EXPECT(assert_equal(H1e, H1a));
EXPECT(assert_equal(H2e, H2a)); EXPECT(assert_equal(H2e, H2a));
EXPECT(assert_equal(H3e, H3a)); EXPECT(assert_equal(H3e, H3a));
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( AHRSFactor, PartialDerivativeExpmap ) {
TEST( AHRSFactor, PartialDerivativeExpmap ) {
// Linearization point // Linearization point
Vector3 biasOmega; Vector3 biasOmega;
biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias
@ -241,8 +239,7 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) {
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>( Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), boost::bind(&evaluateRotation, measuredOmega, _1, deltaT), biasOmega);
biasOmega);
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3( const Matrix3 Jr = Rot3::rightJacobianExpMapSO3(
(measuredOmega - biasOmega) * deltaT); (measuredOmega - biasOmega) * deltaT);
@ -250,12 +247,12 @@ TEST( AHRSFactor, PartialDerivativeExpmap ) {
Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
// Compare Jacobians // 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 ) {
TEST( AHRSFactor, PartialDerivativeLogmap ) {
// Linearization point // Linearization point
Vector3 thetahat; Vector3 thetahat;
thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias
@ -280,8 +277,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( AHRSFactor, fistOrderExponential ) {
TEST( AHRSFactor, fistOrderExponential ) {
// Linearization point // Linearization point
Vector3 biasOmega; Vector3 biasOmega;
biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias
@ -313,8 +309,7 @@ TEST( AHRSFactor, fistOrderExponential ) {
EXPECT(assert_equal(expectedRot, actualRot)); EXPECT(assert_equal(expectedRot, actualRot));
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
// Linearization point // Linearization point
imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases imuBias::ConstantBias bias; ///< Current estimate of acceleration and rotation rate biases
@ -335,30 +330,28 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
// Actual preintegrated values // Actual preintegrated values
AHRSFactor::PreintegratedMeasurements preintegrated = AHRSFactor::PreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredOmegas, evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)); Vector3(M_PI / 100.0, 0.0, 0.0));
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelRdelBias = Matrix expectedDelRdelBias =
numericalDerivative11<Rot3, imuBias::ConstantBias>( numericalDerivative11<Rot3, imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
measuredOmegas, deltaTs, measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias);
Vector3(M_PI / 100.0, 0.0, 0.0)), bias);
Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3); Matrix expectedDelRdelBiasAcc = expectedDelRdelBias.leftCols(3);
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
// Compare Jacobians // Compare Jacobians
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3))); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
EXPECT( EXPECT(
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3));
1e-3)); // 1e-3 needs to be added only when using quaternions for rotations // 1e-3 needs to be added only when using quaternions for rotations
} }
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
/* ************************************************************************* */ /* ************************************************************************* */TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0))); Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
@ -415,23 +408,25 @@ TEST (AHRSFactor, predictTest) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
// Measurements // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity;
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.0, 0.0; gravity << 0, 0, 9.81;
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI / 10.0; Vector3 omegaCoriolis;
omegaCoriolis << 0, 0.0, 0.0;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0;
double deltaT = 0.001; double deltaT = 0.001;
AHRSFactor::PreintegratedMeasurements pre_int_data(bias,Matrix3::Zero()); AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero());
for (int i = 0; i < 1000; ++i){ for (int i = 0; i < 1000; ++i) {
pre_int_data.integrateMeasurement(measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredOmega, deltaT);
} }
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis); AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
// Predict // Predict
Rot3 x; Rot3 x;
Rot3 expectedRot = Rot3().ypr(M_PI/10,0,0); Rot3 expectedRot = Rot3().ypr(M_PI / 10, 0, 0);
Rot3 actualRot = factor.predict(x,bias, pre_int_data, omegaCoriolis); Rot3 actualRot = factor.predict(x, bias, pre_int_data, omegaCoriolis);
EXPECT(assert_equal(expectedRot, actualRot, 1e-6)); EXPECT(assert_equal(expectedRot, actualRot, 1e-6));
} }
/* ************************************************************************* */ /* ************************************************************************* */
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
@ -439,7 +434,7 @@ TEST (AHRSFactor, predictTest) {
TEST (AHRSFactor, graphTest) { TEST (AHRSFactor, graphTest) {
// linearization point // linearization point
Rot3 x1(Rot3::RzRyRx(0, 0, 0)); Rot3 x1(Rot3::RzRyRx(0, 0, 0));
Rot3 x2(Rot3::RzRyRx(0, M_PI/4, 0)); Rot3 x2(Rot3::RzRyRx(0, M_PI / 4, 0));
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
// PreIntegrator // PreIntegrator
@ -453,17 +448,19 @@ TEST (AHRSFactor, graphTest) {
// Pre-integrate measurements // Pre-integrate measurements
Vector3 measuredAcc(0.0, 0.0, 0.0); 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; double deltaT = 1;
// Create Factor // 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; NonlinearFactorGraph graph;
Values values; 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.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); AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
values.insert(X(1), x1); values.insert(X(1), x1);
values.insert(X(2), x2); values.insert(X(2), x2);
@ -471,7 +468,7 @@ TEST (AHRSFactor, graphTest) {
graph.push_back(factor); graph.push_back(factor);
LevenbergMarquardtOptimizer optimizer(graph, values); LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize(); 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)))); EXPECT(assert_equal(expectedRot, result.at<Rot3>(X(2))));
} }