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

Fixed derivative with transform, deprecated integrateMeasurement

release/4.3a0
Frank Dellaert 2024-06-09 10:07:57 -07:00
parent 2dad81d671
commit b531df7004
4 changed files with 155 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/navigation/AHRSFactor.cpp
View File

@ -49,10 +49,9 @@ void PreintegratedAhrsMeasurements::resetIntegration() {
//------------------------------------------------------------------------------
void PreintegratedAhrsMeasurements::integrateMeasurement(
const Vector3& measuredOmega, double deltaT) {
Matrix3 D_incrR_integratedOmega, Fr;
PreintegratedRotation::integrateMeasurement(measuredOmega,
biasHat_, deltaT, &D_incrR_integratedOmega, &Fr);
Matrix3 Fr;
PreintegratedRotation::integrateGyroMeasurement(measuredOmega, biasHat_,
deltaT, &Fr);
// First order uncertainty propagation
// The deltaT allows to pass from continuous time noise to discrete time

45
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -74,34 +74,32 @@ Rot3 PreintegratedRotation::IncrementalRotation::operator()(
Vector3 correctedOmega = measuredOmega - bias;
// 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. If Jacobian is
// requested, the rotation matrix is obtained as `rotate` Jacobian.
Matrix3 body_R_sensor;
if (body_P_sensor) {
const Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
// 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;
Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias); // expensive !!
if (H_bias)
if (H_bias) {
*H_bias *= -deltaT; // Correct so accurately reflects bias derivative
if (body_P_sensor) *H_bias *= body_R_sensor;
}
return incrR;
}
void PreintegratedRotation::integrateMeasurement(
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
void PreintegratedRotation::integrateGyroMeasurement(
const Vector3& measuredOmega, const Vector3& bias, double deltaT,
OptionalJacobian<3, 3> F) {
Matrix3 D_incrR_integratedOmega;
Matrix3 H_bias;
IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
const Rot3 incrR = f(biasHat, D_incrR_integratedOmega);
// If asked, pass first derivative as well
if (optional_D_incrR_integratedOmega) {
*optional_D_incrR_integratedOmega << D_incrR_integratedOmega;
}
const Rot3 incrR = f(bias, H_bias);
// Update deltaTij and rotation
deltaTij_ += deltaT;
@ -109,8 +107,23 @@ void PreintegratedRotation::integrateMeasurement(
// Update Jacobian
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ =
incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT;
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + H_bias;
}
// deprecated!
void PreintegratedRotation::integrateMeasurement(
const Vector3& measuredOmega, const Vector3& bias, double deltaT,
OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
OptionalJacobian<3, 3> F) {
integrateGyroMeasurement(measuredOmega, bias, deltaT, F);
// If asked, pass obsolete Jacobians as well
if (optional_D_incrR_integratedOmega) {
Matrix3 H_bias;
IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
const Rot3 incrR = f(bias, H_bias);
*optional_D_incrR_integratedOmega << H_bias / -deltaT;
}
}
Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,

24
gtsam/navigation/PreintegratedRotation.h
View File

@ -175,7 +175,7 @@ class GTSAM_EXPORT PreintegratedRotation {
* @param H_bias Jacobian of the rotation w.r.t. bias.
* @return The incremental rotation
*/
Rot3 operator()(const Vector3& biasHat,
Rot3 operator()(const Vector3& bias,
OptionalJacobian<3, 3> H_bias = {}) const;
};
@ -183,16 +183,12 @@ class GTSAM_EXPORT PreintegratedRotation {
* @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 biasHat The bias estimate
* @param bias The bias estimate
* @param deltaT The time interval
* @param D_incrR_integratedOmega Optional Jacobian of the incremental
* rotation w.r.t. the integrated angular velocity
* @param F Optional Jacobian of the incremental rotation w.r.t. the bias
* estimate
* @param F Jacobian of internal compose, used in AhrsFactor.
*/
void integrateMeasurement(const Vector3& measuredOmega,
const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega = {},
void integrateGyroMeasurement(const Vector3& measuredOmega,
const Vector3& bias, double deltaT,
OptionalJacobian<3, 3> F = {});
/// Return a bias corrected version of the integrated rotation, with optional Jacobian
@ -210,14 +206,20 @@ class GTSAM_EXPORT PreintegratedRotation {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
/// @deprecated: use IncrementalRotation functor with sane Jacobian
inline Rot3 GTSAM_DEPRECATED incrementalRotation(
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
const Vector3& measuredOmega, const Vector3& bias, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
Rot3 incrR = f(biasHat, D_incrR_integratedOmega);
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: returned hard-to-understand Jacobian D_incrR_integratedOmega.
void GTSAM_DEPRECATED integrateMeasurement(
const Vector3& measuredOmega, const Vector3& bias, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega, OptionalJacobian<3, 3> F);
#endif
/// @}

139
gtsam/navigation/tests/testPreintegratedRotation.cpp
View File

@ -27,18 +27,28 @@
using namespace gtsam;
//******************************************************************************
namespace simple_roll {
auto p = std::make_shared<PreintegratedRotationParams>();
PreintegratedRotation pim(p);
// Example where gyro measures small rotation about x-axis, with bias.
namespace biased_x_rotation {
const double omega = 0.1;
const Vector3 measuredOmega(omega, 0, 0);
const Vector3 bias(0, 0, 0);
const Vector3 trueOmega(omega, 0, 0);
const Vector3 bias(1, 2, 3);
const Vector3 measuredOmega = trueOmega + bias;
const double deltaT = 0.5;
} // namespace simple_roll
} // 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, IncrementalRotation) {
using namespace simple_roll;
TEST(PreintegratedRotation, integrateMeasurement) {
// 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;
@ -51,6 +61,32 @@ TEST(PreintegratedRotation, IncrementalRotation) {
// 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()));
}
//******************************************************************************
TEST(PreintegratedRotation, Deprecated) {
// 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;
PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT,
p->getBodyPSensor()};
Rot3 expected = Rot3::Roll(omega * deltaT);
EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9));
// Ephemeral test for deprecated Jacobian:
Matrix3 D_incrR_integratedOmega;
(void)pim.incrementalRotation(measuredOmega, bias, deltaT,
@ -58,30 +94,32 @@ TEST(PreintegratedRotation, IncrementalRotation) {
auto g = [&](const Vector3& x, const Vector3& y) {
return pim.incrementalRotation(x, y, deltaT, {});
};
EXPECT(assert_equal<Matrix3>(
numericalDerivative22<Rot3, Vector3, Vector3>(g, measuredOmega, bias),
-deltaT * D_incrR_integratedOmega));
}
const Matrix3 oldJacobian =
numericalDerivative22<Rot3, Vector3, Vector3>(g, measuredOmega, bias);
EXPECT(assert_equal<Matrix3>(oldJacobian, -deltaT * D_incrR_integratedOmega));
//******************************************************************************
static std::shared_ptr<PreintegratedRotationParams> paramsWithTransform() {
auto p = std::make_shared<PreintegratedRotationParams>();
p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}});
return p;
}
// Check deprecated version.
Matrix3 D_incrR_integratedOmega2, F;
pim.integrateMeasurement(measuredOmega, bias, deltaT,
D_incrR_integratedOmega2, F);
EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9));
namespace roll_in_rotated_frame {
auto p = paramsWithTransform();
PreintegratedRotation pim(p);
const double omega = 0.1;
const Vector3 measuredOmega(omega, 0, 0);
const Vector3 bias(0, 0, 0);
const double deltaT = 0.5;
} // namespace roll_in_rotated_frame
// Check that system matrix F is the first derivative of compose:
EXPECT(assert_equal<Matrix3>(pim.deltaRij().inverse().AdjointMap(), F));
// Check that deprecated Jacobian is correct.
EXPECT(assert_equal(D_incrR_integratedOmega, D_incrR_integratedOmega2, 1e-9));
// Make sure delRdelBiasOmega is H_bias after integration.
EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
}
//******************************************************************************
TEST(PreintegratedRotation, IncrementalRotationWithTransform) {
using namespace roll_in_rotated_frame;
// 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;
@ -93,6 +131,32 @@ TEST(PreintegratedRotation, IncrementalRotationWithTransform) {
// 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()));
}
//******************************************************************************
TEST(PreintegratedRotation, DeprecatedWithTransform) {
// 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;
PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT,
p->getBodyPSensor()};
Rot3 expected = Rot3::Pitch(omega * deltaT);
EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9));
// Ephemeral test for deprecated Jacobian:
Matrix3 D_incrR_integratedOmega;
(void)pim.incrementalRotation(measuredOmega, bias, deltaT,
@ -100,9 +164,24 @@ TEST(PreintegratedRotation, IncrementalRotationWithTransform) {
auto g = [&](const Vector3& x, const Vector3& y) {
return pim.incrementalRotation(x, y, deltaT, {});
};
EXPECT(assert_equal<Matrix3>(
numericalDerivative22<Rot3, Vector3, Vector3>(g, measuredOmega, bias),
-deltaT * D_incrR_integratedOmega));
const Matrix3 oldJacobian =
numericalDerivative22<Rot3, Vector3, Vector3>(g, measuredOmega, bias);
EXPECT(assert_equal<Matrix3>(oldJacobian, -deltaT * D_incrR_integratedOmega));
// Check deprecated version.
Matrix3 D_incrR_integratedOmega2, F;
pim.integrateMeasurement(measuredOmega, bias, deltaT,
D_incrR_integratedOmega2, 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));
// Check that deprecated Jacobian is correct.
EXPECT(assert_equal(D_incrR_integratedOmega, D_incrR_integratedOmega2, 1e-9));
// Make sure delRdelBiasOmega is H_bias after integration.
EXPECT(assert_equal<Matrix3>(H_bias, pim.delRdelBiasOmega()));
}
//******************************************************************************