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

biascorrectedDeltaRij tests

release/4.3a0
Frank Dellaert 2024-06-09 12:24:45 -07:00
parent 1ac286f97a
commit 0a3fdcc3a4
4 changed files with 71 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -95,11 +95,11 @@ Rot3 PreintegratedRotation::IncrementalRotation::operator()(
}
void PreintegratedRotation::integrateGyroMeasurement(
const Vector3& measuredOmega, const Vector3& bias, double deltaT,
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> F) {
Matrix3 H_bias;
IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
const Rot3 incrR = f(bias, H_bias);
const Rot3 incrR = f(biasHat, H_bias);
// Update deltaTij and rotation
deltaTij_ += deltaT;
@ -112,16 +112,16 @@ void PreintegratedRotation::integrateGyroMeasurement(
// deprecated!
void PreintegratedRotation::integrateMeasurement(
const Vector3& measuredOmega, const Vector3& bias, double deltaT,
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
OptionalJacobian<3, 3> F) {
integrateGyroMeasurement(measuredOmega, bias, deltaT, F);
integrateGyroMeasurement(measuredOmega, biasHat, 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);
const Rot3 incrR = f(biasHat, H_bias);
*optional_D_incrR_integratedOmega << H_bias / -deltaT;
}
}

69
gtsam/navigation/PreintegratedRotation.h
View File

@ -135,18 +135,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
@ -158,6 +150,35 @@ class GTSAM_EXPORT PreintegratedRotation {
/// @name Main functionality
/// @{
/**
* @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 Jacobian of internal compose, used in AhrsFactor.
*/
void integrateGyroMeasurement(const Vector3& measuredOmega,
const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> F = {});
/**
* @brief Return a bias corrected version of the integrated rotation.
* @param biasOmegaIncr An increment with respect to biasHat used above.
* @param H 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;
/// Integrate coriolis correction in body frame rot_i
Vector3 integrateCoriolis(const Rot3& rot_i) const;
/// @}
/// @name Internal, exposed for testing only
/// @{
/**
* @brief Function object for incremental rotation.
* @param measuredOmega The measured angular velocity (as given by the sensor)
@ -179,25 +200,6 @@ class GTSAM_EXPORT PreintegratedRotation {
OptionalJacobian<3, 3> H_bias = {}) const;
};
/**
* @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 bias estimate
* @param deltaT The time interval
* @param F Jacobian of internal compose, used in AhrsFactor.
*/
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
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
OptionalJacobian<3, 3> H = {}) const;
/// Integrate coriolis correction in body frame rot_i
Vector3 integrateCoriolis(const Rot3& rot_i) const;
/// @}
/// @name Deprecated API
@ -215,9 +217,10 @@ class GTSAM_EXPORT PreintegratedRotation {
return incrR;
}
/// @deprecated: returned hard-to-understand Jacobian D_incrR_integratedOmega.
/// @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& bias, double deltaT,
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega, OptionalJacobian<3, 3> F);
#endif

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

@ -301,29 +301,6 @@ TEST(AHRSFactor, FirstOrderPreIntegratedMeasurements) {
// 1e-3 needs to be added only when using quaternions for rotations
}
//******************************************************************************
// TEST(AHRSFactor, PimWithBodyDisplacement) {
// Vector3 bias(0, 0, 0.3);
// Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3);
// double deltaT = 1.0;
// auto p = std::make_shared<PreintegratedAhrsMeasurements::Params>();
// p->gyroscopeCovariance = kMeasuredOmegaCovariance;
// p->body_P_sensor = Pose3(Rot3::Roll(M_PI_2), Point3(0, 0, 0));
// PreintegratedAhrsMeasurements pim(p, Vector3::Zero());
// pim.integrateMeasurement(measuredOmega, deltaT);
// Vector3 biasOmegaIncr(0.01, 0.0, 0.0);
// Matrix3 actualH;
// pim.biascorrectedDeltaRij(biasOmegaIncr, actualH);
// // Numerical derivative using a lambda function:
// auto f = [&](const Vector3& bias) { return pim.biascorrectedDeltaRij(bias); };
// Matrix3 expectedH = numericalDerivative11<Rot3, Vector3>(f, bias);
// EXPECT(assert_equal(expectedH, actualH));
// }
//******************************************************************************
TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
Vector3 bias(0, 0, 0.3);

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

@ -71,6 +71,22 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) {
// 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));
}
//******************************************************************************
@ -100,6 +116,20 @@ TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) {
// 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));
}
//******************************************************************************