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

Merge pull request #1762 from borglab/fix/ahrs 

Correctly deal with sensor rotation in AHRS
release/4.3a0
Frank Dellaert 2024-07-26 21:41:16 -05:00 committed by GitHub
parent feab2a2d20 a90dbc7fc5
commit c6bd3f8e32
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
8 changed files with 568 additions and 340 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -19,3 +19,4 @@ CMakeLists.txt.user*
xcode/
/Dockerfile
/python/gtsam/notebooks/.ipynb_checkpoints/ellipses-checkpoint.ipynb
.cache/

13
gtsam/navigation/AHRSFactor.cpp
View File

@ -49,13 +49,14 @@ void PreintegratedAhrsMeasurements::resetIntegration() {
//------------------------------------------------------------------------------
void PreintegratedAhrsMeasurements::integrateMeasurement(
const Vector3& measuredOmega, double deltaT) {
Matrix3 Fr;
PreintegratedRotation::integrateGyroMeasurement(measuredOmega, biasHat_,
deltaT, &Fr);
Matrix3 D_incrR_integratedOmega, Fr;
PreintegratedRotation::integrateMeasurement(measuredOmega,
biasHat_, deltaT, &D_incrR_integratedOmega, &Fr);
// first order uncertainty propagation
// the deltaT allows to pass from continuous time noise to discrete time noise
// First order uncertainty propagation
// The deltaT allows to pass from continuous time noise to discrete time
// noise. Comparing with the IMUFactor.cpp implementation, the latter is an
// approximation for C * (wCov / dt) * C.transpose(), with C \approx I * dt.
preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + p().gyroscopeCovariance * deltaT;
}

67
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -68,39 +68,40 @@ bool PreintegratedRotation::equals(const PreintegratedRotation& other,
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
}
Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
namespace internal {
Rot3 IncrementalRotation::operator()(
const Vector3& bias, OptionalJacobian<3, 3> H_bias) const {
// First we compensate the measurements for the bias
Vector3 correctedOmega = measuredOmega - biasHat;
Vector3 correctedOmega = measuredOmega - bias;
// Then compensate for sensor-body displacement: we express the quantities
// (originally in the IMU frame) into the body frame
if (p_->body_P_sensor) {
Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix();
// (originally in the IMU frame) into the body frame. If Jacobian is
// requested, the rotation matrix is obtained as `rotate` Jacobian.
Matrix3 body_R_sensor;
if (body_P_sensor) {
// rotation rate vector in the body frame
correctedOmega = body_R_sensor * correctedOmega;
correctedOmega = body_P_sensor->rotation().rotate(
correctedOmega, {}, H_bias ? &body_R_sensor : nullptr);
}
// rotation vector describing rotation increment computed from the
// current rotation rate measurement
const Vector3 integratedOmega = correctedOmega * deltaT;
return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
}
void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
OptionalJacobian<3, 3> F) {
Matrix3 D_incrR_integratedOmega;
const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
D_incrR_integratedOmega);
// If asked, pass first derivative as well
if (optional_D_incrR_integratedOmega) {
*optional_D_incrR_integratedOmega << D_incrR_integratedOmega;
Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias); // expensive !!
if (H_bias) {
*H_bias *= -deltaT; // Correct so accurately reflects bias derivative
if (body_P_sensor) *H_bias *= body_R_sensor;
}
return incrR;
}
} // namespace internal
void PreintegratedRotation::integrateGyroMeasurement(
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> F) {
Matrix3 H_bias;
internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
const Rot3 incrR = f(biasHat, H_bias);
// Update deltaTij and rotation
deltaTij_ += deltaT;
@ -108,10 +109,26 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
// Update Jacobian
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
- D_incrR_integratedOmega * deltaT;
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + H_bias;
}
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
void PreintegratedRotation::integrateMeasurement(
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
OptionalJacobian<3, 3> F) {
integrateGyroMeasurement(measuredOmega, biasHat, deltaT, F);
// If asked, pass obsolete Jacobians as well
if (optional_D_incrR_integratedOmega) {
Matrix3 H_bias;
internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
const Rot3 incrR = f(biasHat, H_bias);
*optional_D_incrR_integratedOmega << H_bias / -deltaT;
}
}
#endif
Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
OptionalJacobian<3, 3> H) const {
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasOmegaIncr;

96
gtsam/navigation/PreintegratedRotation.h
View File

@ -21,12 +21,37 @@
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/std_optional_serialization.h>
#include <gtsam/geometry/Pose3.h>
#include "gtsam/dllexport.h"
namespace gtsam {
namespace internal {
/**
* @brief Function object for incremental rotation.
* @param measuredOmega The measured angular velocity (as given by the sensor)
* @param deltaT The time interval over which the rotation is integrated.
* @param body_P_sensor Optional transform between body and IMU.
*/
struct GTSAM_EXPORT IncrementalRotation {
const Vector3& measuredOmega;
const double deltaT;
const std::optional<Pose3>& body_P_sensor;
/**
* @brief Integrate angular velocity, but corrected by bias.
* @param bias The bias estimate
* @param H_bias Jacobian of the rotation w.r.t. bias.
* @return The incremental rotation
*/
Rot3 operator()(const Vector3& bias,
OptionalJacobian<3, 3> H_bias = {}) const;
};
} // namespace internal
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct GTSAM_EXPORT PreintegratedRotationParams {
@ -65,7 +90,6 @@ struct GTSAM_EXPORT PreintegratedRotationParams {
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
@ -136,18 +160,10 @@ class GTSAM_EXPORT PreintegratedRotation {
/// @name Access instance variables
/// @{
const std::shared_ptr<Params>& params() const {
return p_;
}
const double& deltaTij() const {
return deltaTij_;
}
const Rot3& deltaRij() const {
return deltaRij_;
}
const Matrix3& delRdelBiasOmega() const {
return delRdelBiasOmega_;
}
const std::shared_ptr<Params>& params() const { return p_; }
const double& deltaTij() const { return deltaTij_; }
const Rot3& deltaRij() const { return deltaRij_; }
const Matrix3& delRdelBiasOmega() const { return delRdelBiasOmega_; }
/// @}
/// @name Testable
@ -159,19 +175,24 @@ class GTSAM_EXPORT PreintegratedRotation {
/// @name Main functionality
/// @{
/// Take the gyro measurement, correct it using the (constant) bias estimate
/// and possibly the sensor pose, and then integrate it forward in time to yield
/// an incremental rotation.
Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
/// Calculate an incremental rotation given the gyro measurement and a time interval,
/// and update both deltaTij_ and deltaRij_.
void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega = {},
/**
* @brief Calculate an incremental rotation given the gyro measurement and a
* time interval, and update both deltaTij_ and deltaRij_.
* @param measuredOmega The measured angular velocity (as given by the sensor)
* @param bias The biasHat estimate
* @param deltaT The time interval
* @param F optional Jacobian of internal compose, used in AhrsFactor.
*/
void integrateGyroMeasurement(const Vector3& measuredOmega,
const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> F = {});
/// Return a bias corrected version of the integrated rotation, with optional Jacobian
/**
* @brief Return a bias corrected version of the integrated rotation.
* @param biasOmegaIncr An increment with respect to biasHat used above.
* @param H optional Jacobian of the correction w.r.t. the bias increment.
* @note The *key* functionality of this class used in optimizing the bias.
*/
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
OptionalJacobian<3, 3> H = {}) const;
@ -180,6 +201,31 @@ class GTSAM_EXPORT PreintegratedRotation {
/// @}
/// @name Deprecated API
/// @{
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
/// @deprecated: use IncrementalRotation functor with sane Jacobian
inline Rot3 GTSAM_DEPRECATED incrementalRotation(
const Vector3& measuredOmega, const Vector3& bias, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
Rot3 incrR = f(bias, D_incrR_integratedOmega);
// Backwards compatible "weird" Jacobian, no longer used.
if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT;
return incrR;
}
/// @deprecated: use integrateGyroMeasurement from now on
/// @note this returned hard-to-understand Jacobian D_incrR_integratedOmega.
void GTSAM_DEPRECATED integrateMeasurement(
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega, OptionalJacobian<3, 3> F);
#endif
/// @}
private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */

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

@ -18,47 +18,44 @@
* @author Varun Agrawal
*/
#include <gtsam/navigation/AHRSFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/debug.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/navigation/AHRSFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/slam/BetweenFactor.h>
#include <cmath>
#include <list>
#include <memory>
#include "gtsam/nonlinear/LevenbergMarquardtParams.h"
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;
using symbol_shorthand::R;
Vector3 kZeroOmegaCoriolis(0,0,0);
Vector3 kZeroOmegaCoriolis(0, 0, 0);
// Define covariance matrices
double accNoiseVar = 0.01;
const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3;
double gyroNoiseVar = 0.01;
const Matrix3 kMeasuredOmegaCovariance = gyroNoiseVar * I_3x3;
//******************************************************************************
namespace {
Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i,
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 Vector3& bias) {
return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3));
}
PreintegratedAhrsMeasurements evaluatePreintegratedMeasurements(
const Vector3& bias, const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3::Zero()) {
PreintegratedAhrsMeasurements result(bias, I_3x3);
PreintegratedAhrsMeasurements integrateMeasurements(
const Vector3& biasHat, const list<Vector3>& measuredOmegas,
const list<double>& deltaTs) {
PreintegratedAhrsMeasurements result(biasHat, I_3x3);
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
list<double>::const_iterator itDeltaT = deltaTs.begin();
@ -68,79 +65,59 @@ PreintegratedAhrsMeasurements evaluatePreintegratedMeasurements(
return result;
}
Rot3 evaluatePreintegratedMeasurementsRotation(
const Vector3& bias, const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3::Zero()) {
return Rot3(
evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
initialRotationRate).deltaRij());
}
Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
const double deltaT) {
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
}
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
}
}
} // namespace
//******************************************************************************
TEST( AHRSFactor, PreintegratedAhrsMeasurements ) {
TEST(AHRSFactor, PreintegratedAhrsMeasurements) {
// Linearization point
Vector3 bias(0,0,0); ///< Current estimate of angular rate bias
Vector3 biasHat(0, 0, 0); ///< Current estimate of angular rate bias
// Measurements
Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
double deltaT = 0.5;
// Expected preintegrated values
Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0);
double expectedDeltaT1(0.5);
Rot3 expectedDeltaR1 = Rot3::Roll(0.5 * M_PI / 100.0);
// Actual preintegrated values
PreintegratedAhrsMeasurements actual1(bias, Z_3x3);
PreintegratedAhrsMeasurements actual1(biasHat, kMeasuredOmegaCovariance);
actual1.integrateMeasurement(measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6);
DOUBLES_EQUAL(deltaT, actual1.deltaTij(), 1e-6);
// Check the covariance
Matrix3 expectedMeasCov = kMeasuredOmegaCovariance * deltaT;
EXPECT(assert_equal(expectedMeasCov, actual1.preintMeasCov(), 1e-6));
// Integrate again
Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0);
double expectedDeltaT2(1);
Rot3 expectedDeltaR2 = Rot3::Roll(2.0 * 0.5 * M_PI / 100.0);
// Actual preintegrated values
PreintegratedAhrsMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6);
DOUBLES_EQUAL(deltaT * 2, actual2.deltaTij(), 1e-6);
}
//******************************************************************************
TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) {
Matrix3 gyroscopeCovariance = Matrix3::Ones()*0.4;
TEST(AHRSFactor, PreintegratedAhrsMeasurementsConstructor) {
Matrix3 gyroscopeCovariance = I_3x3 * 0.4;
Vector3 omegaCoriolis(0.1, 0.5, 0.9);
PreintegratedRotationParams params(gyroscopeCovariance, omegaCoriolis);
Vector3 bias(1.0,2.0,3.0); ///< Current estimate of angular rate bias
Vector3 bias(1.0, 2.0, 3.0); ///< Current estimate of angular rate bias
Rot3 deltaRij(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
double deltaTij = 0.02;
Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5;
Matrix3 preintMeasCov = Matrix3::Ones()*0.2;
Matrix3 delRdelBiasOmega = I_3x3 * 0.5;
Matrix3 preintMeasCov = I_3x3 * 0.2;
PreintegratedAhrsMeasurements actualPim(
std::make_shared<PreintegratedRotationParams>(params),
bias,
deltaTij,
deltaRij,
delRdelBiasOmega,
preintMeasCov);
std::make_shared<PreintegratedRotationParams>(params), bias, deltaTij,
deltaRij, delRdelBiasOmega, preintMeasCov);
EXPECT(assert_equal(gyroscopeCovariance,
actualPim.p().getGyroscopeCovariance(), 1e-6));
EXPECT(assert_equal(omegaCoriolis,
*(actualPim.p().getOmegaCoriolis()), 1e-6));
EXPECT(
assert_equal(omegaCoriolis, *(actualPim.p().getOmegaCoriolis()), 1e-6));
EXPECT(assert_equal(bias, actualPim.biasHat(), 1e-6));
DOUBLES_EQUAL(deltaTij, actualPim.deltaTij(), 1e-6);
EXPECT(assert_equal(deltaRij, Rot3(actualPim.deltaRij()), 1e-6));
@ -151,196 +128,146 @@ TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) {
/* ************************************************************************* */
TEST(AHRSFactor, Error) {
// Linearization point
Vector3 bias(0.,0.,0.); // 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));
Vector3 bias(0., 0., 0.); // Bias
Rot3 Ri(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0));
Rot3 Rj(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0));
// Measurements
Vector3 measuredOmega;
measuredOmega << M_PI / 100, 0, 0;
Vector3 measuredOmega(M_PI / 100, 0, 0);
double deltaT = 1.0;
PreintegratedAhrsMeasurements pim(bias, Z_3x3);
PreintegratedAhrsMeasurements pim(bias, kMeasuredOmegaCovariance);
pim.integrateMeasurement(measuredOmega, deltaT);
// Create factor
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, {});
AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis, {});
Vector3 errorActual = factor.evaluateError(x1, x2, bias);
// Expected error
Vector3 errorExpected(3);
errorExpected << 0, 0, 0;
// Check value
Vector3 errorActual = factor.evaluateError(Ri, Rj, bias);
Vector3 errorExpected(0, 0, 0);
EXPECT(assert_equal(Vector(errorExpected), Vector(errorActual), 1e-6));
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector3, Rot3>(
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector3, Rot3>(
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
Matrix H3e = numericalDerivative11<Vector3, Vector3>(
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
// Actual Jacobians
Matrix H1a, H2a, H3a;
(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(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(H3e, H3a, 1e-5));
// 1e-5 needs to be added only when using quaternions for rotations
// Check Derivatives
Values values;
values.insert(R(1), Ri);
values.insert(R(2), Rj);
values.insert(B(1), bias);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6);
}
/* ************************************************************************* */
TEST(AHRSFactor, ErrorWithBiases) {
// Linearization point
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)));
Rot3 Ri(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
Rot3 Rj(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
// Measurements
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3);
double deltaT = 1.0;
PreintegratedAhrsMeasurements pim(Vector3(0,0,0),
Z_3x3);
PreintegratedAhrsMeasurements pim(Vector3(0, 0, 0), kMeasuredOmegaCovariance);
pim.integrateMeasurement(measuredOmega, deltaT);
// Create factor
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis);
Vector errorActual = factor.evaluateError(x1, x2, bias);
// Expected error
Vector errorExpected(3);
errorExpected << 0, 0, 0;
// Check value
Vector3 errorExpected(0, 0, 0);
Vector3 errorActual = factor.evaluateError(Ri, Rj, bias);
EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector, Rot3>(
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, Vector3>(
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias);
// Actual Jacobians
Matrix H1a, H2a, H3a;
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
EXPECT(assert_equal(H1e, H1a));
EXPECT(assert_equal(H2e, H2a));
EXPECT(assert_equal(H3e, H3a));
// Check Derivatives
Values values;
values.insert(R(1), Ri);
values.insert(R(2), Rj);
values.insert(B(1), bias);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6);
}
//******************************************************************************
TEST( AHRSFactor, PartialDerivativeExpmap ) {
TEST(AHRSFactor, PartialDerivativeExpmap) {
// Linearization point
Vector3 biasOmega(0,0,0);
Vector3 biasOmega(0, 0, 0);
// Measurements
Vector3 measuredOmega;
measuredOmega << 0.1, 0, 0;
Vector3 measuredOmega(0.1, 0, 0);
double deltaT = 0.5;
auto f = [&](const Vector3& biasOmega) {
return Rot3::Expmap((measuredOmega - biasOmega) * deltaT);
};
// Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(
std::bind(&evaluateRotation, measuredOmega, std::placeholders::_1, deltaT), biasOmega);
Matrix expectedH = numericalDerivative11<Rot3, Vector3>(f, biasOmega);
const Matrix3 Jr = Rot3::ExpmapDerivative(
(measuredOmega - biasOmega) * deltaT);
const Matrix3 Jr =
Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
Matrix3 actualH = -Jr * deltaT; // the delta bias appears with the minus sign
// Compare Jacobians
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3));
EXPECT(assert_equal(expectedH, actualH, 1e-3));
// 1e-3 needs to be added only when using quaternions for rotations
}
//******************************************************************************
TEST( AHRSFactor, PartialDerivativeLogmap ) {
TEST(AHRSFactor, PartialDerivativeLogmap) {
// Linearization point
Vector3 thetahat;
thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias
Vector3 thetaHat(0.1, 0.1, 0); ///< Current estimate of rotation rate bias
// Measurements
Vector3 deltatheta;
deltatheta << 0, 0, 0;
auto f = [thetaHat](const Vector3 deltaTheta) {
return Rot3::Logmap(
Rot3::Expmap(thetaHat).compose(Rot3::Expmap(deltaTheta)));
};
// Compute numerical derivatives
Matrix expectedDelFdeltheta = numericalDerivative11<Vector3, Vector3>(
std::bind(&evaluateLogRotation, thetahat, std::placeholders::_1), deltatheta);
Vector3 deltaTheta(0, 0, 0);
Matrix expectedH = numericalDerivative11<Vector3, Vector3>(f, deltaTheta);
const Vector3 x = thetahat; // parametrization of so(3)
const Vector3 x = thetaHat; // parametrization of so(3)
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
double normx = x.norm();
const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X
+ (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X
* X;
double norm = x.norm();
const Matrix3 actualH =
I_3x3 + 0.5 * X +
(1 / (norm * norm) - (1 + cos(norm)) / (2 * norm * sin(norm))) * X * X;
// Compare Jacobians
EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
EXPECT(assert_equal(expectedH, actualH));
}
//******************************************************************************
TEST( AHRSFactor, fistOrderExponential ) {
TEST(AHRSFactor, fistOrderExponential) {
// Linearization point
Vector3 biasOmega(0,0,0);
Vector3 biasOmega(0, 0, 0);
// Measurements
Vector3 measuredOmega;
measuredOmega << 0.1, 0, 0;
Vector3 measuredOmega(0.1, 0, 0);
double deltaT = 1.0;
// change w.r.t. linearization point
double alpha = 0.0;
Vector3 deltabiasOmega;
deltabiasOmega << alpha, alpha, alpha;
Vector3 deltaBiasOmega(alpha, alpha, alpha);
const Matrix3 Jr = Rot3::ExpmapDerivative(
(measuredOmega - biasOmega) * deltaT);
const Matrix3 Jr =
Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
Matrix3 delRdelBiasOmega =
-Jr * deltaT; // the delta bias appears with the minus sign
const Matrix expectedRot = Rot3::Expmap(
(measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
const Matrix expectedRot =
Rot3::Expmap((measuredOmega - biasOmega - deltaBiasOmega) * deltaT)
.matrix();
const Matrix3 hatRot =
Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
const Matrix3 actualRot = hatRot
* Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
const Matrix3 actualRot =
hatRot * Rot3::Expmap(delRdelBiasOmega * deltaBiasOmega).matrix();
// Compare Jacobians
EXPECT(assert_equal(expectedRot, actualRot));
}
//******************************************************************************
TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
TEST(AHRSFactor, FirstOrderPreIntegratedMeasurements) {
// Linearization point
Vector3 bias = Vector3::Zero(); ///< Current estimate of rotation rate bias
@ -361,98 +288,76 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
// Actual preintegrated values
PreintegratedAhrsMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
Vector3(M_PI / 100.0, 0.0, 0.0));
integrateMeasurements(bias, measuredOmegas, deltaTs);
auto f = [&](const Vector3& bias) {
return integrateMeasurements(bias, measuredOmegas, deltaTs).deltaRij();
};
// Compute numerical derivatives
Matrix expectedDelRdelBias =
numericalDerivative11<Rot3, Vector3>(
std::bind(&evaluatePreintegratedMeasurementsRotation, std::placeholders::_1,
measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias);
Matrix expectedDelRdelBias = numericalDerivative11<Rot3, Vector3>(f, bias);
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
// Compare Jacobians
EXPECT(
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3));
EXPECT(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 ) {
TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
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)));
Rot3 Ri(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0)));
Rot3 Rj(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
// Measurements
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3);
double deltaT = 1.0;
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
Point3(1, 0, 0));
PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance);
auto p = std::make_shared<PreintegratedAhrsMeasurements::Params>();
p->gyroscopeCovariance = kMeasuredOmegaCovariance;
p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(1, 2, 3)), Point3(1, 0, 0));
PreintegratedAhrsMeasurements pim(p, Vector3::Zero());
pim.integrateMeasurement(measuredOmega, deltaT);
// Check preintegrated covariance
EXPECT(assert_equal(kMeasuredAccCovariance, pim.preintMeasCov()));
EXPECT(assert_equal(kMeasuredOmegaCovariance, pim.preintMeasCov()));
// Create factor
AHRSFactor factor(X(1), X(2), B(1), pim, omegaCoriolis);
AHRSFactor factor(R(1), R(2), B(1), pim, omegaCoriolis);
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector, Rot3>(
std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1);
Matrix H2e = numericalDerivative11<Vector, Rot3>(
std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2);
Matrix H3e = numericalDerivative11<Vector, Vector3>(
std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias);
// Check rotation Jacobians
Matrix RH1e = numericalDerivative11<Rot3, Rot3>(
std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1);
Matrix RH2e = numericalDerivative11<Rot3, Rot3>(
std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2);
Matrix RH3e = numericalDerivative11<Rot3, Vector3>(
std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias);
// Actual Jacobians
Matrix H1a, H2a, H3a;
(void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a);
EXPECT(assert_equal(H1e, H1a));
EXPECT(assert_equal(H2e, H2a));
EXPECT(assert_equal(H3e, H3a));
// Check Derivatives
Values values;
values.insert(R(1), Ri);
values.insert(R(2), Rj);
values.insert(B(1), bias);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6);
}
//******************************************************************************
TEST (AHRSFactor, predictTest) {
Vector3 bias(0,0,0);
TEST(AHRSFactor, predictTest) {
Vector3 bias(0, 0, 0);
// Measurements
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0;
Vector3 measuredOmega(0, 0, M_PI / 10.0);
double deltaT = 0.2;
PreintegratedAhrsMeasurements pim(bias, kMeasuredAccCovariance);
PreintegratedAhrsMeasurements pim(bias, kMeasuredOmegaCovariance);
for (int i = 0; i < 1000; ++i) {
pim.integrateMeasurement(measuredOmega, deltaT);
}
// Check preintegrated covariance
Matrix expectedMeasCov(3,3);
expectedMeasCov = 200*kMeasuredAccCovariance;
Matrix expectedMeasCov(3, 3);
expectedMeasCov = 200 * kMeasuredOmegaCovariance;
EXPECT(assert_equal(expectedMeasCov, pim.preintMeasCov()));
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis);
// Predict
Rot3 x;
Rot3 expectedRot = Rot3::Ypr(20*M_PI, 0, 0);
Rot3 expectedRot = Rot3::Ypr(20 * M_PI, 0, 0);
Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis);
EXPECT(assert_equal(expectedRot, actualRot, 1e-6));
@ -462,22 +367,19 @@ TEST (AHRSFactor, predictTest) {
// Actual Jacobians
Matrix H;
(void) pim.predict(bias,H);
(void)pim.predict(bias, H);
EXPECT(assert_equal(expectedH, H, 1e-8));
}
//******************************************************************************
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Marginals.h>
TEST (AHRSFactor, graphTest) {
TEST(AHRSFactor, graphTest) {
// linearization point
Rot3 x1(Rot3::RzRyRx(0, 0, 0));
Rot3 x2(Rot3::RzRyRx(0, M_PI / 4, 0));
Vector3 bias(0,0,0);
Rot3 Ri(Rot3::RzRyRx(0, 0, 0));
Rot3 Rj(Rot3::RzRyRx(0, M_PI / 4, 0));
Vector3 bias(0, 0, 0);
// PreIntegrator
Vector3 biasHat(0, 0, 0);
PreintegratedAhrsMeasurements pim(biasHat, kMeasuredAccCovariance);
PreintegratedAhrsMeasurements pim(biasHat, kMeasuredOmegaCovariance);
// Pre-integrate measurements
Vector3 measuredOmega(0, M_PI / 20, 0);
@ -492,16 +394,88 @@ TEST (AHRSFactor, graphTest) {
pim.integrateMeasurement(measuredOmega, deltaT);
}
// pim.print("Pre integrated measurementes");
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
values.insert(X(1), x1);
values.insert(X(2), x2);
// pim.print("Pre integrated measurements");
AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis);
values.insert(R(1), Ri);
values.insert(R(2), Rj);
values.insert(B(1), bias);
graph.push_back(factor);
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
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>(R(2))));
}
/* ************************************************************************* */
TEST(AHRSFactor, bodyPSensorWithBias) {
using noiseModel::Diagonal;
int numRotations = 10;
const Vector3 noiseBetweenBiasSigma(3.0e-6, 3.0e-6, 3.0e-6);
SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma);
// Measurements in the sensor frame:
const double omega = 0.1;
const Vector3 realOmega(omega, 0, 0);
const Vector3 realBias(1, 2, 3); // large !
const Vector3 measuredOmega = realOmega + realBias;
auto p = std::make_shared<PreintegratedAhrsMeasurements::Params>();
p->body_P_sensor = Pose3(Rot3::Yaw(M_PI_2), Point3(0, 0, 0));
p->gyroscopeCovariance = 1e-8 * I_3x3;
double deltaT = 0.005;
// Specify noise values on priors
const Vector3 priorNoisePoseSigmas(0.001, 0.001, 0.001);
const Vector3 priorNoiseBiasSigmas(0.5e-1, 0.5e-1, 0.5e-1);
SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas);
SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
// Create a factor graph with priors on initial pose, velocity and bias
NonlinearFactorGraph graph;
Values values;
graph.addPrior(R(0), Rot3(), priorNoisePose);
values.insert(R(0), Rot3());
// The key to this test is that we specify the bias, in the sensor frame, as
// known a priori. We also create factors below that encode our assumption
// that this bias is constant over time In theory, after optimization, we
// should recover that same bias estimate
graph.addPrior(B(0), realBias, priorNoiseBias);
values.insert(B(0), realBias);
// Now add IMU factors and bias noise models
const Vector3 zeroBias(0, 0, 0);
for (int i = 1; i < numRotations; i++) {
PreintegratedAhrsMeasurements pim(p, realBias);
for (int j = 0; j < 200; ++j)
pim.integrateMeasurement(measuredOmega, deltaT);
// Create factors
graph.emplace_shared<AHRSFactor>(R(i - 1), R(i), B(i - 1), pim);
graph.emplace_shared<BetweenFactor<Vector3> >(B(i - 1), B(i), zeroBias,
biasNoiseModel);
values.insert(R(i), Rot3());
values.insert(B(i), realBias);
}
// Finally, optimize, and get bias at last time step
LevenbergMarquardtParams params;
// params.setVerbosityLM("SUMMARY");
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
const Vector3 biasActual = result.at<Vector3>(B(numRotations - 1));
// Bias should be a self-fulfilling prophesy:
EXPECT(assert_equal(realBias, biasActual, 1e-3));
// Check that the successive rotations are all `omega` apart:
for (int i = 0; i < numRotations; i++) {
Rot3 expectedRot = Rot3::Pitch(omega * i);
Rot3 actualRot = result.at<Rot3>(R(i));
EXPECT(assert_equal(expectedRot, actualRot, 1e-3));
}
}
//******************************************************************************

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

