AutonomousVehicleControlBeginnersGuide/src/components/sensors/sensor_parameters.py at main · ShisatoYano/AutonomousVehicleControlBeginnersGuide · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
"""
sensor_parameters.py

Author: Shisato Yano
"""

import numpy as np
import sys
from pathlib import Path

sys.path.append(str(Path(__file__).absolute().parent) + "/../array")
sys.path.append(str(Path(__file__).absolute().parent) + "/../common")
from xy_array import XYArray
from matrix_lib import hom_mat_33


class SensorParameters:
    """
    Parameters class for sensor
    """

    def __init__(self, lon_m=0.0, lat_m=0.0, yaw_deg=0.0, min_m=0.5, max_m=40, reso_deg=2.0,
                 angle_std_scale=0.01, dist_std_rate=0.005, calibrator=None):
        """
        Constructor
        lon_m: longitudinal installation position on vehicle coordinate system[m]
        lat_m: lateral installation position on vehicle coordinate system[m]
        yaw_deg: yaw angle of installation position on vehicle coordinate system[deg]
        min_m: minimum sensing range[m]
        max_m: maximum sensing range[m]
        reso_deg: resolution of sensing angle[deg]
        angle_std_scale: scale of angle's standard deviation
        dist_std_rate: rate of distance's standard deviation
        calibrator: instance of calibrator module
        """

        self.INST_LON_M = lon_m
        self.INST_LAT_M = lat_m
        self.INST_YAW_RAD = np.deg2rad(yaw_deg)

        self.MIN_RANGE_M = min_m
        self.MAX_RANGE_M = max_m

        self.RESO_RAD = np.deg2rad(reso_deg)

        self.ANGLE_STD_SCALE = angle_std_scale
        self.DIST_STD_RATE = dist_std_rate

        self.inst_pos_array = XYArray(np.array([[self.INST_LON_M], [self.INST_LAT_M]]))
        self.global_x_m = None
        self.global_y_m = None

        # 3x3 homogeneous transformation matrix of sensor position and pose
        self.first_sensor_pos = True
        self.prev_sensor_tf = np.zeros((3, 3)) # at previous time
        self.curr_sensor_tf = np.zeros((3, 3)) # at current time

        # 3x3 homogeneous transformation matrix of vehicle position and pose
        self.first_vehicle_pos = True
        self.prev_vehicle_tf = np.zeros((3, 3))
        self.curr_vehicle_tf = np.zeros((3, 3))

        self.calibrator = calibrator

    def calculate_global_pos(self, state):
        """
        Function to calculate sensor's installation position on global coordinate system
        state: vehicle's state object
        """

        pose = state.x_y_yaw()
        transformed_array = self.inst_pos_array.homogeneous_transformation(pose[0, 0], pose[1, 0], pose[2, 0])

        self.global_x_m = transformed_array.get_x_data()
        self.global_y_m = transformed_array.get_y_data()

    def calculate_sensor_odometry(self, state):
        """
        Function to calculate sensor's odometry
        state: vehicle's state object
        """

        pose = state.x_y_yaw()

        # convert sensor's global pose to homegeneous transformation matrix
        if self.first_sensor_pos:
            self.prev_sensor_tf = hom_mat_33(self.global_x_m[0], self.global_y_m[0], pose[2, 0])
            self.curr_sensor_tf = self.prev_sensor_tf
            self.first_sensor_pos = False
        else:
            self.prev_sensor_tf = self.curr_sensor_tf
            self.curr_sensor_tf = hom_mat_33(self.global_x_m[0], self.global_y_m[0], pose[2, 0])

    def calibrate_extrinsic_params(self, vehicle_state):
        """
        Function to calibrate extrinsic parameters of sensor
        Those parameters are longitudinal, lateral position and yaw angle on vehicle coordinate system
        """

        # only when calibrator module exists, this process works
        if self.calibrator:
            # sensor odometry between 2 steps
            sensor_odom_tf = np.linalg.inv(self.prev_sensor_tf) @ self.curr_sensor_tf

            # vehicle odometry between 2 steps
            pose = vehicle_state.x_y_yaw()
            if self.first_vehicle_pos:
                self.prev_vehicle_tf = hom_mat_33(pose[0, 0], pose[1, 0], pose[2, 0])
                self.curr_vehicle_tf = self.prev_vehicle_tf
                self.first_vehicle_pos = False
            else:
                self.prev_vehicle_tf = self.curr_vehicle_tf
                self.curr_vehicle_tf = hom_mat_33(pose[0, 0], pose[1, 0], pose[2, 0])
            vehicle_odom_tf = np.linalg.inv(self.prev_vehicle_tf) @ self.curr_vehicle_tf

            self.calibrator.calibrate_extrinsic_params(sensor_odom_tf, vehicle_odom_tf)

    def get_global_x_m(self):
        """
        Getter of sensor's x installation position on global coordinate system
        """

        return self.global_x_m[0]

    def get_global_y_m(self):
        """
        Getter of sensor's y installation position on global coordinate system
        """

        return self.global_y_m[0]

    def draw_pos(self, axes, elems, state):
        """
        Function to draw sensor's installation position on vehicle
        axes: axes object of figure
        elems: list of plot object
        state: vehicle state object
        """

        self.calculate_global_pos(state)
        pos_plot, = axes.plot(self.global_x_m, self.global_y_m, marker='.', color='b')
        elems.append(pos_plot)

        # only when calibrator module exists, this process works
        if self.calibrator:
            self.calibrator.draw_calib_result(axes, elems, state,
                                              self.INST_LON_M,
                                              self.INST_LAT_M,
                                              self.INST_YAW_RAD)