Test incrementalRotation

gtsam/navigation/PreintegratedRotation.cpp

@@ -89,13 +89,13 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
   return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
 }
 
-void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
-    const Vector3& biasHat, double deltaT,
+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);
+                                         D_incrR_integratedOmega);
 
   // If asked, pass first derivative as well
   if (optional_D_incrR_integratedOmega) {
@@ -108,8 +108,8 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
 
   // Update Jacobian
   const Matrix3 incrRt = incrR.transpose();
-  delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
-      - D_incrR_integratedOmega * deltaT;
+  delRdelBiasOmega_ =
+      incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT;
 }
 
 Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,

gtsam/navigation/PreintegratedRotation.h

@@ -65,7 +65,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);
 
@@ -159,15 +158,34 @@ 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;
+  /**
+   * @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.
+   * @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
+   */
+  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,
+  /**
+   * @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 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
+   */
+  void integrateMeasurement(const Vector3& measuredOmega,
+                            const Vector3& biasHat, double deltaT,
                             OptionalJacobian<3, 3> D_incrR_integratedOmega = {},
                             OptionalJacobian<3, 3> F = {});
 

gtsam/navigation/tests/testPreintegratedRotation.cpp

@@ -0,0 +1,122 @@
+/* ----------------------------------------------------------------------------
+
+ * 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;
+
+//******************************************************************************
+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 deltaT = 0.5;
+}  // namespace simple_roll
+
+//******************************************************************************
+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);
+  EXPECT(assert_equal(expected, incrR, 1e-9));
+
+  // Lambda for numerical derivative:
+  auto f = [&](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),
+      -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;
+}
+
+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 deltaT = 0.5;
+}  // namespace roll_in_rotated_frame
+
+//******************************************************************************
+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));
+
+  // Lambda for numerical derivative:
+  auto f = [&](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),
+      -deltaT * D_incrR_integratedOmega));
+}
+
+//******************************************************************************
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+//******************************************************************************