WIP

JeffLIrion · Nov 6, 2023 · 4231595 · 4231595
1 parent 0b32b91
commit 4231595
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/graphslam/pose/se3.py b/graphslam/pose/se3.py
@@ -503,8 +503,8 @@ def jacobian_inverse(self):
         """
         # fmt: off
         return np.array([[-1. + 2. * (self[4]**2 + self[5]**2), -2. * (self[3] * self[4] + self[5] * self[6]), -2. * (self[3] * self[5] - self[4] * self[6]), -2. * (self[4] * self[1] + self[5] * self[2]), 2. * (2. * self[4] * self[0] - self[3] * self[1] + self[6] * self[2]), 2. * (2. * self[5] * self[0] - self[6] * self[1] - self[3] * self[2]), 2. * (-self[5] * self[1] + self[4] * self[2])],
-                         [-2. * (self[3] * self[4] - self[5] * self[6]), -1. + 2. * (self[3]**2 + self[5]**2), -2. * (self[4] * self[5] + self[3] * self[6]), 2. * (-self[4] * self[0] + 2. * self[3] * self[1] - self[6] * self[2]), 2. * (-self[3] * self[0] - self[5] * self[2]), 2. * (self[6] * self[0] + 2. * self[5] * self[1] - self[4] * self[2]), 2. * (self[5] * self[0] - self[3] * self[2])],
-                         [-2. * (self[3] * self[5] + self[4] * self[6]), -2. * (self[4] * self[5] - self[3] * self[6]), -1. + 2. * (self[3]**2 + self[4]**2), 2. * (-self[5] * self[0] + self[6] * self[1] + 2. * self[3] * self[2]), 2. * (-self[6] * self[0] - self[5] * self[1] + 2. * self[4] * self[2]), 2. * (-self[3] * self[0] - self[4] * self[1]), 2. * (-self[4] * self[0] + self[3] * self[1])],
+                         [-2. * (self[3] * self[4] - self[5] * self[6]), -1. + 2. * (self[3]**2 + self[5]**2), -2. * (self[4] * self[5] + self[3] * self[6]), 2. * (-self[4] * self[0] + 2. * self[3] * self[1] - self[6] * self[2]), -2. * (self[3] * self[0] + self[5] * self[2]), 2. * (self[6] * self[0] + 2. * self[5] * self[1] - self[4] * self[2]), 2. * (self[5] * self[0] - self[3] * self[2])],
+                         [-2. * (self[3] * self[5] + self[4] * self[6]), -2. * (self[4] * self[5] - self[3] * self[6]), -1. + 2. * (self[3]**2 + self[4]**2), 2. * (-self[5] * self[0] + self[6] * self[1] + 2. * self[3] * self[2]), -2. * (self[6] * self[0] + self[5] * self[1] - 2. * self[4] * self[2]), -2. * (self[3] * self[0] + self[4] * self[1]), 2. * (-self[4] * self[0] + self[3] * self[1])],
                          [0., 0., 0., -1., 0., 0., 0.],
                          [0., 0., 0., 0., -1., 0., 0.],
                          [0., 0., 0., 0., 0., -1., 0.],