Fix inheritance

Author: dellaert

gtsam/navigation/navigation.i

@@ -847,7 +847,7 @@ class LeggedInvariantEKF : gtsam::LeggedEstimator {
   size_t numFeet() const;
 };
 
-class LeggedInvariantIEKF : gtsam::LeggedEstimator {
+class LeggedInvariantIEKF : gtsam::LeggedInvariantEKF {
   LeggedInvariantIEKF(const gtsam::NavState& navState0,
                       const gtsam::Matrix& footholds0,
                       const gtsam::Matrix& P0,

python/gtsam/tests/test_LeggedEstimator.py

@@ -26,6 +26,40 @@ def make_simple_estimator():
     )
 
 
+def make_estimator(variant: str):
+    params = make_legged_estimator_params()
+    params.useFullContactInitialization = False
+
+    nav_state0 = gtsam.NavState(
+        gtsam.Rot3(), np.array([0.0, 0.0, 0.12]), np.array([0.0, 0.0, -0.12])
+    )
+    footholds0 = np.zeros((3, 4), dtype=float)
+    foot_names = ["lf", "rf", "lh", "rh"]
+
+    if variant in ("invariant_ekf", "invariant_graph"):
+        covariance0 = np.zeros((21, 21), dtype=float)
+        covariance0[:9, :9] = np.eye(9) * 1e-3
+        covariance0[9:, 9:] = np.eye(12) * (params.footholdInitSigma**2)
+        if variant == "invariant_ekf":
+            return gtsam.LeggedInvariantEKF(
+                nav_state0, footholds0, covariance0, params, foot_names
+            )
+        return gtsam.LeggedInvariantIEKF(
+            nav_state0, footholds0, covariance0, params, foot_names
+        )
+
+    base_covariance0 = np.eye(9) * 1e-3
+    if variant == "fixed_lag_single_bias":
+        return gtsam.LeggedFixedLagSmoother(
+            nav_state0, footholds0, base_covariance0, params, 1.0, foot_names
+        )
+    if variant == "fixed_lag_combined_bias":
+        return gtsam.LeggedCombinedFixedLagSmoother(
+            nav_state0, footholds0, base_covariance0, params, 1.0, foot_names
+        )
+    raise ValueError(f"unknown estimator variant: {variant}")
+
+
 def single_contact_update(estimator, terrain_height):
     if terrain_height is None:
         estimator.turnHeightPriorOff()
@@ -42,6 +76,25 @@ def single_contact_update(estimator, terrain_height):
 
 
 class TestLeggedEstimator(unittest.TestCase):
+    def test_wrapper_inheritance_uses_base_height_prior_methods(self):
+        self.assertIs(gtsam.LeggedInvariantIEKF.__mro__[1], gtsam.LeggedInvariantEKF)
+
+        variants = (
+            "invariant_ekf",
+            "invariant_graph",
+            "fixed_lag_single_bias",
+            "fixed_lag_combined_bias",
+        )
+        for variant in variants:
+            cls = type(make_estimator(variant))
+            self.assertTrue(callable(getattr(cls, "turnHeightPriorOn")))
+            self.assertTrue(callable(getattr(cls, "turnHeightPriorOff")))
+            no_prior_foot_z = single_contact_update(make_estimator(variant), None)
+            high_prior_foot_z = single_contact_update(make_estimator(variant), 10.0)
+            low_prior_foot_z = single_contact_update(make_estimator(variant), -10.0)
+            self.assertNotAlmostEqual(no_prior_foot_z, high_prior_foot_z, places=6)
+            self.assertNotAlmostEqual(high_prior_foot_z, low_prior_foot_z, places=6)
+
     def test_height_prior_api_changes_filter_estimate_in_python(self):
         no_prior_estimator = make_simple_estimator()
         high_prior_estimator = make_simple_estimator()