Using utility rot_mat_2d (AtsushiSakai#683)

Author: AtsushiSakai

Localization/ensemble_kalman_filter/ensemble_kalman_filter.py

@@ -10,11 +10,15 @@
 
 """
 
-import math
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
 
+import math
 import matplotlib.pyplot as plt
 import numpy as np
-from scipy.spatial.transform import Rotation as Rot
+
+from utils.angle import rot_mat_2d
 
 #  Simulation parameter
 Q_sim = np.diag([0.2, np.deg2rad(1.0)]) ** 2
@@ -168,8 +172,7 @@ def plot_covariance_ellipse(xEst, PEst):  # pragma: no cover
     x = [a * math.cos(it) for it in t]
     y = [b * math.sin(it) for it in t]
     angle = math.atan2(eig_vec[1, big_ind], eig_vec[0, big_ind])
-    rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
-    fx = np.stack([x, y]).T @ rot
+    fx = np.stack([x, y]).T @ rot_mat_2d(angle)
 
     px = np.array(fx[:, 0] + xEst[0, 0]).flatten()
     py = np.array(fx[:, 1] + xEst[1, 0]).flatten()

Localization/extended_kalman_filter/extended_kalman_filter.py

@@ -6,11 +6,15 @@
 
 """
 
-import math
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
 
+import math
 import matplotlib.pyplot as plt
 import numpy as np
-from scipy.spatial.transform import Rotation as Rot
+
+from utils.angle import rot_mat_2d
 
 # Covariance for EKF simulation
 Q = np.diag([
@@ -149,8 +153,7 @@ def plot_covariance_ellipse(xEst, PEst):  # pragma: no cover
     x = [a * math.cos(it) for it in t]
     y = [b * math.sin(it) for it in t]
     angle = math.atan2(eigvec[1, bigind], eigvec[0, bigind])
-    rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
-    fx = rot @ (np.array([x, y]))
+    fx = rot_mat_2d(angle) @ (np.array([x, y]))
     px = np.array(fx[0, :] + xEst[0, 0]).flatten()
     py = np.array(fx[1, :] + xEst[1, 0]).flatten()
     plt.plot(px, py, "--r")

Localization/particle_filter/particle_filter.py

@@ -5,12 +5,16 @@
 author: Atsushi Sakai (@Atsushi_twi)
 
 """
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
 
 import math
 
 import matplotlib.pyplot as plt
 import numpy as np
-from scipy.spatial.transform import Rotation as Rot
+
+from utils.angle import rot_mat_2d
 
 # Estimation parameter of PF
 Q = np.diag([0.2]) ** 2  # range error
@@ -189,10 +193,9 @@ def plot_covariance_ellipse(x_est, p_est):  # pragma: no cover
     x = [a * math.cos(it) for it in t]
     y = [b * math.sin(it) for it in t]
     angle = math.atan2(eig_vec[1, big_ind], eig_vec[0, big_ind])
-    rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
-    fx = rot.dot(np.array([[x, y]]))
-    px = np.array(fx[0, :] + x_est[0, 0]).flatten()
-    py = np.array(fx[1, :] + x_est[1, 0]).flatten()
+    fx = rot_mat_2d(angle) @ np.array([[x, y]])
+    px = np.array(fx[:, 0] + x_est[0, 0]).flatten()
+    py = np.array(fx[:, 1] + x_est[1, 0]).flatten()
     plt.plot(px, py, "--r")
 
 

Localization/unscented_kalman_filter/unscented_kalman_filter.py

@@ -6,13 +6,18 @@
 
 """
 
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+
 import math
 
 import matplotlib.pyplot as plt
 import numpy as np
-from scipy.spatial.transform import Rotation as Rot
 import scipy.linalg
 
+from utils.angle import rot_mat_2d
+
 # Covariance for UKF simulation
 Q = np.diag([
     0.1,  # variance of location on x-axis
@@ -182,8 +187,7 @@ def plot_covariance_ellipse(xEst, PEst):  # pragma: no cover
     x = [a * math.cos(it) for it in t]
     y = [b * math.sin(it) for it in t]
     angle = math.atan2(eigvec[1, bigind], eigvec[0, bigind])
-    rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
-    fx = rot @ np.array([x, y])
+    fx = rot_mat_2d(angle) @ np.array([x, y])
     px = np.array(fx[0, :] + xEst[0, 0]).flatten()
     py = np.array(fx[1, :] + xEst[1, 0]).flatten()
     plt.plot(px, py, "--r")

Mapping/rectangle_fitting/rectangle_fitting.py

@@ -12,11 +12,17 @@
 
 """
 
+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 @@ def _rectangle_search(self, x, y):
         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 @@
 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 @@ def plot(self):
         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 @@ def ray_casting_filter(theta_l, range_l, angle_resolution):
         return rx, ry
 
 
-def main():
-    print("start!!")
-
-    print("done!!")
-
-
 if __name__ == '__main__':
     main()

PathPlanning/GridBasedSweepCPP/grid_based_sweep_coverage_path_planner.py

@@ -4,15 +4,18 @@
 author: Atsushi Sakai
 """
 
-import math
-import os
 import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+
+import math
 from enum import IntEnum
 
 import numpy as np
-from scipy.spatial.transform import Rotation as Rot
 import matplotlib.pyplot as plt
 
+from utils.angle import rot_mat_2d
+
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
                 "/../../Mapping/")
 
@@ -148,16 +151,14 @@ def convert_grid_coordinate(ox, oy, sweep_vec, sweep_start_position):
     tx = [ix - sweep_start_position[0] for ix in ox]
     ty = [iy - sweep_start_position[1] for iy in oy]
     th = math.atan2(sweep_vec[1], sweep_vec[0])
-    rot = Rot.from_euler('z', th).as_matrix()[0:2, 0:2]
-    converted_xy = np.stack([tx, ty]).T @ rot
+    converted_xy = np.stack([tx, ty]).T @ rot_mat_2d(th)
 
     return converted_xy[:, 0], converted_xy[:, 1]
 
 
 def convert_global_coordinate(x, y, sweep_vec, sweep_start_position):
     th = math.atan2(sweep_vec[1], sweep_vec[0])
-    rot = Rot.from_euler('z', -th).as_matrix()[0:2, 0:2]
-    converted_xy = np.stack([x, y]).T @ rot
+    converted_xy = np.stack([x, y]).T @ rot_mat_2d(-th)
     rx = [ix + sweep_start_position[0] for ix in converted_xy[:, 0]]
     ry = [iy + sweep_start_position[1] for iy in converted_xy[:, 1]]
     return rx, ry

PathPlanning/HybridAStar/car.py

@@ -6,11 +6,16 @@
 
 """
 
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+
 from math import cos, sin, tan, pi
 
 import matplotlib.pyplot as plt
 import numpy as np
-from scipy.spatial.transform import Rotation as Rot
+
+from utils.angle import rot_mat_2d
 
 WB = 3.0  # rear to front wheel
 W = 2.0  # width of car
@@ -45,7 +50,7 @@ def check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree):
 
 def rectangle_check(x, y, yaw, ox, oy):
     # transform obstacles to base link frame
-    rot = Rot.from_euler('z', yaw).as_matrix()[0:2, 0:2]
+    rot = rot_mat_2d(yaw)
     for iox, ioy in zip(ox, oy):
         tx = iox - x
         ty = ioy - y

PathPlanning/InformedRRTStar/informed_rrt_star.py

@@ -9,15 +9,19 @@
 https://arxiv.org/pdf/1404.2334.pdf
 
 """
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
 
 import copy
 import math
 import random
 
 import matplotlib.pyplot as plt
-from scipy.spatial.transform import Rotation as Rot
 import numpy as np
 
+from utils.angle import rot_mat_2d
+
 show_animation = True
 
 
@@ -308,8 +312,7 @@ def plot_ellipse(xCenter, cBest, cMin, e_theta):  # pragma: no cover
         t = np.arange(0, 2 * math.pi + 0.1, 0.1)
         x = [a * math.cos(it) for it in t]
         y = [b * math.sin(it) for it in t]
-        rot = Rot.from_euler('z', -angle).as_matrix()[0:2, 0:2]
-        fx = rot @ np.array([x, y])
+        fx = rot_mat_2d(-angle) @ np.array([x, y])
         px = np.array(fx[0, :] + cx).flatten()
         py = np.array(fx[1, :] + cy).flatten()
         plt.plot(cx, cy, "xc")

SLAM/GraphBasedSLAM/graph_based_slam.py

@@ -10,6 +10,9 @@
 (http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf)
 
 """
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
 
 import copy
 import itertools
@@ -63,7 +66,7 @@ def cal_observation_sigma():
     return sigma
 
 
-def calc_rotational_matrix(angle):
+def calc_3d_rotational_matrix(angle):
     return Rot.from_euler('z', angle).as_matrix()
 
 
@@ -82,8 +85,8 @@ def calc_edge(x1, y1, yaw1, x2, y2, yaw2, d1,
     edge.e[1, 0] = y2 - y1 - tmp3 + tmp4
     edge.e[2, 0] = 0
 
-    Rt1 = calc_rotational_matrix(tangle1)
-    Rt2 = calc_rotational_matrix(tangle2)
+    Rt1 = calc_3d_rotational_matrix(tangle1)
+    Rt2 = calc_3d_rotational_matrix(tangle2)
 
     sig1 = cal_observation_sigma()
     sig2 = cal_observation_sigma()