mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-02-18 17:43:54 +08:00
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
This commit is contained in:
Matt Purnell
@@ -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')
|
||||
|
||||
@@ -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')
|
||||
@@ -1,100 +0,0 @@
|
||||
#define _USE_MATH_DEFINES
|
||||
|
||||
#include "common/transformations/coordinates.hpp"
|
||||
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
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);
|
||||
}
|
||||
@@ -1,43 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
#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);
|
||||
};
|
||||
@@ -1,144 +0,0 @@
|
||||
#define _USE_MATH_DEFINES
|
||||
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
#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};
|
||||
}
|
||||
|
||||
@@ -1,17 +0,0 @@
|
||||
#pragma once
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#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);
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
342
common/transformations/transformations.py
Normal file
342
common/transformations/transformations.py
Normal file
@@ -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])
|
||||
@@ -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(<double*>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
|
||||
@@ -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()
|
||||
|
||||
|
||||
Reference in New Issue
Block a user