@ -410,33 +410,33 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) {
const Matrix3 Jr =
Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT);
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
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-9));
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualDelRdelBiasOmega, 1e-9));
}
/* ************************************************************************* */
TEST(ImuFactor, PartialDerivativeLogmap) {
// Linearization point
Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias
Vector3 thetaHat(0.1, 0.1, 0); // Current estimate of rotation rate bias
// Measurements
Vector3 deltatheta(0, 0, 0);
Vector3 deltaTheta(0, 0, 0);
auto evaluateLogRotation = [=](const Vector3 deltatheta) {
auto evaluateLogRotation = [=](const Vector3 delta) {
return Rot3::Logmap(
Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
Rot3::Expmap(thetaHat).compose(Rot3::Expmap(delta)));
};
// Compute numerical derivatives
Matrix expectedDelFdeltheta =
numericalDerivative11<Vector, Vector3>(evaluateLogRotation, deltatheta);
Matrix expectedDelFdelTheta =
numericalDerivative11<Vector, Vector3>(evaluateLogRotation, deltaTheta);
Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat);
Matrix3 actualDelFdelTheta = Rot3::LogmapDerivative(thetaHat);
// Compare Jacobians
EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta));
EXPECT(assert_equal(expectedDelFdelTheta, actualDelFdelTheta));
}
/* ************************************************************************* */
@ -450,8 +450,8 @@ TEST(ImuFactor, fistOrderExponential) {
// change w.r.t. linearization point
double alpha = 0.0;
Vector3 deltabiasOmega;
deltabiasOmega << alpha, alpha, alpha;
Vector3 deltaBiasOmega;
deltaBiasOmega << alpha, alpha, alpha;
const Matrix3 Jr = Rot3::ExpmapDerivative(
(measuredOmega - biasOmega) * deltaT);
@ -459,13 +459,12 @@ TEST(ImuFactor, fistOrderExponential) {
Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign
const Matrix expectedRot = Rot3::Expmap(
(measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix();
(measuredOmega - biasOmega - deltaBiasOmega) * deltaT).matrix();
const Matrix3 hatRot =
Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix();
const Matrix3 actualRot = hatRot
* Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix();
// hatRot * (I_3x3 + skewSymmetric(delRdelBiasOmega * deltabiasOmega));
* Rot3::Expmap(delRdelBiasOmega * deltaBiasOmega).matrix();
// This is a first order expansion so the equality is only an approximation
EXPECT(assert_equal(expectedRot, actualRot));
@ -728,7 +727,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
using noiseModel::Diagonal;
typedef Bias Bias;
int numFactors = 10;
int numPoses = 10;
Vector6 noiseBetweenBiasSigma;
noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6,
3.0e-6, 3.0e-6);
@ -761,7 +760,7 @@ TEST(ImuFactor, bodyPSensorWithBias) {
SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas);
Vector3 zeroVel(0, 0, 0);
// Create a factor graph with priors on initial pose, vlocity and bias
// Create a factor graph with priors on initial pose, velocity and bias
NonlinearFactorGraph graph;
Values values;
@ -780,9 +779,8 @@ TEST(ImuFactor, bodyPSensorWithBias) {
// Now add IMU factors and bias noise models
Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0));
for (int i = 1; i < numFactors; i++) {
PreintegratedImuMeasurements pim = PreintegratedImuMeasurements(p,
priorBias);
for (int i = 1; i < numPoses; i++) {
PreintegratedImuMeasurements pim(p, priorBias);
for (int j = 0; j < 200; ++j)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -796,8 +794,8 @@ TEST(ImuFactor, bodyPSensorWithBias) {
}
// Finally, optimize, and get bias at last time step
Values results = LevenbergMarquardtOptimizer(graph, values).optimize();
Bias biasActual = results.at<Bias>(B(numFactors - 1));
Values result = LevenbergMarquardtOptimizer(graph, values).optimize();
Bias biasActual = result.at<Bias>(B(numPoses - 1));
// And compare it with expected value (our prior)
Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0));
@ -851,11 +849,11 @@ struct ImuFactorMergeTest {
actual_pim02.preintegrated(), tol));
EXPECT(assert_equal(pim02_expected, actual_pim02, tol));
ImuFactor::shared_ptr factor01 =
auto factor01 =
std::make_shared<ImuFactor>(X(0), V(0), X(1), V(1), B(0), pim01);
ImuFactor::shared_ptr factor12 =
auto factor12 =
std::make_shared<ImuFactor>(X(1), V(1), X(2), V(2), B(0), pim12);
ImuFactor::shared_ptr factor02_expected = std::make_shared<ImuFactor>(
auto factor02_expected = std::make_shared<ImuFactor>(
X(0), V(0), X(2), V(2), B(0), pim02_expected);
ImuFactor::shared_ptr factor02_merged = ImuFactor::Merge(factor01, factor12);

53
gtsam/navigation/tests/testManifoldPreintegration.cpp
View File

@ -109,6 +109,59 @@ TEST(ManifoldPreintegration, computeError) {
EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9));
}
/* ************************************************************************* */
TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) {
// Create a PreintegratedRotation object
auto p = std::make_shared<PreintegratedRotationParams>();
PreintegratedRotation pim(p);
// Integrate a single measurement
const double omega = 0.1;
const Vector3 trueOmega(omega, 0, 0);
const Vector3 bias(1, 2, 3);
const Vector3 measuredOmega = trueOmega + bias;
const double deltaT = 0.5;
// Check the accumulated rotation.
Rot3 expected = Rot3::Roll(omega * deltaT);
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT);
EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
// Now do the same for a ManifoldPreintegration object
imuBias::ConstantBias biasHat {Z_3x1, bias};
ManifoldPreintegration manifoldPim(testing::Params(), biasHat);
manifoldPim.integrateMeasurement(Z_3x1, measuredOmega, deltaT);
EXPECT(assert_equal(expected, manifoldPim.deltaRij(), 1e-9));
// Check their internal Jacobians are the same:
EXPECT(assert_equal(pim.delRdelBiasOmega(), manifoldPim.delRdelBiasOmega()));
// Check PreintegratedRotation::biascorrectedDeltaRij.
Matrix3 H;
const double delta = 0.05;
const Vector3 biasOmegaIncr(delta, 0, 0);
Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected));
const Rot3 expected2 = Rot3::Roll((omega - delta) * deltaT);
EXPECT(assert_equal(expected2, corrected, 1e-9));
// Check ManifoldPreintegration::biasCorrectedDelta.
imuBias::ConstantBias bias_i {Z_3x1, bias + biasOmegaIncr};
Matrix96 H2;
Vector9 biasCorrected = manifoldPim.biasCorrectedDelta(bias_i, H2);
Vector9 expected3;
expected3 << Rot3::Logmap(expected2), Z_3x1, Z_3x1;
EXPECT(assert_equal(expected3, biasCorrected, 1e-9));
// Check that this one is sane:
auto g = [&](const Vector3& increment) {
return manifoldPim.biasCorrectedDelta({Z_3x1, bias + increment}, {});
};
EXPECT(assert_equal<Matrix>(numericalDerivative11<Vector9, Vector3>(g, Z_3x1),
H2.rightCols<3>(),
1e-4)); // NOTE(frank): reduced tolerance
}
/* ************************************************************************* */
int main() {
TestResult tr;

138
gtsam/navigation/tests/testPreintegratedRotation.cpp Normal file
View File

@ -0,0 +1,138 @@
/* ----------------------------------------------------------------------------
* 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 testPreintegratedRotation.cpp
* @brief Unit test for PreintegratedRotation
* @author Frank Dellaert
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/navigation/PreintegratedRotation.h>
#include <memory>
#include "gtsam/base/Matrix.h"
#include "gtsam/base/Vector.h"
using namespace gtsam;
//******************************************************************************
// Example where gyro measures small rotation about x-axis, with bias.
namespace biased_x_rotation {
const double omega = 0.1;
const Vector3 trueOmega(omega, 0, 0);
const Vector3 bias(1, 2, 3);
const Vector3 measuredOmega = trueOmega + bias;
const double deltaT = 0.5;
} // namespace biased_x_rotation
// Create params where x and y axes are exchanged.
static std::shared_ptr<PreintegratedRotationParams> paramsWithTransform() {
auto p = std::make_shared<PreintegratedRotationParams>();
p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}});
return p;
}
//******************************************************************************
TEST(PreintegratedRotation, integrateGyroMeasurement) {
// Example where IMU is identical to body frame, then omega is roll
using namespace biased_x_rotation;
auto p = std::make_shared<PreintegratedRotationParams>();
PreintegratedRotation pim(p);
// Check the value.
Matrix3 H_bias;
internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()};
const Rot3 incrR = f(bias, H_bias);
Rot3 expected = Rot3::Roll(omega * deltaT);
EXPECT(assert_equal(expected, incrR, 1e-9));
// Check the derivative:
EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias));
// Check value of deltaRij() after integration.
Matrix3 F;
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F);
EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
// Check that system matrix F is the first derivative of compose:
EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F));
// Make sure delRdelBiasOmega is H_bias after integration.
EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
// Check if we make a correction to the bias, the value and Jacobian are
// correct. Note that the bias is subtracted from the measurement, and the
// integration time is taken into account, so we expect -deltaT*delta change.
Matrix3 H;
const double delta = 0.05;
const Vector3 biasOmegaIncr(delta, 0, 0);
Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected));
EXPECT(assert_equal(Rot3::Roll((omega - delta) * deltaT), corrected, 1e-9));
// TODO(frank): again the derivative is not the *sane* one!
// auto g = [&](const Vector3& increment) {
// return pim.biascorrectedDeltaRij(increment, {});
// };
// EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(g, Z_3x1), H));
}
//******************************************************************************
TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) {
// Example where IMU is rotated, so measured omega indicates pitch.
using namespace biased_x_rotation;
auto p = paramsWithTransform();
PreintegratedRotation pim(p);
// Check the value.
Matrix3 H_bias;
internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()};
Rot3 expected = Rot3::Pitch(omega * deltaT);
EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9));
// Check the derivative:
EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias));
// Check value of deltaRij() after integration.
Matrix3 F;
pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F);
EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
// Check that system matrix F is the first derivative of compose:
EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F));
// Make sure delRdelBiasOmega is H_bias after integration.
EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
// Check the bias correction in same way, but will now yield pitch change.
Matrix3 H;
const double delta = 0.05;
const Vector3 biasOmegaIncr(delta, 0, 0);
Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H);
EQUALITY(Vector3(0, -deltaT * delta, 0), expected.logmap(corrected));
EXPECT(assert_equal(Rot3::Pitch((omega - delta) * deltaT), corrected, 1e-9));
// TODO(frank): again the derivative is not the *sane* one!
// auto g = [&](const Vector3& increment) {
// return pim.biascorrectedDeltaRij(increment, {});
// };
// EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(g, Z_3x1), H));
}
//******************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
//******************************************************************************