mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-10-01 03:43:51 +08:00
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
This commit is contained in:
Willem Melching
@@ -1,113 +1,19 @@
|
||||
"""
|
||||
Coordinate transformation module. All methods accept arrays as input
|
||||
with each row as a position.
|
||||
"""
|
||||
import numpy as np
|
||||
# pylint: skip-file
|
||||
from common.transformations.orientation import numpy_wrap
|
||||
from common.transformations.transformations import (ecef2geodetic_single,
|
||||
geodetic2ecef_single)
|
||||
from common.transformations.transformations import LocalCoord as LocalCoord_single
|
||||
|
||||
|
||||
a = 6378137
|
||||
b = 6356752.3142
|
||||
esq = 6.69437999014 * 0.001
|
||||
e1sq = 6.73949674228 * 0.001
|
||||
class LocalCoord(LocalCoord_single):
|
||||
ecef2ned = numpy_wrap(LocalCoord_single.ecef2ned_single, (3,), (3,))
|
||||
ned2ecef = numpy_wrap(LocalCoord_single.ned2ecef_single, (3,), (3,))
|
||||
geodetic2ned = numpy_wrap(LocalCoord_single.geodetic2ned_single, (3,), (3,))
|
||||
ned2geodetic = numpy_wrap(LocalCoord_single.ned2geodetic_single, (3,), (3,))
|
||||
|
||||
|
||||
def geodetic2ecef(geodetic, radians=False):
|
||||
geodetic = np.array(geodetic)
|
||||
input_shape = geodetic.shape
|
||||
geodetic = np.atleast_2d(geodetic)
|
||||
|
||||
ratio = 1.0 if radians else (np.pi / 180.0)
|
||||
lat = ratio*geodetic[:, 0]
|
||||
lon = ratio*geodetic[:, 1]
|
||||
alt = geodetic[:, 2]
|
||||
|
||||
xi = np.sqrt(1 - 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 - esq) + alt) * np.sin(lat)
|
||||
ecef = np.array([x, y, z]).T
|
||||
return ecef.reshape(input_shape)
|
||||
|
||||
|
||||
def ecef2geodetic(ecef, radians=False):
|
||||
"""
|
||||
Convert ECEF coordinates to geodetic using ferrari's method
|
||||
"""
|
||||
# Save shape and export column
|
||||
ecef = np.atleast_1d(ecef)
|
||||
input_shape = ecef.shape
|
||||
ecef = np.atleast_2d(ecef)
|
||||
x, y, z = ecef[:, 0], ecef[:, 1], ecef[:, 2]
|
||||
|
||||
ratio = 1.0 if radians else (180.0 / np.pi)
|
||||
|
||||
# Conver from ECEF to geodetic using Ferrari's methods
|
||||
# https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#Ferrari.27s_solution
|
||||
r = np.sqrt(x * x + y * y)
|
||||
Esq = a * a - b * b
|
||||
F = 54 * b * b * z * z
|
||||
G = r * r + (1 - esq) * z * z - esq * Esq
|
||||
C = (esq * esq * F * r * r) / (pow(G, 3))
|
||||
S = np.cbrt(1 + C + np.sqrt(C * C + 2 * C))
|
||||
P = F / (3 * pow((S + 1 / S + 1), 2) * G * G)
|
||||
Q = np.sqrt(1 + 2 * esq * esq * P)
|
||||
r_0 = -(P * esq * r) / (1 + Q) + np.sqrt(0.5 * a * a*(1 + 1.0 / Q) -
|
||||
P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r)
|
||||
U = np.sqrt(pow((r - esq * r_0), 2) + z * z)
|
||||
V = np.sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z)
|
||||
Z_0 = b * b * z / (a * V)
|
||||
h = U * (1 - b * b / (a * V))
|
||||
lat = ratio*np.arctan((z + e1sq * Z_0) / r)
|
||||
lon = ratio*np.arctan2(y, x)
|
||||
|
||||
# stack the new columns and return to the original shape
|
||||
geodetic = np.column_stack((lat, lon, h))
|
||||
return geodetic.reshape(input_shape)
|
||||
|
||||
geodetic2ecef = numpy_wrap(geodetic2ecef_single, (3,), (3,))
|
||||
ecef2geodetic = numpy_wrap(ecef2geodetic_single, (3,), (3,))
|
||||
|
||||
geodetic_from_ecef = ecef2geodetic
|
||||
ecef_from_geodetic = geodetic2ecef
|
||||
|
||||
|
||||
class LocalCoord():
|
||||
"""
|
||||
Allows conversions to local frames. In this case NED.
|
||||
That is: North East Down from the start position in
|
||||
meters.
|
||||
"""
|
||||
def __init__(self, init_geodetic, init_ecef):
|
||||
self.init_ecef = init_ecef
|
||||
lat, lon, _ = (np.pi/180)*np.array(init_geodetic)
|
||||
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
|
||||
self.ecef_from_ned_matrix = self.ned2ecef_matrix
|
||||
self.ned_from_ecef_matrix = self.ecef2ned_matrix
|
||||
|
||||
@classmethod
|
||||
def from_geodetic(cls, init_geodetic):
|
||||
init_ecef = geodetic2ecef(init_geodetic)
|
||||
return LocalCoord(init_geodetic, init_ecef)
|
||||
|
||||
@classmethod
|
||||
def from_ecef(cls, init_ecef):
|
||||
init_geodetic = ecef2geodetic(init_ecef)
|
||||
return LocalCoord(init_geodetic, init_ecef)
|
||||
|
||||
def ecef2ned(self, ecef):
|
||||
ecef = np.array(ecef)
|
||||
return np.dot(self.ecef2ned_matrix, (ecef - self.init_ecef).T).T
|
||||
|
||||
def ned2ecef(self, ned):
|
||||
ned = np.array(ned)
|
||||
# Transpose so that init_ecef will broadcast correctly for 1d or 2d ned.
|
||||
return (np.dot(self.ned2ecef_matrix, ned.T).T + self.init_ecef)
|
||||
|
||||
def geodetic2ned(self, geodetic):
|
||||
ecef = geodetic2ecef(geodetic)
|
||||
return self.ecef2ned(ecef)
|
||||
|
||||
def ned2geodetic(self, ned):
|
||||
ecef = self.ned2ecef(ned)
|
||||
return ecef2geodetic(ecef)
|
||||
|
||||
Reference in New Issue
Block a user