File size: 2,159 Bytes
393d3de |
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 |
# coding=utf-8
# Copyright 2022 The Reach ML Authors.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""A simple 6DOF pose container.
"""
import dataclasses
import numpy as np
from scipy.spatial import transform
class NoCopyAsDict(object):
"""Base class for dataclasses. Avoids a copy in the asdict() call."""
def asdict(self):
"""Replacement for dataclasses.asdict.
TF Dataset does not handle dataclasses.asdict, which uses copy.deepcopy when
setting values in the output dict. This causes issues with tf.Dataset.
Instead, shallow copy contents.
Returns:
dict containing contents of dataclass.
"""
return {k.name: getattr(self, k.name) for k in dataclasses.fields(self)}
@dataclasses.dataclass
class Pose3d(NoCopyAsDict):
"""Simple container for translation and rotation."""
rotation: transform.Rotation
translation: np.ndarray
@property
def vec7(self):
return np.concatenate([self.translation, self.rotation.as_quat()])
def serialize(self):
return {
"rotation": self.rotation.as_quat().tolist(),
"translation": self.translation.tolist(),
}
@staticmethod
def deserialize(data):
return Pose3d(
rotation=transform.Rotation.from_quat(data["rotation"]),
translation=np.array(data["translation"]),
)
def __eq__(self, other):
return np.array_equal(
self.rotation.as_quat(), other.rotation.as_quat()
) and np.array_equal(self.translation, other.translation)
def __ne__(self, other):
return not self.__eq__(other)
|