added forgotten np.abs() for rotation_diff

salihmarangoz · Mar 18, 2023 · f026dae · f026dae
1 parent c20f2c7
commit f026dae
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/scripts/scan_matcher_v1.py b/scripts/scan_matcher_v1.py
@@ -101,16 +101,16 @@ def process_scan(self, pc, max_iter=50):
         T, R = self.match(self.pc_keyframe, pc, max_iter)
 
         # Calculate local pos difference
-        translation_diff = T.flatten()
-        rotation_diff = np.arctan2(R[1,0], R[0,0])
+        translation_diff = np.linalg.norm(T.flatten())
+        rotation_diff = np.abs(np.arctan2(R[1,0], R[0,0]))
 
         # Calculate global pos difference
         global_R = np.matmul(R, self.R_keyframe)
         global_T = self.T_keyframe + np.matmul(global_R, T)
         robot_xy = global_T.flatten()
         robot_theta = np.arctan2(global_R[1,0], global_R[0,0])
 
-        if np.linalg.norm(translation_diff) > 0.1 or rotation_diff > 0.2:
+        if translation_diff > 0.1 or rotation_diff > 0.2:
             self.pc_keyframe = pc
             self.R_keyframe = global_R
             self.T_keyframe = global_T