wip

prime-slam · Feb 7, 2024 · 9b15587 · 9b15587
1 parent 8d1ea4f
commit 9b15587
Showing 1 changed file with 1 addition and 3 deletions.
diff --git a/slam/utils/undistortioner.py b/slam/utils/undistortioner.py
@@ -35,8 +35,6 @@ def __call__(
         initial_point_cloud.transform(initial_pose)
         target_point_cloud.transform(target_pose)
 
-        initial_point_cloud.transform(np.linalg.inv(initial_pose))
-
         coefficients = np.roll(coefficients, -len(coefficients) // 2)
 
         for i in range(1, self._segments_number):
@@ -57,7 +55,7 @@ def _get_intermediate_transform(
         target_pose: Array4x4[float],
         coefficient: float,
     ):
-        dxi = mrob.geometry.SE3(initial_pose @ np.linalg.inv(target_pose)).Ln()
+        dxi = mrob.geometry.SE3(target_pose @ np.linalg.inv(initial_pose)).Ln()
         return mrob.geometry.SE3(coefficient * dxi).T() @ initial_pose
 
     def _cut_segment(