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

Function object with sane Jacobian

release/4.3a0
Frank Dellaert 2024-06-09 08:59:48 -07:00
parent d290f83268
commit 2dad81d671
3 changed files with 83 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -68,17 +68,15 @@ 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 {
Rot3 PreintegratedRotation::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();
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;
}
@ -86,7 +84,10 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
// 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 !!
Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias); // expensive !!
if (H_bias)
*H_bias *= -deltaT; // Correct so accurately reflects bias derivative
return incrR;
}
void PreintegratedRotation::integrateMeasurement(
@ -94,8 +95,8 @@ void PreintegratedRotation::integrateMeasurement(
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);
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) {

48
gtsam/navigation/PreintegratedRotation.h
View File

@ -21,9 +21,9 @@
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/std_optional_serialization.h>
#include <gtsam/geometry/Pose3.h>
namespace gtsam {
@ -159,19 +159,25 @@ class GTSAM_EXPORT PreintegratedRotation {
/// @{
/**
* @brief 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.
* @brief Function object for incremental rotation.
* @param measuredOmega The measured angular velocity (as given by the sensor)
* @param biasHat The bias estimate
* @param deltaT The time interval
* @param D_incrR_integratedOmega Jacobian of the incremental rotation w.r.t.
* delta_T * (measuredOmega - biasHat), possibly rotated by body_R_sensor.
* @return The incremental rotation
* @param deltaT The time interval over which the rotation is integrated.
* @param body_P_sensor Optional transform between body and IMU.
*/
Rot3 incrementalRotation(
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
struct 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& biasHat,
OptionalJacobian<3, 3> H_bias = {}) const;
};
/**
* @brief Calculate an incremental rotation given the gyro measurement and a
@ -198,6 +204,24 @@ 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& biasHat, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor};
Rot3 incrR = f(biasHat, D_incrR_integratedOmega);
// Backwards compatible "weird" Jacobian, no longer used.
if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT;
return incrR;
}
#endif
/// @}
private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/** Serialization function */

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

@ -30,40 +30,36 @@ using namespace gtsam;
namespace simple_roll {
auto p = std::make_shared<PreintegratedRotationParams>();
PreintegratedRotation pim(p);
const double roll = 0.1;
const Vector3 measuredOmega(roll, 0, 0);
const Vector3 biasHat(0, 0, 0);
const double omega = 0.1;
const Vector3 measuredOmega(omega, 0, 0);
const Vector3 bias(0, 0, 0);
const double deltaT = 0.5;
} // namespace simple_roll
//******************************************************************************
TEST(PreintegratedRotation, incrementalRotation) {
TEST(PreintegratedRotation, IncrementalRotation) {
using namespace simple_roll;
// Check the value.
Matrix3 D_incrR_integratedOmega;
const Rot3 incrR = pim.incrementalRotation(measuredOmega, biasHat, deltaT,
D_incrR_integratedOmega);
Rot3 expected = Rot3::Roll(roll * deltaT);
Matrix3 H_bias;
PreintegratedRotation::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));
// Lambda for numerical derivative:
auto f = [&](const Vector3& x, const Vector3& y) {
// Check the derivative:
EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias));
// Ephemeral test for deprecated Jacobian:
Matrix3 D_incrR_integratedOmega;
(void)pim.incrementalRotation(measuredOmega, bias, deltaT,
D_incrR_integratedOmega);
auto g = [&](const Vector3& x, const Vector3& y) {
return pim.incrementalRotation(x, y, deltaT, {});
};
// NOTE(frank): these derivatives as computed by the function violate the
// "Jacobian contract". We should refactor this. It's not clear that the
// deltaT factor is actually understood in calling code.
// Check derivative with respect to measuredOmega
EXPECT(assert_equal<Matrix3>(
numericalDerivative21<Rot3, Vector3, Vector3>(f, measuredOmega, biasHat),
deltaT * D_incrR_integratedOmega));
// Check derivative with respect to biasHat
EXPECT(assert_equal<Matrix3>(
numericalDerivative22<Rot3, Vector3, Vector3>(f, measuredOmega, biasHat),
numericalDerivative22<Rot3, Vector3, Vector3>(g, measuredOmega, bias),
-deltaT * D_incrR_integratedOmega));
}
@ -77,40 +73,35 @@ static std::shared_ptr<PreintegratedRotationParams> paramsWithTransform() {
namespace roll_in_rotated_frame {
auto p = paramsWithTransform();
PreintegratedRotation pim(p);
const double roll = 0.1;
const Vector3 measuredOmega(roll, 0, 0);
const Vector3 biasHat(0, 0, 0);
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
//******************************************************************************
TEST(PreintegratedRotation, incrementalRotationWithTransform) {
TEST(PreintegratedRotation, IncrementalRotationWithTransform) {
using namespace roll_in_rotated_frame;
// Check the value.
Matrix3 D_incrR_integratedOmega;
const Rot3 incrR = pim.incrementalRotation(measuredOmega, biasHat, deltaT,
D_incrR_integratedOmega);
Rot3 expected = Rot3::Pitch(roll * deltaT);
EXPECT(assert_equal(expected, incrR, 1e-9));
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));
// Lambda for numerical derivative:
auto f = [&](const Vector3& x, const Vector3& y) {
// Check the derivative:
EXPECT(assert_equal(numericalDerivative11<Rot3, Vector3>(f, bias), H_bias));
// Ephemeral test for deprecated Jacobian:
Matrix3 D_incrR_integratedOmega;
(void)pim.incrementalRotation(measuredOmega, bias, deltaT,
D_incrR_integratedOmega);
auto g = [&](const Vector3& x, const Vector3& y) {
return pim.incrementalRotation(x, y, deltaT, {});
};
// NOTE(frank): Here, once again, the derivatives are weird, as they do not
// take the rotation into account. They *are* the derivatives of the rotated
// omegas, but not the derivatives with respect to the function arguments.
// Check derivative with respect to measuredOmega
EXPECT(assert_equal<Matrix3>(
numericalDerivative21<Rot3, Vector3, Vector3>(f, measuredOmega, biasHat),
deltaT * D_incrR_integratedOmega));
// Check derivative with respect to biasHat
EXPECT(assert_equal<Matrix3>(
numericalDerivative22<Rot3, Vector3, Vector3>(f, measuredOmega, biasHat),
numericalDerivative22<Rot3, Vector3, Vector3>(g, measuredOmega, bias),
-deltaT * D_incrR_integratedOmega));
}