From 5aeea011238c4df1d4d08cea6a14fb0851c59d48 Mon Sep 17 00:00:00 2001 From: Jeff Irion Date: Sun, 5 Nov 2023 23:12:56 -0800 Subject: [PATCH] WIP --- graphslam/pose/se3.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/graphslam/pose/se3.py b/graphslam/pose/se3.py index bd98f40..9073914 100644 --- 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.],