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

Merge remote-tracking branch 'origin/feature/AHRS_Vector3bias' into feature/AHRS_Polish

release/4.3a0
dellaert 2014-11-24 17:26:57 +01:00
parent 3a5457ccdd db53c1b714
commit 7c5dd8420e
3 changed files with 60 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/navigation/AHRSFactor.cpp
View File

@ -28,7 +28,7 @@ namespace gtsam {
// Inner class PreintegratedMeasurements // Inner class PreintegratedMeasurements
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements( AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements(
const imuBias::ConstantBias& bias, const Matrix3& measuredOmegaCovariance) : const Vector3& bias, const Matrix3& measuredOmegaCovariance) :
biasHat_(bias), deltaTij_(0.0) { biasHat_(bias), deltaTij_(0.0) {
measurementCovariance_ << measuredOmegaCovariance; measurementCovariance_ << measuredOmegaCovariance;
delRdelBiasOmega_.setZero(); delRdelBiasOmega_.setZero();
@ -37,7 +37,7 @@ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements(
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() : AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
biasHat_(imuBias::ConstantBias()), deltaTij_(0.0) { biasHat_(Vector3()), deltaTij_(0.0) {
measurementCovariance_.setZero(); measurementCovariance_.setZero();
delRdelBiasOmega_.setZero(); delRdelBiasOmega_.setZero();
delRdelBiasOmega_.setZero(); delRdelBiasOmega_.setZero();
@ -47,7 +47,7 @@ AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const { void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const {
std::cout << s << std::endl; std::cout << s << std::endl;
biasHat_.print(" biasHat"); std::cout << "biasHat [" << biasHat_.transpose() << "]" << std::endl;
deltaRij_.print(" deltaRij "); deltaRij_.print(" deltaRij ");
std::cout << " measurementCovariance [" << measurementCovariance_ << " ]" std::cout << " measurementCovariance [" << measurementCovariance_ << " ]"
<< std::endl; << std::endl;
@ -57,7 +57,7 @@ void AHRSFactor::PreintegratedMeasurements::print(const std::string& s) const {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool AHRSFactor::PreintegratedMeasurements::equals( bool AHRSFactor::PreintegratedMeasurements::equals(
const PreintegratedMeasurements& other, double tol) const { const PreintegratedMeasurements& other, double tol) const {
return biasHat_.equals(other.biasHat_, tol) return equal_with_abs_tol(biasHat_, other.biasHat_, tol)
&& equal_with_abs_tol(measurementCovariance_, && equal_with_abs_tol(measurementCovariance_,
other.measurementCovariance_, tol) other.measurementCovariance_, tol)
&& deltaRij_.equals(other.deltaRij_, tol) && deltaRij_.equals(other.deltaRij_, tol)
@ -80,7 +80,7 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
// NOTE: order is important here because each update uses old values. // NOTE: order is important here because each update uses old values.
// First we compensate the measurements for the bias // First we compensate the measurements for the bias
Vector3 correctedOmega = biasHat_.correctGyroscope(measuredOmega); Vector3 correctedOmega = measuredOmega - biasHat_;
// Then compensate for sensor-body displacement: we express the quantities // Then compensate for sensor-body displacement: we express the quantities
// (originally in the IMU frame) into the body frame // (originally in the IMU frame) into the body frame
@ -136,9 +136,9 @@ void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Vector3 AHRSFactor::PreintegratedMeasurements::predict( Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias,
const imuBias::ConstantBias& bias, boost::optional<Matrix&> H) const { boost::optional<Matrix&> H) const {
const Vector3 biasOmegaIncr = bias.gyroscope() - biasHat_.gyroscope(); const Vector3 biasOmegaIncr = bias - biasHat_;
Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr; Vector3 delRdelBiasOmega_biasOmegaIncr = delRdelBiasOmega_ * biasOmegaIncr;
const Rot3 deltaRij_biascorrected = deltaRij_.retract( const Rot3 deltaRij_biascorrected = deltaRij_.retract(
delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP); delRdelBiasOmega_biasOmegaIncr, Rot3::EXPMAP);
@ -172,7 +172,7 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles(
// AHRSFactor methods // AHRSFactor methods
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
AHRSFactor::AHRSFactor() : AHRSFactor::AHRSFactor() :
preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero()) { preintegratedMeasurements_(Vector3(), Matrix3::Zero()) {
} }
AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
@ -217,7 +217,7 @@ bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j, Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j,
const imuBias::ConstantBias& bias, boost::optional<Matrix&> H1, const Vector3& bias, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2, boost::optional<Matrix&> H3) const { boost::optional<Matrix&> H2, boost::optional<Matrix&> H3) const {
// Do bias correction, if (H3) will contain 3*3 derivative used below // Do bias correction, if (H3) will contain 3*3 derivative used below
@ -258,8 +258,8 @@ Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j,
if (H3) { if (H3) {
// dfR/dBias, note H3 contains derivative of predict // dfR/dBias, note H3 contains derivative of predict
const Matrix3 JbiasOmega = Jr_theta_bcc * (*H3); const Matrix3 JbiasOmega = Jr_theta_bcc * (*H3);
H3->resize(3, 6); H3->resize(3, 3);
(*H3) << Matrix::Zero(3, 3), Jrinv_fRhat * (-fRhat.transpose() * JbiasOmega); (*H3) << Jrinv_fRhat * (-fRhat.transpose() * JbiasOmega);
} }
Vector error(3); Vector error(3);
@ -268,7 +268,7 @@ Vector AHRSFactor::evaluateError(const Rot3& rot_i, const Rot3& rot_j,
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Rot3 AHRSFactor::predict(const Rot3& rot_i, const imuBias::ConstantBias& bias, Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedMeasurements preintegratedMeasurements, const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) { const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) {

27
gtsam/navigation/AHRSFactor.h
View File

@ -13,6 +13,7 @@
* @file AHRSFactor.h * @file AHRSFactor.h
* @author Krunal Chande * @author Krunal Chande
* @author Luca Carlone * @author Luca Carlone
* @author Frank Dellaert
* @date July 2014 * @date July 2014
**/ **/
@ -20,11 +21,11 @@
/* GTSAM includes */ /* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/ImuBias.h> #include <gtsam/geometry/Pose3.h>
namespace gtsam { namespace gtsam {
class AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, imuBias::ConstantBias> { class AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
public: public:
/** /**
@ -39,7 +40,7 @@ public:
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 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) 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)
@ -57,7 +58,7 @@ public:
* @param bias Current estimate of acceleration and rotation rate biases * @param bias Current estimate of acceleration and rotation rate biases
* @param measuredOmegaCovariance Covariance matrix of measured angular rate * @param measuredOmegaCovariance Covariance matrix of measured angular rate
*/ */
PreintegratedMeasurements(const imuBias::ConstantBias& bias, PreintegratedMeasurements(const Vector3& bias,
const Matrix3& measuredOmegaCovariance); const Matrix3& measuredOmegaCovariance);
/// print /// print
@ -75,8 +76,8 @@ public:
double deltaTij() const { double deltaTij() const {
return deltaTij_; return deltaTij_;
} }
Vector6 biasHat() const { Vector3 biasHat() const {
return biasHat_.vector(); return biasHat_;
} }
const Matrix3& delRdelBiasOmega() const { const Matrix3& delRdelBiasOmega() const {
return delRdelBiasOmega_; return delRdelBiasOmega_;
@ -99,8 +100,8 @@ public:
/// Predict bias-corrected incremental rotation /// Predict bias-corrected incremental rotation
/// TODO: The matrix Hbias is the derivative of predict? Unit-test? /// TODO: The matrix Hbias is the derivative of predict? Unit-test?
Vector3 predict(const imuBias::ConstantBias& bias, Vector3 predict(const Vector3& bias, boost::optional<Matrix&> H =
boost::optional<Matrix&> H = boost::none) const; boost::none) const;
/// Integrate coriolis correction in body frame rot_i /// Integrate coriolis correction in body frame rot_i
Vector3 integrateCoriolis(const Rot3& rot_i, Vector3 integrateCoriolis(const Rot3& rot_i,
@ -129,7 +130,7 @@ public:
private: private:
typedef AHRSFactor This; typedef AHRSFactor This;
typedef NoiseModelFactor3<Rot3, Rot3, imuBias::ConstantBias> Base; typedef NoiseModelFactor3<Rot3, Rot3, Vector3> Base;
PreintegratedMeasurements preintegratedMeasurements_; PreintegratedMeasurements preintegratedMeasurements_;
Vector3 gravity_; Vector3 gravity_;
@ -188,12 +189,12 @@ public:
/// vector of errors /// vector of errors
Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j, Vector evaluateError(const Rot3& rot_i, const Rot3& rot_j,
const imuBias::ConstantBias& bias, boost::optional<Matrix&> H1 = const Vector3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::none, boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::optional<Matrix&> H3 = boost::none) const; boost::none) const;
/// predicted states from IMU /// predicted states from IMU
static Rot3 predict(const Rot3& rot_i, const imuBias::ConstantBias& bias, static Rot3 predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedMeasurements preintegratedMeasurements, const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& omegaCoriolis, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none); boost::optional<const Pose3&> body_P_sensor = boost::none);

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

@ -39,19 +39,19 @@ using symbol_shorthand::B;
//****************************************************************************** //******************************************************************************
namespace { namespace {
Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i, 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); return factor.evaluateError(rot_i, rot_j, bias);
} }
Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i, 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)); return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3));
} }
AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias, const list<Vector3>& measuredOmegas, const Vector3& bias, 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()) {
AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity()); AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity());
list<Vector3>::const_iterator itOmega = measuredOmegas.begin(); list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@ -64,9 +64,9 @@ AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
} }
Rot3 evaluatePreintegratedMeasurementsRotation( Rot3 evaluatePreintegratedMeasurementsRotation(
const imuBias::ConstantBias& bias, const list<Vector3>& measuredOmegas, const Vector3& bias, 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::Zero()) {
return Rot3( return Rot3(
evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs, evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
initialRotationRate).deltaRij()); initialRotationRate).deltaRij());
@ -85,7 +85,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 Vector3 bias(0,0,0); ///< Current estimate of angular rate bias
// Measurements // Measurements
Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
@ -117,7 +117,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
//****************************************************************************** //******************************************************************************
TEST( ImuFactor, Error ) { TEST( ImuFactor, Error ) {
// Linearization point // 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 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)); Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0));
@ -147,7 +147,7 @@ TEST( ImuFactor, Error ) {
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>( Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); 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); boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
// Check rotation Jacobians // Check rotation Jacobians
@ -178,7 +178,7 @@ TEST( ImuFactor, Error ) {
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) Vector3 bias(0, 0, 0.3);
Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0))); 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))); Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
@ -189,8 +189,7 @@ TEST( ImuFactor, ErrorWithBiases ) {
measuredOmega << 0, 0, M_PI / 10.0 + 0.3; measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
double deltaT = 1.0; double deltaT = 1.0;
AHRSFactor::PreintegratedMeasurements pre_int_data( AHRSFactor::PreintegratedMeasurements pre_int_data(Vector3(0,0,0),
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Zero()); Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredOmega, deltaT);
@ -211,7 +210,7 @@ TEST( ImuFactor, ErrorWithBiases ) {
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>( Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); 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); boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
// Check rotation Jacobians // Check rotation Jacobians
@ -219,7 +218,7 @@ TEST( ImuFactor, ErrorWithBiases ) {
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>( Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); 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); boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
// Actual Jacobians // Actual Jacobians
@ -234,8 +233,7 @@ TEST( ImuFactor, ErrorWithBiases ) {
//****************************************************************************** //******************************************************************************
TEST( AHRSFactor, PartialDerivativeExpmap ) { TEST( AHRSFactor, PartialDerivativeExpmap ) {
// Linearization point // Linearization point
Vector3 biasOmega; Vector3 biasOmega(0,0,0);
biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias
// Measurements // Measurements
Vector3 measuredOmega; Vector3 measuredOmega;
@ -286,8 +284,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) {
//****************************************************************************** //******************************************************************************
TEST( AHRSFactor, fistOrderExponential ) { TEST( AHRSFactor, fistOrderExponential ) {
// Linearization point // Linearization point
Vector3 biasOmega; Vector3 biasOmega(0,0,0);
biasOmega << 0, 0, 0; ///< Current estimate of rotation rate bias
// Measurements // Measurements
Vector3 measuredOmega; Vector3 measuredOmega;
@ -319,7 +316,7 @@ TEST( AHRSFactor, fistOrderExponential ) {
//****************************************************************************** //******************************************************************************
TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
// Linearization point // 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)); Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1));
@ -343,14 +340,12 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelRdelBias = Matrix expectedDelRdelBias =
numericalDerivative11<Rot3, imuBias::ConstantBias>( numericalDerivative11<Rot3, Vector3>(
boost::bind(&evaluatePreintegratedMeasurementsRotation, _1, boost::bind(&evaluatePreintegratedMeasurementsRotation, _1,
measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias); measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias);
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( EXPECT(
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 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
@ -362,7 +357,7 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
//****************************************************************************** //******************************************************************************
TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { 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 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))); Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
@ -378,8 +373,7 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
Point3(1, 0, 0)); Point3(1, 0, 0));
AHRSFactor::PreintegratedMeasurements pre_int_data( AHRSFactor::PreintegratedMeasurements pre_int_data(Vector3::Zero(),
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Zero()); Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredOmega, deltaT); pre_int_data.integrateMeasurement(measuredOmega, deltaT);
@ -392,7 +386,7 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
boost::bind(&callEvaluateError, factor, _1, x2, bias), x1); boost::bind(&callEvaluateError, factor, _1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>( Matrix H2e = numericalDerivative11<Vector, Rot3>(
boost::bind(&callEvaluateError, factor, x1, _1, bias), x2); 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); boost::bind(&callEvaluateError, factor, x1, x2, _1), bias);
// Check rotation Jacobians // Check rotation Jacobians
@ -400,7 +394,7 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1); boost::bind(&evaluateRotationError, factor, _1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>( Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
boost::bind(&evaluateRotationError, factor, x1, _1, bias), x2); 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); boost::bind(&evaluateRotationError, factor, x1, x2, _1), bias);
// Actual Jacobians // Actual Jacobians
@ -413,7 +407,7 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
} }
//****************************************************************************** //******************************************************************************
TEST (AHRSFactor, predictTest) { TEST (AHRSFactor, predictTest) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot) Vector3 bias(0,0,0);
// Measurements // Measurements
Vector3 gravity; Vector3 gravity;
@ -435,6 +429,15 @@ TEST (AHRSFactor, predictTest) {
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));
// 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/linear/GaussianFactorGraph.h>
@ -444,10 +447,10 @@ 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)); Vector3 bias(0,0,0);
// PreIntegrator // PreIntegrator
imuBias::ConstantBias biasHat(Vector3(0, 0, 0), Vector3(0, 0, 0)); Vector3 biasHat(0, 0, 0);
Vector3 gravity; Vector3 gravity;
gravity << 0, 0, 9.81; gravity << 0, 0, 9.81;
Vector3 omegaCoriolis; Vector3 omegaCoriolis;
@ -456,7 +459,6 @@ TEST (AHRSFactor, graphTest) {
Matrix3::Identity()); Matrix3::Identity());
// Pre-integrate measurements // 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; double deltaT = 1;