Use odot operator for SE2 PointRelativeMeasurement

decargroup · Jan 15, 2025 · 42101bf · 42101bf
1 parent 249ad9f
commit 42101bf
Showing 1 changed file with 14 additions and 23 deletions.
diff --git a/navlie/lib/models.py b/navlie/lib/models.py
@@ -472,29 +472,20 @@ def jacobian(self, x: MatrixLieGroupState) -> np.ndarray:
         r_pw_a = self._landmark_position.reshape((-1, 1))
         y = C_ab.T @ (r_pw_a - r_zw_a).reshape(-1, 1)
 
-        if isinstance(x, SE2State):
-            P = np.array([[0, -1], [1, 0]])
-            if x.direction == "right":
-                return x.jacobian_from_blocks(
-                    attitude=-P @ C_ab.T @ (r_pw_a - r_zw_a).reshape(-1, 1),
-                    position=-np.eye(2),
-                )
-            if x.direction == "left":
-                return x.jacobian_from_blocks(
-                    attitude=-C_ab.T @ P @ (r_pw_a).reshape(-1, 1),
-                    position=-C_ab.T,
-                )
-
-        if isinstance(x, SE3State) or isinstance(x, SE23State):  # SE2
-            if x.direction == "right":
-                return x.jacobian_from_blocks(
-                    attitude=-SO3.odot(y), position=-np.identity(r_zw_a.shape[0])
-                )
-
-            elif x.direction == "left":
-                return x.jacobian_from_blocks(
-                    attitude=-C_ab.T @ SO3.odot(r_pw_a), position=-C_ab.T
-                )
+        if C_ab.shape[0] == 2:
+            group = SO2
+        if C_ab.shape[0] == 3:
+            group = SO3
+
+        if x.direction == "right":
+            return x.jacobian_from_blocks(
+                attitude=-group.odot(y), position=-np.identity(r_zw_a.shape[0])
+            )
+
+        elif x.direction == "left":
+            return x.jacobian_from_blocks(
+                attitude=-C_ab.T @ group.odot(r_pw_a), position=-C_ab.T
+            )
 
     def covariance(self, x: MatrixLieGroupState) -> np.ndarray:
         return self._R