ParticleFilter/data_utils.py at main · TeamMeanMachine/ParticleFilter · 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
150
151
152
153
154
155
156
157
158
159
160
161
from dataclasses import dataclass
import numpy as np
import transformations as tf
from typing import List
from wpimath.geometry import Transform3d, Translation3d, Rotation3d, Pose3d, Pose2d
from wpiutil import wpistruct


@wpistruct.make_wpistruct
@dataclass
class CameraInfo:
    transform: Transform3d
    hasCameraMatrix: wpistruct.int32
    M00: wpistruct.double
    M01: wpistruct.double
    M02: wpistruct.double
    M10: wpistruct.double
    M11: wpistruct.double
    M12: wpistruct.double
    M20: wpistruct.double
    M21: wpistruct.double
    M22: wpistruct.double
    hasDistCoeffs: wpistruct.int32
    d0: wpistruct.double
    d1: wpistruct.double
    d2: wpistruct.double
    d3: wpistruct.double
    d4: wpistruct.double
    d5: wpistruct.double
    d6: wpistruct.double
    d7: wpistruct.double

    def __getstate__(self):
        state = self.__dict__.copy()
        # Manually serialize Transform3d
        state["__X"] = state["transform"].translation().X()
        state["__Y"] = state["transform"].translation().Y()
        state["__Z"] = state["transform"].translation().Z()
        state["__rotX"] = state["transform"].rotation().X()
        state["__rotY"] = state["transform"].rotation().Y()
        state["__rotZ"] = state["transform"].rotation().Z()
        del state["transform"]
        return state

    def __setstate__(self, state):
        self.__dict__.update(state)
        # Manually deserialize Transform3d
        self.transform = Transform3d(
            Translation3d(state["__X"], state["__Y"], state["__Z"]),
            Rotation3d(state["__rotX"], state["__rotY"], state["__rotZ"]),
        )

    def getTransformMatrix(self):
        return tf.compose_matrix(
            translate=[
                self.transform.translation().X(),
                self.transform.translation().Y(),
                self.transform.translation().Z(),
            ],
            angles=[
                self.transform.rotation().X(),
                self.transform.rotation().Y(),
                self.transform.rotation().Z(),
            ],
        )

    def hasCalibration(self):
        return self.hasCameraMatrix and self.hasDistCoeffs

    def getCameraMatrix(self):
        if not self.hasCameraMatrix:
            return None
        return np.array(
            [
                [self.M00, self.M01, self.M02],
                [self.M10, self.M11, self.M12],
                [self.M20, self.M21, self.M22],
            ]
        )

    def getDistCoeffs(self):
        if not self.hasDistCoeffs:
            return None
        return np.array(
            [self.d0, self.d1, self.d2, self.d3, self.d4, self.d5, self.d6, self.d7]
        )


@wpistruct.make_wpistruct
@dataclass
class Fiducial:
    type: wpistruct.int32
    id: wpistruct.int32
    pose: Pose3d
    size: wpistruct.double

    def __getstate__(self):
        state = self.__dict__.copy()
        # Manually serialize Pose3d
        state["__X"] = state["pose"].translation().X()
        state["__Y"] = state["pose"].translation().Y()
        state["__Z"] = state["pose"].translation().Z()
        state["__rotX"] = state["pose"].rotation().X()
        state["__rotY"] = state["pose"].rotation().Y()
        state["__rotZ"] = state["pose"].rotation().Z()
        del state["pose"]
        return state

    def __setstate__(self, state):
        self.__dict__.update(state)
        # Manually deserialize Pose3d
        self.pose = Pose3d(
            Translation3d(state["__X"], state["__Y"], state["__Z"]),
            Rotation3d(state["__rotX"], state["__rotY"], state["__rotZ"]),
        )

    def getPoseMatrix(self):
        return tf.compose_matrix(
            translate=[
                self.pose.translation().X(),
                self.pose.translation().Y(),
                self.pose.translation().Z(),
            ],
            angles=[
                self.pose.rotation().X(),
                self.pose.rotation().Y(),
                self.pose.rotation().Z(),
            ],
        )


@dataclass
class Vision:
    cameraID: int
    targetID: int
    targetCornerID: int
    pixelX: float
    pixelY: float
    pixelSigma: float


@dataclass
class Odometry:
    x: float
    y: float
    theta: float


@dataclass
class Measurement:
    id: int
    odom: Odometry
    vision: List[Vision]


@wpistruct.make_wpistruct
@dataclass
class PoseEstimate:
    id: wpistruct.int32
    pose: Pose2d
    hasVision: wpistruct.int32