sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
openpilot is an open source driver assistance system. openpilot performs the functions of Automated Lane Centering and Adaptive Cruise Control for over 200 supported car makes and models.
11503 Commits
123 Branches
82 Tags
5.1 GiB
openpilot_comma/common/transformations/tests/test_orientation.py

69 lines
3.3 KiB
Raw Normal View History Unescape

common folder old-commit-hash: e8d888c45b5cb84bf38bdb96cae579a10f7ae281
5 years ago
#!/usr/bin/env python3
import numpy as np
import unittest
add openpilot prefix to imports (#29498) * add openpilot prefix to imports * more * more * fix docs * fix linter * bump submodules * fix patched tests * update dynamic imports * debug * Revert "debug" This reverts commit db5e13b9911cc74438bee123bc3430da6c31b24b. * fix pm test old-commit-hash: a9626f95b69af19306143fc4def02fb5769405d2
2 years ago
from openpilot.common.transformations.orientation import euler2quat, quat2euler, euler2rot, rot2euler, \
common folder old-commit-hash: e8d888c45b5cb84bf38bdb96cae579a10f7ae281
5 years ago
rot2quat, quat2rot, \
ned_euler_from_ecef
eulers = np.array([[ 1.46520501, 2.78688383, 2.92780854],
[ 4.86909526, 3.60618161, 4.30648981],
[ 3.72175965, 2.68763705, 5.43895988],
[ 5.92306687, 5.69573614, 0.81100357],
[ 0.67838374, 5.02402037, 2.47106426]])
quats = np.array([[ 0.66855182, -0.71500939, 0.19539353, 0.06017818],
[ 0.43163717, 0.70013301, 0.28209145, 0.49389021],
[ 0.44121991, -0.08252646, 0.34257534, 0.82532207],
[ 0.88578382, -0.04515356, -0.32936046, 0.32383617],
[ 0.06578165, 0.61282835, 0.07126891, 0.78424163]])
ecef_positions = np.array([[-2711076.55270557, -4259167.14692758, 3884579.87669935],
[ 2068042.69652729, -5273435.40316622, 2927004.89190746],
[-2160412.60461669, -4932588.89873832, 3406542.29652851],
[-1458247.92550567, 5983060.87496612, 1654984.6099885 ],
[ 4167239.10867871, 4064301.90363223, 2602234.6065749 ]])
ned_eulers = np.array([[ 0.46806039, -0.4881889 , 1.65697808],
[-2.14525969, -0.36533066, 0.73813479],
[-1.39523364, -0.58540761, -1.77376356],
[-1.84220435, 0.61828016, -1.03310421],
[ 2.50450101, 0.36304151, 0.33136365]])
class TestOrientation(unittest.TestCase):
def test_quat_euler(self):
for i, eul in enumerate(eulers):
np.testing.assert_allclose(quats[i], euler2quat(eul), rtol=1e-7)
np.testing.assert_allclose(quats[i], euler2quat(quat2euler(quats[i])), rtol=1e-6)
for i, eul in enumerate(eulers):
np.testing.assert_allclose(quats[i], euler2quat(list(eul)), rtol=1e-7)
np.testing.assert_allclose(quats[i], euler2quat(quat2euler(list(quats[i]))), rtol=1e-6)
np.testing.assert_allclose(quats, euler2quat(eulers), rtol=1e-7)
np.testing.assert_allclose(quats, euler2quat(quat2euler(quats)), rtol=1e-6)
def test_rot_euler(self):
for eul in eulers:
np.testing.assert_allclose(euler2quat(eul), euler2quat(rot2euler(euler2rot(eul))), rtol=1e-7)
for eul in eulers:
np.testing.assert_allclose(euler2quat(eul), euler2quat(rot2euler(euler2rot(list(eul)))), rtol=1e-7)
np.testing.assert_allclose(euler2quat(eulers), euler2quat(rot2euler(euler2rot(eulers))), rtol=1e-7)
def test_rot_quat(self):
for quat in quats:
np.testing.assert_allclose(quat, rot2quat(quat2rot(quat)), rtol=1e-7)
for quat in quats:
np.testing.assert_allclose(quat, rot2quat(quat2rot(list(quat))), rtol=1e-7)
np.testing.assert_allclose(quats, rot2quat(quat2rot(quats)), rtol=1e-7)
def test_euler_ned(self):
for i in range(len(eulers)):
np.testing.assert_allclose(ned_eulers[i], ned_euler_from_ecef(ecef_positions[i], eulers[i]), rtol=1e-7)
#np.testing.assert_allclose(eulers[i], ecef_euler_from_ned(ecef_positions[i], ned_eulers[i]), rtol=1e-7)
Write orientation & transform in C++ (#1637) * locationd at 20hz * update ref * bump cereal * dont modify global state * add scons files * ecef2geodetic and geodetic2ecef * Finish local coords class * Add header file * Add orientation.cc * cleanup * Add functions to header file * Add cython wrapper * y u no work? * This passes the tests * test rot2quat and quat2rot * Teste euler2rot and rot2euler * rot_matrix * test ecef_euler_from_ned and ned_euler_from_ecef * add benchmark * Add test * Consistent newlines * no more radians supported in geodetic * test localcoord single * test localcoord single * all tests pass * Unused import * Add alternate namings * Add source for formulas * no explicit tests needed * remove benchmark * Add release files * Typo * Remove print statement * no access to raw transform matrix * temporarily add tolerance * handcode quat2euler * update ref old-commit-hash: c18e7da3c25c59a19efda8ec3a1b716c2571cc2f
5 years ago
# np.testing.assert_allclose(ned_eulers, ned_euler_from_ecef(ecef_positions, eulers), rtol=1e-7)
common folder old-commit-hash: e8d888c45b5cb84bf38bdb96cae579a10f7ae281
5 years ago
if __name__ == "__main__":
unittest.main()