Remove unnecessary neg_pi_to_pi() calls (#48)

JeffLIrion · Nov 6, 2023 · 33d7f06 · 33d7f06
1 parent dfefa6d
commit 33d7f06
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/graphslam/pose/se2.py b/graphslam/pose/se2.py
@@ -179,7 +179,7 @@ def __add__(self, other):
             # fmt: off
             return PoseSE2([self[0] + other[0] * np.cos(self[2]) - other[1] * np.sin(self[2]),
                             self[1] + other[0] * np.sin(self[2]) + other[1] * np.cos(self[2])],
-                           neg_pi_to_pi(self[2] + other[2]))
+                           self[2] + other[2])
             # fmt: on
 
         if isinstance(other, PoseR2) or (isinstance(other, np.ndarray) and len(other) == 2):
@@ -209,7 +209,7 @@ def __sub__(self, other):
         # fmt: off
         return PoseSE2([(self[0] - other[0]) * np.cos(other[2]) + (self[1] - other[1]) * np.sin(other[2]),
                         (other[0] - self[0]) * np.sin(other[2]) + (self[1] - other[1]) * np.cos(other[2])],
-                       neg_pi_to_pi(self[2] - other[2]))
+                       self[2] - other[2])
         # fmt: on
 
     # ======================================================================= #