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 refpull/1651/head
Willem Melching
5 years ago
committed by
GitHub
parent
90d97de74d
commit
c18e7da3c2
22 changed files with 683 additions and 420 deletions
@ -1 +1 @@ |
||||
Subproject commit 1aaf1bfd7c07e1c5184e8f13823e8ed02e2c7af2 |
||||
Subproject commit 0021fa241994cd9d94945ba305b0f3f1c43feaae |
||||
@ -0,0 +1,2 @@ |
||||
transformations |
||||
transformations.cpp |
||||
@ -0,0 +1,9 @@ |
||||
Import('env') |
||||
|
||||
d = Dir('.') |
||||
|
||||
env.Command( |
||||
['transformations.so'], |
||||
['transformations.pxd', 'transformations.pyx', |
||||
'coordinates.cc', 'orientation.cc', 'coordinates.hpp', 'orientation.hpp'], |
||||
'cd ' + d.path + ' && python3 setup.py build_ext --inplace') |
||||
@ -0,0 +1,104 @@ |
||||
#define _USE_MATH_DEFINES |
||||
|
||||
#include <iostream> |
||||
#include <cmath> |
||||
#include <eigen3/Eigen/Dense> |
||||
|
||||
#include "coordinates.hpp" |
||||
|
||||
#define DEG2RAD(x) ((x) * M_PI / 180.0) |
||||
#define RAD2DEG(x) ((x) * 180.0 / M_PI) |
||||
|
||||
|
||||
double a = 6378137; |
||||
double b = 6356752.3142; |
||||
double esq = 6.69437999014 * 0.001; |
||||
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(Geodetic g){ |
||||
g = to_radians(g); |
||||
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(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(Geodetic g, ECEF e){ |
||||
init_ecef << e.x, e.y, e.z; |
||||
|
||||
g = to_radians(g); |
||||
|
||||
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(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(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(Geodetic g) { |
||||
ECEF e = ::geodetic2ecef(g); |
||||
return ecef2ned(e); |
||||
} |
||||
|
||||
Geodetic LocalCoord::ned2geodetic(NED n){ |
||||
ECEF e = ned2ecef(n); |
||||
return ::ecef2geodetic(e); |
||||
} |
||||
@ -0,0 +1,36 @@ |
||||
#pragma once |
||||
|
||||
struct ECEF { |
||||
double x, y, z; |
||||
Eigen::Vector3d to_vector(){ |
||||
return Eigen::Vector3d(x, y, z); |
||||
} |
||||
}; |
||||
|
||||
struct NED { |
||||
double n, e, d; |
||||
}; |
||||
|
||||
struct Geodetic { |
||||
double lat, lon, alt; |
||||
bool radians=false; |
||||
}; |
||||
|
||||
ECEF geodetic2ecef(Geodetic g); |
||||
Geodetic ecef2geodetic(ECEF e); |
||||
|
||||
class LocalCoord { |
||||
private: |
||||
Eigen::Matrix3d ned2ecef_matrix; |
||||
Eigen::Matrix3d ecef2ned_matrix; |
||||
Eigen::Vector3d init_ecef; |
||||
public: |
||||
LocalCoord(Geodetic g, ECEF e); |
||||
LocalCoord(Geodetic g) : LocalCoord(g, ::geodetic2ecef(g)) {} |
||||
LocalCoord(ECEF e) : LocalCoord(::ecef2geodetic(e), e) {} |
||||
|
||||
NED ecef2ned(ECEF e); |
||||
ECEF ned2ecef(NED n); |
||||
NED geodetic2ned(Geodetic g); |
||||
Geodetic ned2geodetic(NED n); |
||||
}; |
||||
@ -0,0 +1,147 @@ |
||||
#define _USE_MATH_DEFINES |
||||
|
||||
#include <iostream> |
||||
#include <cmath> |
||||
#include <eigen3/Eigen/Dense> |
||||
|
||||
#include "orientation.hpp" |
||||
#include "coordinates.hpp" |
||||
|
||||
Eigen::Quaterniond ensure_unique(Eigen::Quaterniond quat){ |
||||
if (quat.w() > 0){ |
||||
return quat; |
||||
} else { |
||||
return Eigen::Quaterniond(-quat.w(), -quat.x(), -quat.y(), -quat.z()); |
||||
} |
||||
} |
||||
|
||||
Eigen::Quaterniond euler2quat(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(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 theta = asin(2 * (quat.w() * quat.y() - quat.z() * quat.x())); |
||||
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(Eigen::Quaterniond quat){ |
||||
return quat.toRotationMatrix(); |
||||
} |
||||
|
||||
Eigen::Quaterniond rot2quat(Eigen::Matrix3d rot){ |
||||
return ensure_unique(Eigen::Quaterniond(rot)); |
||||
} |
||||
|
||||
Eigen::Matrix3d euler2rot(Eigen::Vector3d euler){ |
||||
return quat2rot(euler2quat(euler)); |
||||
} |
||||
|
||||
Eigen::Vector3d rot2euler(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(Eigen::Vector3d axis, double angle){ |
||||
Eigen::Quaterniond q; |
||||
q = Eigen::AngleAxisd(angle, axis); |
||||
return q.toRotationMatrix(); |
||||
} |
||||
|
||||
|
||||
Eigen::Vector3d ecef_euler_from_ned(ECEF ecef_init, 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(ECEF ecef_init, 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}; |
||||
} |
||||
|
||||
|
||||
|
||||
int main(void){ |
||||
} |
||||
@ -0,0 +1,17 @@ |
||||
#pragma once |
||||
#include <eigen3/Eigen/Dense> |
||||
#include "coordinates.hpp" |
||||
|
||||
|
||||
Eigen::Quaterniond ensure_unique(Eigen::Quaterniond quat); |
||||
|
||||
Eigen::Quaterniond euler2quat(Eigen::Vector3d euler); |
||||
Eigen::Vector3d quat2euler(Eigen::Quaterniond quat); |
||||
Eigen::Matrix3d quat2rot(Eigen::Quaterniond quat); |
||||
Eigen::Quaterniond rot2quat(Eigen::Matrix3d rot); |
||||
Eigen::Matrix3d euler2rot(Eigen::Vector3d euler); |
||||
Eigen::Vector3d rot2euler(Eigen::Matrix3d rot); |
||||
Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw); |
||||
Eigen::Matrix3d rot(Eigen::Vector3d axis, double angle); |
||||
Eigen::Vector3d ecef_euler_from_ned(ECEF ecef_init, Eigen::Vector3d ned_pose); |
||||
Eigen::Vector3d ned_euler_from_ecef(ECEF ecef_init, Eigen::Vector3d ecef_pose); |
||||
@ -0,0 +1,42 @@ |
||||
import os |
||||
import numpy |
||||
import sysconfig |
||||
|
||||
from Cython.Build import cythonize |
||||
from Cython.Distutils import build_ext |
||||
from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module |
||||
|
||||
def get_ext_filename_without_platform_suffix(filename): |
||||
name, ext = os.path.splitext(filename) |
||||
ext_suffix = sysconfig.get_config_var('EXT_SUFFIX') |
||||
|
||||
if ext_suffix == ext: |
||||
return filename |
||||
|
||||
ext_suffix = ext_suffix.replace(ext, '') |
||||
idx = name.find(ext_suffix) |
||||
|
||||
if idx == -1: |
||||
return filename |
||||
else: |
||||
return name[:idx] + ext |
||||
|
||||
|
||||
class BuildExtWithoutPlatformSuffix(build_ext): |
||||
def get_ext_filename(self, ext_name): |
||||
filename = super().get_ext_filename(ext_name) |
||||
return get_ext_filename_without_platform_suffix(filename) |
||||
|
||||
|
||||
setup( |
||||
name='Cython transformations wrapper', |
||||
cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, |
||||
ext_modules=cythonize( |
||||
Extension( |
||||
"transformations", |
||||
sources=["transformations.pyx"], |
||||
language="c++", |
||||
extra_compile_args=["-std=c++14"], |
||||
include_dirs=[numpy.get_include()], |
||||
) |
||||
)) |
||||
@ -0,0 +1,68 @@ |
||||
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(Vector3) |
||||
Vector3 quat2euler(Quaternion) |
||||
Matrix3 quat2rot(Quaternion) |
||||
Quaternion rot2quat(Matrix3) |
||||
Vector3 rot2euler(Matrix3) |
||||
Matrix3 euler2rot(Vector3) |
||||
Matrix3 rot_matrix(double, double, double) |
||||
Vector3 ecef_euler_from_ned(ECEF, Vector3) |
||||
Vector3 ned_euler_from_ecef(ECEF, 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(Geodetic) |
||||
Geodetic ecef2geodetic(ECEF) |
||||
|
||||
cdef cppclass LocalCoord_c "LocalCoord": |
||||
LocalCoord_c(Geodetic, ECEF) |
||||
LocalCoord_c(Geodetic) |
||||
LocalCoord_c(ECEF) |
||||
|
||||
NED ecef2ned(ECEF) |
||||
ECEF ned2ecef(NED) |
||||
NED geodetic2ned(Geodetic) |
||||
Geodetic ned2geodetic(NED) |
||||
|
||||
cdef extern from "coordinates.hpp": |
||||
pass |
||||
@ -0,0 +1,156 @@ |
||||
from transformations cimport Matrix3, Vector3, Quaternion |
||||
from transformations cimport ECEF, NED, Geodetic |
||||
|
||||
from transformations cimport euler2quat as euler2quat_c |
||||
from transformations cimport quat2euler as quat2euler_c |
||||
from transformations cimport quat2rot as quat2rot_c |
||||
from transformations cimport rot2quat as rot2quat_c |
||||
from transformations cimport euler2rot as euler2rot_c |
||||
from transformations cimport rot2euler as rot2euler_c |
||||
from transformations cimport rot_matrix as rot_matrix_c |
||||
from transformations cimport ecef_euler_from_ned as ecef_euler_from_ned_c |
||||
from transformations cimport ned_euler_from_ecef as ned_euler_from_ecef_c |
||||
from transformations cimport geodetic2ecef as geodetic2ecef_c |
||||
from transformations cimport ecef2geodetic as ecef2geodetic_c |
||||
from transformations cimport LocalCoord_c |
||||
|
||||
|
||||
import cython |
||||
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)) |
||||
|
||||
@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 +1 @@ |
||||
0533f640ab27f7b5af57aa4ebf4a29200550b3e8 |
||||
834f4cd7e90ff266ced8ea142d7d7d05076186aa |
||||
Loading…
Reference in new issue