WIP

JeffLIrion · Nov 6, 2023 · 5aeea01 · 5aeea01
1 parent 9852203
commit 5aeea01
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/graphslam/pose/se3.py b/graphslam/pose/se3.py
@@ -504,7 +504,7 @@ def jacobian_inverse(self):
         # fmt: off
         IDENTITY = self.identity()
         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] * (IDENTITY[0] - self[0]) - 2. * self[3] * (IDENTITY[1] - self[1]) + self[6] * (IDENTITY[2] - self[2])), 2. * (self[3] * (IDENTITY[0] - self[0]) + self[5] * (IDENTITY[2] - self[2])), 2. * (-self[6] * (IDENTITY[0] - self[0]) - 2. * self[5] * (IDENTITY[1] - self[1]) + self[4] * (IDENTITY[2] - self[2])), 2. * (-self[5] * (IDENTITY[0] - self[0]) + self[3] * (IDENTITY[2] - 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] * (IDENTITY[0] - self[0]) + self[5] * (IDENTITY[2] - self[2])), 2. * (-self[6] * (IDENTITY[0] - self[0]) - 2. * self[5] * (IDENTITY[1] - self[1]) + self[4] * (IDENTITY[2] - self[2])), 2. * (-self[5] * (IDENTITY[0] - self[0]) + self[3] * (IDENTITY[2] - 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] * (IDENTITY[0] - self[0]) - self[6] * (IDENTITY[1] - self[1]) - 2. * self[3] * (IDENTITY[2] - self[2])), 2. * (self[6] * (IDENTITY[0] - self[0]) + self[5] * (IDENTITY[1] - self[1]) - 2. * self[4] * (IDENTITY[2] - self[2])), 2. * (self[3] * (IDENTITY[0] - self[0]) + self[4] * (IDENTITY[1] - self[1])), 2. * (self[4] * (IDENTITY[0] - self[0]) - self[3] * (IDENTITY[1] - self[1]))],
                          [0., 0., 0., -1., 0., 0., 0.],
                          [0., 0., 0., 0., -1., 0., 0.],