Using utility rot_mat_2d (#683)

Mapping/rectangle_fitting/rectangle_fitting.py

@@ -12,11 +12,17 @@ efficient-l-shape-fitting-for-vehicle-detection-using-laser-scanners/
 
 """
 
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+
+
 import matplotlib.pyplot as plt
 import numpy as np
 import itertools
 from enum import Enum
-from scipy.spatial.transform import Rotation as Rot
+
+from utils.angle import rot_mat_2d
 
 from Mapping.rectangle_fitting.simulator \
     import VehicleSimulator, LidarSimulator
@@ -104,8 +110,7 @@ class LShapeFitting:
         min_cost = (-float('inf'), None)
         for theta in np.arange(0.0, np.pi / 2.0 - d_theta, d_theta):
 
-            rot = Rot.from_euler('z', theta).as_matrix()[0:2, 0:2]
-            c = X @ rot
+            c = X @ rot_mat_2d(theta)
             c1 = c[:, 0]
             c2 = c[:, 1]
 

Mapping/rectangle_fitting/simulator.py

@@ -5,12 +5,16 @@ Simulator
 author: Atsushi Sakai
 
 """
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
 
 import numpy as np
 import matplotlib.pyplot as plt
 import math
 import random
-from scipy.spatial.transform import Rotation as Rot
+
+from utils.angle import rot_mat_2d
 
 
 class VehicleSimulator:
@@ -41,8 +45,7 @@ class VehicleSimulator:
         plt.plot(gx, gy, "--b")
 
     def calc_global_contour(self):
-        rot = Rot.from_euler('z', self.yaw).as_matrix()[0:2, 0:2]
-        gxy = np.stack([self.vc_x, self.vc_y]).T @ rot
+        gxy = np.stack([self.vc_x, self.vc_y]).T @ rot_mat_2d(self.yaw)
         gx = gxy[:, 0] + self.x
         gy = gxy[:, 1] + self.y
 
@@ -137,11 +140,5 @@ class LidarSimulator:
         return rx, ry
 
 
-def main():
-    print("start!!")
-
-    print("done!!")
-
-
 if __name__ == '__main__':
     main()