Store the offset as a pose, not a point

JeffLIrion · Nov 13, 2023 · d05c19b · d05c19b
1 parent b8aaea7
commit d05c19b
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 7 deletions.
diff --git a/graphslam/edge/edge_landmark.py b/graphslam/edge/edge_landmark.py
@@ -84,7 +84,7 @@ def calc_jacobians(self):
         """
         pose_oplus_offset = self.vertices[0].pose + self.offset
         # fmt: off
-        return [-np.dot(np.dot(np.dot(pose_oplus_offset.inverse.jacobian_self_oplus_point_wrt_self(self.vertices[1].pose), pose_oplus_offset.jacobian_inverse()), self.vertices[0].pose.jacobian_self_oplus_point_wrt_self(self.offset)), self.vertices[0].pose.jacobian_boxplus()),
+        return [-np.dot(np.dot(np.dot(pose_oplus_offset.inverse.jacobian_self_oplus_point_wrt_self(self.vertices[1].pose), pose_oplus_offset.jacobian_inverse()), self.vertices[0].pose.jacobian_self_oplus_other_wrt_self(self.offset)), self.vertices[0].pose.jacobian_boxplus()),
                 -np.dot(pose_oplus_offset.inverse.jacobian_self_oplus_point_wrt_point(self.vertices[1].pose), self.vertices[1].pose.jacobian_boxplus())]
         # fmt: on
 
@@ -128,7 +128,7 @@ def from_g2o(cls, line):
             numbers = line[len("EDGE_SE2_XY "):].split()  # fmt: skip
             arr = np.array([float(number) for number in numbers[2:]], dtype=np.float64)
             vertex_ids = [int(numbers[0]), int(numbers[1])]
-            estimate = PoseR2(arr[:2])
+            estimate = PoseSE2(arr[:2], 0.0)
             information = upper_triangular_matrix_to_full_matrix(arr[2:], 2)
             return EdgeLandmark(vertex_ids, information, estimate)
 
@@ -137,7 +137,7 @@ def from_g2o(cls, line):
             numbers = line[len("EDGE_SE3_TRACKXYZ "):].split()  # fmt: skip
             arr = np.array([float(number) for number in numbers[3:]], dtype=np.float64)
             vertex_ids = [int(numbers[0]), int(numbers[1])]
-            estimate = PoseR3(arr[:3])
+            estimate = PoseSE3(arr[:3], [0.0, 0.0, 0.0, 1.0])
             information = upper_triangular_matrix_to_full_matrix(arr[3:], 3)
             return EdgeLandmark(vertex_ids, information, estimate)
 

diff --git a/tests/test_edge_landmark.py b/tests/test_edge_landmark.py
@@ -28,7 +28,7 @@ def test_calc_jacobians3d(self):
         for a in range(10):
             p1 = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
             p2 = PoseR3(np.random.random_sample(3))
-            offset = PoseR3(np.random.random_sample(3))
+            offset = PoseSE3(np.random.random_sample(3), [0.0, 0.0, 0.0, 1.0])
 
             p1.normalize()
 
@@ -52,7 +52,7 @@ def test_calc_jacobians2d(self):
         for a in range(10):
             p1 = PoseSE2(np.random.random_sample(2), np.random.random_sample())
             p2 = PoseR2(np.random.random_sample(2))
-            offset = PoseR2(np.random.random_sample(2))
+            offset = PoseSE2(np.random.random_sample(2), 0.0)
 
             v1 = Vertex(1, p1)
             v2 = Vertex(2, p2)
@@ -75,7 +75,7 @@ def test_to_g2o_and_from_g2o_2d(self):
             p1 = PoseSE2(np.random.random_sample(2), np.random.random_sample())
             p2 = PoseR2(np.random.random_sample(2))
             offset = PoseR2(np.random.random_sample(2))
-            estimate = PoseR2(np.random.random_sample(2))
+            estimate = PoseSE2(np.random.random_sample(2), 0.0)
 
             v1 = Vertex(1, p1)
             v2 = Vertex(2, p2)
@@ -92,7 +92,7 @@ def test_to_g2o_and_from_g2o_3d(self):
             p1 = PoseSE3(np.random.random_sample(3), np.random.random_sample(4))
             p2 = PoseR3(np.random.random_sample(3))
             offset = PoseR3(np.random.random_sample(3))
-            estimate = PoseR3(np.random.random_sample(3))
+            estimate = PoseSE3(np.random.random_sample(3), [0.0, 0.0, 0.0, 1.0])
 
             p1.normalize()