From 1f9efd93115db3dba28459bec790cfb35ee58e2f Mon Sep 17 00:00:00 2001 From: Matt Purnell <65473602+mpurnell1@users.noreply.github.com> Date: Sat, 17 Jan 2026 00:31:26 -0600 Subject: [PATCH] transformations: move Cython to pure Python (#36830) * Remove cython for transformations * Add new test * Switch back to program to fix mac builds * Convert to Python instead * Fix failing builds * lint * Implement conversion in pure python/numpy * Add more tests * Fix bugs in tests --- common/SConscript | 7 +- common/transformations/SConscript | 5 - common/transformations/coordinates.cc | 100 ----- common/transformations/coordinates.hpp | 43 --- common/transformations/orientation.cc | 144 -------- common/transformations/orientation.hpp | 17 - .../transformations/tests/test_coordinates.py | 33 ++ .../transformations/tests/test_orientation.py | 30 ++ common/transformations/transformations.pxd | 72 ---- common/transformations/transformations.py | 342 ++++++++++++++++++ common/transformations/transformations.pyx | 173 --------- selfdrive/modeld/SConscript | 2 +- 12 files changed, 407 insertions(+), 561 deletions(-) delete mode 100644 common/transformations/SConscript delete mode 100644 common/transformations/coordinates.cc delete mode 100644 common/transformations/coordinates.hpp delete mode 100644 common/transformations/orientation.cc delete mode 100644 common/transformations/orientation.hpp delete mode 100644 common/transformations/transformations.pxd create mode 100644 common/transformations/transformations.py delete mode 100644 common/transformations/transformations.pyx diff --git a/common/SConscript b/common/SConscript index c771ee78b7..1c68cf05c7 100644 --- a/common/SConscript +++ b/common/SConscript @@ -19,11 +19,6 @@ if GetOption('extras'): # Cython bindings params_python = envCython.Program('params_pyx.so', 'params_pyx.pyx', LIBS=envCython['LIBS'] + [_common, 'zmq', 'json11']) -SConscript([ - 'transformations/SConscript', -]) - -Import('transformations_python') -common_python = [params_python, transformations_python] +common_python = [params_python] Export('common_python') diff --git a/common/transformations/SConscript b/common/transformations/SConscript deleted file mode 100644 index 4ac73a165e..0000000000 --- a/common/transformations/SConscript +++ /dev/null @@ -1,5 +0,0 @@ -Import('env', 'envCython') - -transformations = env.Library('transformations', ['orientation.cc', 'coordinates.cc']) -transformations_python = envCython.Program('transformations.so', 'transformations.pyx') -Export('transformations', 'transformations_python') diff --git a/common/transformations/coordinates.cc b/common/transformations/coordinates.cc deleted file mode 100644 index f3f10e547f..0000000000 --- a/common/transformations/coordinates.cc +++ /dev/null @@ -1,100 +0,0 @@ -#define _USE_MATH_DEFINES - -#include "common/transformations/coordinates.hpp" - -#include -#include -#include - -double a = 6378137; // lgtm [cpp/short-global-name] -double b = 6356752.3142; // lgtm [cpp/short-global-name] -double esq = 6.69437999014 * 0.001; // lgtm [cpp/short-global-name] -double e1sq = 6.73949674228 * 0.001; - - -static Geodetic to_degrees(Geodetic geodetic){ - geodetic.lat = RAD2DEG(geodetic.lat); - geodetic.lon = RAD2DEG(geodetic.lon); - return geodetic; -} - -static Geodetic to_radians(Geodetic geodetic){ - geodetic.lat = DEG2RAD(geodetic.lat); - geodetic.lon = DEG2RAD(geodetic.lon); - return geodetic; -} - - -ECEF geodetic2ecef(const Geodetic &geodetic) { - auto g = to_radians(geodetic); - double xi = sqrt(1.0 - esq * pow(sin(g.lat), 2)); - double x = (a / xi + g.alt) * cos(g.lat) * cos(g.lon); - double y = (a / xi + g.alt) * cos(g.lat) * sin(g.lon); - double z = (a / xi * (1.0 - esq) + g.alt) * sin(g.lat); - return {x, y, z}; -} - -Geodetic ecef2geodetic(const ECEF &e) { - // Convert from ECEF to geodetic using Ferrari's methods - // https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#Ferrari.27s_solution - double x = e.x; - double y = e.y; - double z = e.z; - - double r = sqrt(x * x + y * y); - double Esq = a * a - b * b; - double F = 54 * b * b * z * z; - double G = r * r + (1 - esq) * z * z - esq * Esq; - double C = (esq * esq * F * r * r) / (pow(G, 3)); - double S = cbrt(1 + C + sqrt(C * C + 2 * C)); - double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G); - double Q = sqrt(1 + 2 * esq * esq * P); - double r_0 = -(P * esq * r) / (1 + Q) + sqrt(0.5 * a * a*(1 + 1.0 / Q) - P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r); - double U = sqrt(pow((r - esq * r_0), 2) + z * z); - double V = sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z); - double Z_0 = b * b * z / (a * V); - double h = U * (1 - b * b / (a * V)); - - double lat = atan((z + e1sq * Z_0) / r); - double lon = atan2(y, x); - - return to_degrees({lat, lon, h}); -} - -LocalCoord::LocalCoord(const Geodetic &geodetic, const ECEF &e) { - init_ecef << e.x, e.y, e.z; - - auto g = to_radians(geodetic); - - ned2ecef_matrix << - -sin(g.lat)*cos(g.lon), -sin(g.lon), -cos(g.lat)*cos(g.lon), - -sin(g.lat)*sin(g.lon), cos(g.lon), -cos(g.lat)*sin(g.lon), - cos(g.lat), 0, -sin(g.lat); - ecef2ned_matrix = ned2ecef_matrix.transpose(); -} - -NED LocalCoord::ecef2ned(const ECEF &e) { - Eigen::Vector3d ecef; - ecef << e.x, e.y, e.z; - - Eigen::Vector3d ned = (ecef2ned_matrix * (ecef - init_ecef)); - return {ned[0], ned[1], ned[2]}; -} - -ECEF LocalCoord::ned2ecef(const NED &n) { - Eigen::Vector3d ned; - ned << n.n, n.e, n.d; - - Eigen::Vector3d ecef = (ned2ecef_matrix * ned) + init_ecef; - return {ecef[0], ecef[1], ecef[2]}; -} - -NED LocalCoord::geodetic2ned(const Geodetic &g) { - ECEF e = ::geodetic2ecef(g); - return ecef2ned(e); -} - -Geodetic LocalCoord::ned2geodetic(const NED &n) { - ECEF e = ned2ecef(n); - return ::ecef2geodetic(e); -} diff --git a/common/transformations/coordinates.hpp b/common/transformations/coordinates.hpp deleted file mode 100644 index dc8ff7a4b6..0000000000 --- a/common/transformations/coordinates.hpp +++ /dev/null @@ -1,43 +0,0 @@ -#pragma once - -#include - -#define DEG2RAD(x) ((x) * M_PI / 180.0) -#define RAD2DEG(x) ((x) * 180.0 / M_PI) - -struct ECEF { - double x, y, z; - Eigen::Vector3d to_vector() const { - return Eigen::Vector3d(x, y, z); - } -}; - -struct NED { - double n, e, d; - Eigen::Vector3d to_vector() const { - return Eigen::Vector3d(n, e, d); - } -}; - -struct Geodetic { - double lat, lon, alt; - bool radians=false; -}; - -ECEF geodetic2ecef(const Geodetic &g); -Geodetic ecef2geodetic(const ECEF &e); - -class LocalCoord { -public: - Eigen::Matrix3d ned2ecef_matrix; - Eigen::Matrix3d ecef2ned_matrix; - Eigen::Vector3d init_ecef; - LocalCoord(const Geodetic &g, const ECEF &e); - LocalCoord(const Geodetic &g) : LocalCoord(g, ::geodetic2ecef(g)) {} - LocalCoord(const ECEF &e) : LocalCoord(::ecef2geodetic(e), e) {} - - NED ecef2ned(const ECEF &e); - ECEF ned2ecef(const NED &n); - NED geodetic2ned(const Geodetic &g); - Geodetic ned2geodetic(const NED &n); -}; diff --git a/common/transformations/orientation.cc b/common/transformations/orientation.cc deleted file mode 100644 index fb5e47a86a..0000000000 --- a/common/transformations/orientation.cc +++ /dev/null @@ -1,144 +0,0 @@ -#define _USE_MATH_DEFINES - -#include -#include -#include - -#include "common/transformations/orientation.hpp" -#include "common/transformations/coordinates.hpp" - -Eigen::Quaterniond ensure_unique(const Eigen::Quaterniond &quat) { - if (quat.w() > 0){ - return quat; - } else { - return Eigen::Quaterniond(-quat.w(), -quat.x(), -quat.y(), -quat.z()); - } -} - -Eigen::Quaterniond euler2quat(const Eigen::Vector3d &euler) { - Eigen::Quaterniond q; - - q = Eigen::AngleAxisd(euler(2), Eigen::Vector3d::UnitZ()) - * Eigen::AngleAxisd(euler(1), Eigen::Vector3d::UnitY()) - * Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitX()); - return ensure_unique(q); -} - - -Eigen::Vector3d quat2euler(const Eigen::Quaterniond &quat) { - // TODO: switch to eigen implementation if the range of the Euler angles doesn't matter anymore - // Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 0); - // return {euler(2), euler(1), euler(0)}; - double gamma = atan2(2 * (quat.w() * quat.x() + quat.y() * quat.z()), 1 - 2 * (quat.x()*quat.x() + quat.y()*quat.y())); - double asin_arg_clipped = std::clamp(2 * (quat.w() * quat.y() - quat.z() * quat.x()), -1.0, 1.0); - double theta = asin(asin_arg_clipped); - double psi = atan2(2 * (quat.w() * quat.z() + quat.x() * quat.y()), 1 - 2 * (quat.y()*quat.y() + quat.z()*quat.z())); - return {gamma, theta, psi}; -} - -Eigen::Matrix3d quat2rot(const Eigen::Quaterniond &quat) { - return quat.toRotationMatrix(); -} - -Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot) { - return ensure_unique(Eigen::Quaterniond(rot)); -} - -Eigen::Matrix3d euler2rot(const Eigen::Vector3d &euler) { - return quat2rot(euler2quat(euler)); -} - -Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot) { - return quat2euler(rot2quat(rot)); -} - -Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw) { - return euler2rot({roll, pitch, yaw}); -} - -Eigen::Matrix3d rot(const Eigen::Vector3d &axis, double angle) { - Eigen::Quaterniond q; - q = Eigen::AngleAxisd(angle, axis); - return q.toRotationMatrix(); -} - - -Eigen::Vector3d ecef_euler_from_ned(const ECEF &ecef_init, const Eigen::Vector3d &ned_pose) { - /* - Using Rotations to Build Aerospace Coordinate Systems - Don Koks - https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf - */ - LocalCoord converter = LocalCoord(ecef_init); - Eigen::Vector3d zero = ecef_init.to_vector(); - - Eigen::Vector3d x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero; - Eigen::Vector3d y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero; - Eigen::Vector3d z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero; - - Eigen::Vector3d x1 = rot(z0, ned_pose(2)) * x0; - Eigen::Vector3d y1 = rot(z0, ned_pose(2)) * y0; - Eigen::Vector3d z1 = rot(z0, ned_pose(2)) * z0; - - Eigen::Vector3d x2 = rot(y1, ned_pose(1)) * x1; - Eigen::Vector3d y2 = rot(y1, ned_pose(1)) * y1; - Eigen::Vector3d z2 = rot(y1, ned_pose(1)) * z1; - - Eigen::Vector3d x3 = rot(x2, ned_pose(0)) * x2; - Eigen::Vector3d y3 = rot(x2, ned_pose(0)) * y2; - - - x0 = Eigen::Vector3d(1, 0, 0); - y0 = Eigen::Vector3d(0, 1, 0); - z0 = Eigen::Vector3d(0, 0, 1); - - double psi = atan2(x3.dot(y0), x3.dot(x0)); - double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2))); - - y2 = rot(z0, psi) * y0; - z2 = rot(y2, theta) * z0; - - double phi = atan2(y3.dot(z2), y3.dot(y2)); - - return {phi, theta, psi}; -} - -Eigen::Vector3d ned_euler_from_ecef(const ECEF &ecef_init, const Eigen::Vector3d &ecef_pose) { - /* - Using Rotations to Build Aerospace Coordinate Systems - Don Koks - https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf - */ - LocalCoord converter = LocalCoord(ecef_init); - - Eigen::Vector3d x0 = Eigen::Vector3d(1, 0, 0); - Eigen::Vector3d y0 = Eigen::Vector3d(0, 1, 0); - Eigen::Vector3d z0 = Eigen::Vector3d(0, 0, 1); - - Eigen::Vector3d x1 = rot(z0, ecef_pose(2)) * x0; - Eigen::Vector3d y1 = rot(z0, ecef_pose(2)) * y0; - Eigen::Vector3d z1 = rot(z0, ecef_pose(2)) * z0; - - Eigen::Vector3d x2 = rot(y1, ecef_pose(1)) * x1; - Eigen::Vector3d y2 = rot(y1, ecef_pose(1)) * y1; - Eigen::Vector3d z2 = rot(y1, ecef_pose(1)) * z1; - - Eigen::Vector3d x3 = rot(x2, ecef_pose(0)) * x2; - Eigen::Vector3d y3 = rot(x2, ecef_pose(0)) * y2; - - Eigen::Vector3d zero = ecef_init.to_vector(); - x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero; - y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero; - z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero; - - double psi = atan2(x3.dot(y0), x3.dot(x0)); - double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2))); - - y2 = rot(z0, psi) * y0; - z2 = rot(y2, theta) * z0; - - double phi = atan2(y3.dot(z2), y3.dot(y2)); - - return {phi, theta, psi}; -} - diff --git a/common/transformations/orientation.hpp b/common/transformations/orientation.hpp deleted file mode 100644 index 0874a0a814..0000000000 --- a/common/transformations/orientation.hpp +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once -#include -#include "common/transformations/coordinates.hpp" - - -Eigen::Quaterniond ensure_unique(const Eigen::Quaterniond &quat); - -Eigen::Quaterniond euler2quat(const Eigen::Vector3d &euler); -Eigen::Vector3d quat2euler(const Eigen::Quaterniond &quat); -Eigen::Matrix3d quat2rot(const Eigen::Quaterniond &quat); -Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot); -Eigen::Matrix3d euler2rot(const Eigen::Vector3d &euler); -Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot); -Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw); -Eigen::Matrix3d rot(const Eigen::Vector3d &axis, double angle); -Eigen::Vector3d ecef_euler_from_ned(const ECEF &ecef_init, const Eigen::Vector3d &ned_pose); -Eigen::Vector3d ned_euler_from_ecef(const ECEF &ecef_init, const Eigen::Vector3d &ecef_pose); diff --git a/common/transformations/tests/test_coordinates.py b/common/transformations/tests/test_coordinates.py index 11a6bf70ee..0b5d1c36df 100644 --- a/common/transformations/tests/test_coordinates.py +++ b/common/transformations/tests/test_coordinates.py @@ -102,3 +102,36 @@ class TestNED: np.testing.assert_allclose(converter.ned2ecef(ned_offsets_batch), ecef_positions_offset_batch, rtol=1e-9, atol=1e-7) + + def test_errors(self): + # Test wrong shape/type for geodetic2ecef + # numpy_wrap raises IndexError for scalar input + with np.testing.assert_raises(IndexError): + coord.geodetic2ecef(1.0) + + with np.testing.assert_raises_regex(ValueError, "Geodetic must be size 3"): + coord.geodetic2ecef([0, 0]) + + with np.testing.assert_raises_regex(ValueError, "Geodetic must be size 3"): + coord.geodetic2ecef([0, 0, 0, 0]) + + with np.testing.assert_raises(TypeError): + coord.geodetic2ecef(['a', 'b', 'c']) + + # Test LocalCoord constructor errors + with np.testing.assert_raises(ValueError): + coord.LocalCoord.from_geodetic([0, 0]) + + with np.testing.assert_raises(ValueError): + coord.LocalCoord.from_geodetic(1) + + with np.testing.assert_raises(TypeError): + coord.LocalCoord.from_geodetic(['a', 'b', 'c']) + + # Test wrong shape/type for ecef2geodetic + with np.testing.assert_raises(ValueError): + coord.ecef2geodetic([1, 2]) + with np.testing.assert_raises(ValueError): + coord.ecef2geodetic([1, 2, 3, 4]) + with np.testing.assert_raises(IndexError): + coord.ecef2geodetic(1.0) diff --git a/common/transformations/tests/test_orientation.py b/common/transformations/tests/test_orientation.py index 55fbc6581e..1bf94115c8 100644 --- a/common/transformations/tests/test_orientation.py +++ b/common/transformations/tests/test_orientation.py @@ -1,4 +1,5 @@ import numpy as np +import pytest from openpilot.common.transformations.orientation import euler2quat, quat2euler, euler2rot, rot2euler, \ rot2quat, quat2rot, \ @@ -59,3 +60,32 @@ class TestOrientation: 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) # np.testing.assert_allclose(ned_eulers, ned_euler_from_ecef(ecef_positions, eulers), rtol=1e-7) + + def test_inputs(self): + with pytest.raises(ValueError): + euler2quat([1, 2]) + + with pytest.raises(ValueError): + quat2rot([1, 2, 3]) + + with pytest.raises(IndexError): + rot2quat(np.zeros((2, 2))) + + def test_euler_rot_consistency(self): + rpy = [0.1, 0.2, 0.3] + R = euler2rot(rpy) + + # R -> q -> R + q = rot2quat(R) + R_new = quat2rot(q) + np.testing.assert_allclose(R, R_new, atol=1e-15) + + # q -> R -> Euler (quat2euler) -> R + rpy_new = quat2euler(q) + R_new2 = euler2rot(rpy_new) + np.testing.assert_allclose(R, R_new2, atol=1e-15) + + # R -> Euler (rot2euler) -> R + rpy_from_rot = rot2euler(R) + R_new3 = euler2rot(rpy_from_rot) + np.testing.assert_allclose(R, R_new3, atol=1e-15) diff --git a/common/transformations/transformations.pxd b/common/transformations/transformations.pxd deleted file mode 100644 index fe32e18dea..0000000000 --- a/common/transformations/transformations.pxd +++ /dev/null @@ -1,72 +0,0 @@ -# cython: language_level=3 -from libcpp cimport bool - -cdef extern from "orientation.cc": - pass - -cdef extern from "orientation.hpp": - cdef cppclass Quaternion "Eigen::Quaterniond": - Quaternion() - Quaternion(double, double, double, double) - double w() - double x() - double y() - double z() - - cdef cppclass Vector3 "Eigen::Vector3d": - Vector3() - Vector3(double, double, double) - double operator()(int) - - cdef cppclass Matrix3 "Eigen::Matrix3d": - Matrix3() - Matrix3(double*) - - double operator()(int, int) - - Quaternion euler2quat(const Vector3 &) - Vector3 quat2euler(const Quaternion &) - Matrix3 quat2rot(const Quaternion &) - Quaternion rot2quat(const Matrix3 &) - Vector3 rot2euler(const Matrix3 &) - Matrix3 euler2rot(const Vector3 &) - Matrix3 rot_matrix(double, double, double) - Vector3 ecef_euler_from_ned(const ECEF &, const Vector3 &) - Vector3 ned_euler_from_ecef(const ECEF &, const Vector3 &) - - -cdef extern from "coordinates.cc": - cdef struct ECEF: - double x - double y - double z - - cdef struct NED: - double n - double e - double d - - cdef struct Geodetic: - double lat - double lon - double alt - bool radians - - ECEF geodetic2ecef(const Geodetic &) - Geodetic ecef2geodetic(const ECEF &) - - cdef cppclass LocalCoord_c "LocalCoord": - Matrix3 ned2ecef_matrix - Matrix3 ecef2ned_matrix - - LocalCoord_c(const Geodetic &, const ECEF &) - LocalCoord_c(const Geodetic &) - LocalCoord_c(const ECEF &) - - NED ecef2ned(const ECEF &) - ECEF ned2ecef(const NED &) - NED geodetic2ned(const Geodetic &) - Geodetic ned2geodetic(const NED &) - -cdef extern from "coordinates.hpp": - pass diff --git a/common/transformations/transformations.py b/common/transformations/transformations.py new file mode 100644 index 0000000000..5cb6220f95 --- /dev/null +++ b/common/transformations/transformations.py @@ -0,0 +1,342 @@ +import numpy as np + + +# Constants +a = 6378137.0 +b = 6356752.3142 +esq = 6.69437999014e-3 +e1sq = 6.73949674228e-3 + + +def geodetic2ecef_single(g): + """ + Convert geodetic coordinates (latitude, longitude, altitude) to ECEF. + """ + try: + if len(g) != 3: + raise ValueError("Geodetic must be size 3") + except TypeError: + raise ValueError("Geodetic must be a sequence of length 3") from None + + lat, lon, alt = g + lat = np.radians(lat) + lon = np.radians(lon) + xi = np.sqrt(1.0 - esq * np.sin(lat)**2) + x = (a / xi + alt) * np.cos(lat) * np.cos(lon) + y = (a / xi + alt) * np.cos(lat) * np.sin(lon) + z = (a / xi * (1.0 - esq) + alt) * np.sin(lat) + return np.array([x, y, z]) + + +def ecef2geodetic_single(e): + """ + Convert ECEF to geodetic coordinates using Ferrari's solution. + """ + x, y, z = e + r = np.sqrt(x**2 + y**2) + Esq = a**2 - b**2 + F = 54 * b**2 * z**2 + G = r**2 + (1 - esq) * z**2 - esq * Esq + C = (esq**2 * F * r**2) / (G**3) + S = np.cbrt(1 + C + np.sqrt(C**2 + 2 * C)) + P = F / (3 * (S + 1 / S + 1)**2 * G**2) + Q = np.sqrt(1 + 2 * esq**2 * P) + r_0 = -(P * esq * r) / (1 + Q) + np.sqrt(0.5 * a**2 * (1 + 1.0 / Q) - P * (1 - esq) * z**2 / (Q * (1 + Q)) - 0.5 * P * r**2) + U = np.sqrt((r - esq * r_0)**2 + z**2) + V = np.sqrt((r - esq * r_0)**2 + (1 - esq) * z**2) + Z_0 = b**2 * z / (a * V) + h = U * (1 - b**2 / (a * V)) + lat = np.arctan((z + e1sq * Z_0) / r) + lon = np.arctan2(y, x) + return np.array([np.degrees(lat), np.degrees(lon), h]) + + +def euler2quat_single(euler): + """ + Convert Euler angles (roll, pitch, yaw) to a quaternion. + Rotation order: Z-Y-X (yaw, pitch, roll). + """ + phi, theta, psi = euler + + c_phi, s_phi = np.cos(phi / 2), np.sin(phi / 2) + c_theta, s_theta = np.cos(theta / 2), np.sin(theta / 2) + c_psi, s_psi = np.cos(psi / 2), np.sin(psi / 2) + + w = c_phi * c_theta * c_psi + s_phi * s_theta * s_psi + x = s_phi * c_theta * c_psi - c_phi * s_theta * s_psi + y = c_phi * s_theta * c_psi + s_phi * c_theta * s_psi + z = c_phi * c_theta * s_psi - s_phi * s_theta * c_psi + + if w < 0: + return np.array([-w, -x, -y, -z]) + return np.array([w, x, y, z]) + + +def quat2euler_single(q): + """ + Convert a quaternion to Euler angles (roll, pitch, yaw). + """ + w, x, y, z = q + gamma = np.arctan2(2 * (w * x + y * z), 1 - 2 * (x**2 + y**2)) + sin_arg = 2 * (w * y - z * x) + sin_arg = np.clip(sin_arg, -1.0, 1.0) + theta = np.arcsin(sin_arg) + psi = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y**2 + z**2)) + return np.array([gamma, theta, psi]) + + +def quat2rot_single(q): + """ + Convert a quaternion to a 3x3 rotation matrix. + """ + w, x, y, z = q + xx, yy, zz = x * x, y * y, z * z + xy, xz, yz = x * y, x * z, y * z + wx, wy, wz = w * x, w * y, w * z + + mat = np.array([ + [1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy)], + [2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx)], + [2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy)] + ]) + return mat + + +def rot2quat_single(rot): + """ + Convert a 3x3 rotation matrix to a quaternion. + """ + trace = np.trace(rot) + if trace > 0: + s = 0.5 / np.sqrt(trace + 1.0) + w = 0.25 / s + x = (rot[2, 1] - rot[1, 2]) * s + y = (rot[0, 2] - rot[2, 0]) * s + z = (rot[1, 0] - rot[0, 1]) * s + else: + if rot[0, 0] > rot[1, 1] and rot[0, 0] > rot[2, 2]: + s = 2.0 * np.sqrt(1.0 + rot[0, 0] - rot[1, 1] - rot[2, 2]) + w = (rot[2, 1] - rot[1, 2]) / s + x = 0.25 * s + y = (rot[0, 1] + rot[1, 0]) / s + z = (rot[0, 2] + rot[2, 0]) / s + elif rot[1, 1] > rot[2, 2]: + s = 2.0 * np.sqrt(1.0 + rot[1, 1] - rot[0, 0] - rot[2, 2]) + w = (rot[0, 2] - rot[2, 0]) / s + x = (rot[0, 1] + rot[1, 0]) / s + y = 0.25 * s + z = (rot[1, 2] + rot[2, 1]) / s + else: + s = 2.0 * np.sqrt(1.0 + rot[2, 2] - rot[0, 0] - rot[1, 1]) + w = (rot[1, 0] - rot[0, 1]) / s + x = (rot[0, 2] + rot[2, 0]) / s + y = (rot[1, 2] + rot[2, 1]) / s + z = 0.25 * s + + if w < 0: + return np.array([-w, -x, -y, -z]) + return np.array([w, x, y, z]) + + +def euler2rot_single(euler): + """ + Convert Euler angles (roll, pitch, yaw) to a 3x3 rotation matrix. + Rotation order: Z-Y-X (yaw, pitch, roll). + """ + phi, theta, psi = euler + + cx, sx = np.cos(phi), np.sin(phi) + cy, sy = np.cos(theta), np.sin(theta) + cz, sz = np.cos(psi), np.sin(psi) + + Rx = np.array([[1, 0, 0], [0, cx, -sx], [0, sx, cx]]) + Ry = np.array([[cy, 0, sy], [0, 1, 0], [-sy, 0, cy]]) + Rz = np.array([[cz, -sz, 0], [sz, cz, 0], [0, 0, 1]]) + + return Rz @ Ry @ Rx + + +def rot2euler_single(rot): + """ + Convert a 3x3 rotation matrix to Euler angles (roll, pitch, yaw). + """ + return quat2euler_single(rot2quat_single(rot)) + + +def rot_matrix(roll, pitch, yaw): + """ + Create a 3x3 rotation matrix from roll, pitch, and yaw angles. + """ + return euler2rot_single([roll, pitch, yaw]) + + +def axis_angle_to_rot(axis, angle): + """ + Convert an axis-angle representation to a 3x3 rotation matrix. + """ + c = np.cos(angle / 2) + s = np.sin(angle / 2) + q = np.array([c, s*axis[0], s*axis[1], s*axis[2]]) + return quat2rot_single(q) + + +class LocalCoord: + """ + A class to handle conversions between ECEF and local NED coordinates. + """ + def __init__(self, geodetic=None, ecef=None): + """ + Initialize LocalCoord with either geodetic or ECEF coordinates. + """ + if geodetic is not None: + self.init_ecef = geodetic2ecef_single(geodetic) + lat, lon, _ = geodetic + elif ecef is not None: + self.init_ecef = np.array(ecef) + lat, lon, _ = ecef2geodetic_single(ecef) + else: + raise ValueError("Must provide geodetic or ecef") + + lat = np.radians(lat) + lon = np.radians(lon) + + self.ned2ecef_matrix = np.array([ + [-np.sin(lat) * np.cos(lon), -np.sin(lon), -np.cos(lat) * np.cos(lon)], + [-np.sin(lat) * np.sin(lon), np.cos(lon), -np.cos(lat) * np.sin(lon)], + [np.cos(lat), 0, -np.sin(lat)] + ]) + self.ecef2ned_matrix = self.ned2ecef_matrix.T + + @classmethod + def from_geodetic(cls, geodetic): + """ + Create a LocalCoord instance from geodetic coordinates. + """ + return cls(geodetic=geodetic) + + @classmethod + def from_ecef(cls, ecef): + """ + Create a LocalCoord instance from ECEF coordinates. + """ + return cls(ecef=ecef) + + def ecef2ned_single(self, ecef): + """ + Convert a single ECEF point to NED coordinates relative to the origin. + """ + return self.ecef2ned_matrix @ (ecef - self.init_ecef) + + def ned2ecef_single(self, ned): + """ + Convert a single NED point to ECEF coordinates. + """ + return self.ned2ecef_matrix @ ned + self.init_ecef + + def geodetic2ned_single(self, geodetic): + """ + Convert a single geodetic point to NED coordinates. + """ + ecef = geodetic2ecef_single(geodetic) + return self.ecef2ned_single(ecef) + + def ned2geodetic_single(self, ned): + """ + Convert a single NED point to geodetic coordinates. + """ + ecef = self.ned2ecef_single(ned) + return ecef2geodetic_single(ecef) + + @property + def ned_from_ecef_matrix(self): + """ + Returns the rotation matrix from ECEF to NED coordinates. + """ + return self.ecef2ned_matrix + + @property + def ecef_from_ned_matrix(self): + """ + Returns the rotation matrix from NED to ECEF coordinates. + """ + return self.ned2ecef_matrix + + +def ecef_euler_from_ned_single(ecef_init, ned_pose): + """ + Convert NED Euler angles (roll, pitch, yaw) at a given ECEF origin + to equivalent ECEF Euler angles. + """ + converter = LocalCoord(ecef=ecef_init) + zero = np.array(ecef_init) + + x0 = converter.ned2ecef_single([1, 0, 0]) - zero + y0 = converter.ned2ecef_single([0, 1, 0]) - zero + z0 = converter.ned2ecef_single([0, 0, 1]) - zero + + phi, theta, psi = ned_pose + + x1 = axis_angle_to_rot(z0, psi) @ x0 + y1 = axis_angle_to_rot(z0, psi) @ y0 + z1 = axis_angle_to_rot(z0, psi) @ z0 + + x2 = axis_angle_to_rot(y1, theta) @ x1 + y2 = axis_angle_to_rot(y1, theta) @ y1 + z2 = axis_angle_to_rot(y1, theta) @ z1 + + x3 = axis_angle_to_rot(x2, phi) @ x2 + y3 = axis_angle_to_rot(x2, phi) @ y2 + + x0 = np.array([1.0, 0, 0]) + y0 = np.array([0, 1.0, 0]) + z0 = np.array([0, 0, 1.0]) + + psi_out = np.arctan2(np.dot(x3, y0), np.dot(x3, x0)) + theta_out = np.arctan2(-np.dot(x3, z0), np.sqrt(np.dot(x3, x0)**2 + np.dot(x3, y0)**2)) + + y2 = axis_angle_to_rot(z0, psi_out) @ y0 + z2 = axis_angle_to_rot(y2, theta_out) @ z0 + + phi_out = np.arctan2(np.dot(y3, z2), np.dot(y3, y2)) + + return np.array([phi_out, theta_out, psi_out]) + + +def ned_euler_from_ecef_single(ecef_init, ecef_pose): + """ + Convert ECEF Euler angles (roll, pitch, yaw) at a given ECEF origin + to equivalent NED Euler angles. + """ + converter = LocalCoord(ecef=ecef_init) + + x0 = np.array([1.0, 0, 0]) + y0 = np.array([0, 1.0, 0]) + z0 = np.array([0, 0, 1.0]) + + phi, theta, psi = ecef_pose + + x1 = axis_angle_to_rot(z0, psi) @ x0 + y1 = axis_angle_to_rot(z0, psi) @ y0 + z1 = axis_angle_to_rot(z0, psi) @ z0 + + x2 = axis_angle_to_rot(y1, theta) @ x1 + y2 = axis_angle_to_rot(y1, theta) @ y1 + z2 = axis_angle_to_rot(y1, theta) @ z1 + + x3 = axis_angle_to_rot(x2, phi) @ x2 + y3 = axis_angle_to_rot(x2, phi) @ y2 + + zero = np.array(ecef_init) + x0 = converter.ned2ecef_single([1, 0, 0]) - zero + y0 = converter.ned2ecef_single([0, 1, 0]) - zero + z0 = converter.ned2ecef_single([0, 0, 1]) - zero + + psi_out = np.arctan2(np.dot(x3, y0), np.dot(x3, x0)) + theta_out = np.arctan2(-np.dot(x3, z0), np.sqrt(np.dot(x3, x0)**2 + np.dot(x3, y0)**2)) + + y2 = axis_angle_to_rot(z0, psi_out) @ y0 + z2 = axis_angle_to_rot(y2, theta_out) @ z0 + + phi_out = np.arctan2(np.dot(y3, z2), np.dot(y3, y2)) + + return np.array([phi_out, theta_out, psi_out]) diff --git a/common/transformations/transformations.pyx b/common/transformations/transformations.pyx deleted file mode 100644 index ae045c369d..0000000000 --- a/common/transformations/transformations.pyx +++ /dev/null @@ -1,173 +0,0 @@ -# distutils: language = c++ -# cython: language_level = 3 -from openpilot.common.transformations.transformations cimport Matrix3, Vector3, Quaternion -from openpilot.common.transformations.transformations cimport ECEF, NED, Geodetic - -from openpilot.common.transformations.transformations cimport euler2quat as euler2quat_c -from openpilot.common.transformations.transformations cimport quat2euler as quat2euler_c -from openpilot.common.transformations.transformations cimport quat2rot as quat2rot_c -from openpilot.common.transformations.transformations cimport rot2quat as rot2quat_c -from openpilot.common.transformations.transformations cimport euler2rot as euler2rot_c -from openpilot.common.transformations.transformations cimport rot2euler as rot2euler_c -from openpilot.common.transformations.transformations cimport rot_matrix as rot_matrix_c -from openpilot.common.transformations.transformations cimport ecef_euler_from_ned as ecef_euler_from_ned_c -from openpilot.common.transformations.transformations cimport ned_euler_from_ecef as ned_euler_from_ecef_c -from openpilot.common.transformations.transformations cimport geodetic2ecef as geodetic2ecef_c -from openpilot.common.transformations.transformations cimport ecef2geodetic as ecef2geodetic_c -from openpilot.common.transformations.transformations cimport LocalCoord_c - - -import numpy as np -cimport numpy as np - -cdef np.ndarray[double, ndim=2] matrix2numpy(Matrix3 m): - return np.array([ - [m(0, 0), m(0, 1), m(0, 2)], - [m(1, 0), m(1, 1), m(1, 2)], - [m(2, 0), m(2, 1), m(2, 2)], - ]) - -cdef Matrix3 numpy2matrix(np.ndarray[double, ndim=2, mode="fortran"] m): - assert m.shape[0] == 3 - assert m.shape[1] == 3 - return Matrix3(m.data) - -cdef ECEF list2ecef(ecef): - cdef ECEF e - e.x = ecef[0] - e.y = ecef[1] - e.z = ecef[2] - return e - -cdef NED list2ned(ned): - cdef NED n - n.n = ned[0] - n.e = ned[1] - n.d = ned[2] - return n - -cdef Geodetic list2geodetic(geodetic): - cdef Geodetic g - g.lat = geodetic[0] - g.lon = geodetic[1] - g.alt = geodetic[2] - return g - -def euler2quat_single(euler): - cdef Vector3 e = Vector3(euler[0], euler[1], euler[2]) - cdef Quaternion q = euler2quat_c(e) - return [q.w(), q.x(), q.y(), q.z()] - -def quat2euler_single(quat): - cdef Quaternion q = Quaternion(quat[0], quat[1], quat[2], quat[3]) - cdef Vector3 e = quat2euler_c(q) - return [e(0), e(1), e(2)] - -def quat2rot_single(quat): - cdef Quaternion q = Quaternion(quat[0], quat[1], quat[2], quat[3]) - cdef Matrix3 r = quat2rot_c(q) - return matrix2numpy(r) - -def rot2quat_single(rot): - cdef Matrix3 r = numpy2matrix(np.asfortranarray(rot, dtype=np.double)) - cdef Quaternion q = rot2quat_c(r) - return [q.w(), q.x(), q.y(), q.z()] - -def euler2rot_single(euler): - cdef Vector3 e = Vector3(euler[0], euler[1], euler[2]) - cdef Matrix3 r = euler2rot_c(e) - return matrix2numpy(r) - -def rot2euler_single(rot): - cdef Matrix3 r = numpy2matrix(np.asfortranarray(rot, dtype=np.double)) - cdef Vector3 e = rot2euler_c(r) - return [e(0), e(1), e(2)] - -def rot_matrix(roll, pitch, yaw): - return matrix2numpy(rot_matrix_c(roll, pitch, yaw)) - -def ecef_euler_from_ned_single(ecef_init, ned_pose): - cdef ECEF init = list2ecef(ecef_init) - cdef Vector3 pose = Vector3(ned_pose[0], ned_pose[1], ned_pose[2]) - - cdef Vector3 e = ecef_euler_from_ned_c(init, pose) - return [e(0), e(1), e(2)] - -def ned_euler_from_ecef_single(ecef_init, ecef_pose): - cdef ECEF init = list2ecef(ecef_init) - cdef Vector3 pose = Vector3(ecef_pose[0], ecef_pose[1], ecef_pose[2]) - - cdef Vector3 e = ned_euler_from_ecef_c(init, pose) - return [e(0), e(1), e(2)] - -def geodetic2ecef_single(geodetic): - cdef Geodetic g = list2geodetic(geodetic) - cdef ECEF e = geodetic2ecef_c(g) - return [e.x, e.y, e.z] - -def ecef2geodetic_single(ecef): - cdef ECEF e = list2ecef(ecef) - cdef Geodetic g = ecef2geodetic_c(e) - return [g.lat, g.lon, g.alt] - - -cdef class LocalCoord: - cdef LocalCoord_c * lc - - def __init__(self, geodetic=None, ecef=None): - assert (geodetic is not None) or (ecef is not None) - if geodetic is not None: - self.lc = new LocalCoord_c(list2geodetic(geodetic)) - elif ecef is not None: - self.lc = new LocalCoord_c(list2ecef(ecef)) - - @property - def ned2ecef_matrix(self): - return matrix2numpy(self.lc.ned2ecef_matrix) - - @property - def ecef2ned_matrix(self): - return matrix2numpy(self.lc.ecef2ned_matrix) - - @property - def ned_from_ecef_matrix(self): - return self.ecef2ned_matrix - - @property - def ecef_from_ned_matrix(self): - return self.ned2ecef_matrix - - @classmethod - def from_geodetic(cls, geodetic): - return cls(geodetic=geodetic) - - @classmethod - def from_ecef(cls, ecef): - return cls(ecef=ecef) - - def ecef2ned_single(self, ecef): - assert self.lc - cdef ECEF e = list2ecef(ecef) - cdef NED n = self.lc.ecef2ned(e) - return [n.n, n.e, n.d] - - def ned2ecef_single(self, ned): - assert self.lc - cdef NED n = list2ned(ned) - cdef ECEF e = self.lc.ned2ecef(n) - return [e.x, e.y, e.z] - - def geodetic2ned_single(self, geodetic): - assert self.lc - cdef Geodetic g = list2geodetic(geodetic) - cdef NED n = self.lc.geodetic2ned(g) - return [n.n, n.e, n.d] - - def ned2geodetic_single(self, ned): - assert self.lc - cdef NED n = list2ned(ned) - cdef Geodetic g = self.lc.ned2geodetic(n) - return [g.lat, g.lon, g.alt] - - def __dealloc__(self): - del self.lc diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript index 8b33a457f2..a184b6a23d 100644 --- a/selfdrive/modeld/SConscript +++ b/selfdrive/modeld/SConscript @@ -1,7 +1,7 @@ import os import glob -Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'visionipc', 'transformations') +Import('env', 'envCython', 'arch', 'cereal', 'messaging', 'common', 'visionipc') lenv = env.Clone() lenvCython = envCython.Clone()