IQ.Pilot Release Commit @ b6534c0

iqpilot/common/transformations/.gitignore

@@ -0,0 +1,2 @@
+transformations
+transformations.cpp

iqpilot/common/transformations/README.md

@@ -0,0 +1,70 @@
+
+Reference Frames
+------
+Many reference frames are used throughout. This
+folder contains all helper functions needed to
+transform between them. Generally this is done
+by generating a rotation matrix and multiplying.
+
+
+| Name | [x, y, z] | Units | Notes |
+| :-------------: |:-------------:| :-----:| :----: |
+| Geodetic | [Latitude, Longitude, Altitude] | geodetic coordinates | Sometimes used as [lon, lat, alt], avoid this frame. |
+| ECEF | [x, y, z] | meters | We use **ITRF14 (IGS14)**, NOT NAD83. <br> This is the global Mesh3D frame. |
+| NED | [North, East, Down] | meters | Relative to earth's surface, useful for visualizing. |
+| Device | [Forward, Right, Down] | meters | This is the Mesh3D local frame. <br> Relative to camera, **not imu.** <br> ![img](http://upload.wikimedia.org/wikipedia/commons/thumb/2/2f/RPY_angles_of_airplanes.png/440px-RPY_angles_of_airplanes.png)|
+| Calibrated | [Forward, Right, Down] | meters | This is the frame the model outputs are in. <br> More details below. <br>|
+| Car | [Forward, Right, Down] | meters | This is useful for estimating position of points on the road. <br> More details below. <br>|
+| View | [Right, Down, Forward] | meters | Like device frame, but according to camera conventions. |
+| Camera | [u, v, focal] | pixels | Like view frame, but 2d on the camera image.|
+| Normalized Camera | [u / focal, v / focal, 1] | / | |
+| Model | [u, v, focal] | pixels | The sampled rectangle of the full camera frame the model uses. |
+| Normalized Model | [u / focal, v / focal, 1] | / | |
+
+
+
+
+Orientation Conventions
+------
+Quaternions, rotation matrices and euler angles are three
+equivalent representations of orientation and all three are
+used throughout the code base.
+
+For euler angles the preferred convention is [roll, pitch, yaw]
+which corresponds to rotations around the [x, y, z] axes. All
+euler angles should always be in radians or radians/s unless
+for plotting or display purposes. For quaternions the hamilton
+notations is preferred which is [q<sub>w</sub>, q<sub>x</sub>, q<sub>y</sub>, q<sub>z</sub>]. All quaternions
+should always be normalized with a strictly positive q<sub>w</sub>. **These
+quaternions are a unique representation of orientation whereas euler angles
+or rotation matrices are not.**
+
+To rotate from one frame into another with euler angles the
+convention is to rotate around roll, then pitch and then yaw,
+while rotating around the rotated axes, not the original axes.
+
+
+Car frame
+------
+Device frame is aligned with the road-facing camera used by openpilot. However, when controlling the vehicle it is helpful to think in a reference frame aligned with the vehicle. These two reference frames can be different.
+
+The orientation of car frame is defined to be aligned with the car's direction of travel and the road plane when the vehicle is driving on a flat road and not turning. The origin of car frame is defined to be directly below device frame (in car frame), such that it is on the road plane. The position and orientation of this frame is not necessarily always aligned with the direction of travel or the road plane due to suspension movements and other effects.
+
+
+Calibrated frame
+------
+It is helpful for openpilot's driving model to take in images that look similar when mounted differently in different cars. To achieve this we "calibrate" the images by transforming it into calibrated frame. Calibrated frame is defined to be aligned with car frame in pitch and yaw, and aligned with device frame in roll. It also has the same origin as device frame.
+
+
+Example
+------
+To transform global Mesh3D positions and orientations (positions_ecef, quats_ecef) into the local frame described by the
+first position and orientation from Mesh3D one would do:
+```
+ecef_from_local = rot_from_quat(quats_ecef[0])
+local_from_ecef = ecef_from_local.T
+positions_local = np.einsum('ij,kj->ki', local_from_ecef, postions_ecef - positions_ecef[0])
+rotations_global = rot_from_quat(quats_ecef)
+rotations_local = np.einsum('ij,kjl->kil', local_from_ecef, rotations_global)
+eulers_local = euler_from_rot(rotations_local)
+```

iqpilot/common/transformations/SConscript

@@ -0,0 +1,4 @@
+Import('env')
+
+transformations = env.Library('transformations', ['orientation.cc', 'coordinates.cc'])
+Export('transformations')

iqpilot/common/transformations/camera.py

@@ -0,0 +1,179 @@
+import itertools
+import numpy as np
+from dataclasses import dataclass
+
+import iqpilot.common.transformations.orientation as orient
+
+## -- hardcoded hardware params --
+@dataclass(frozen=True)
+class CameraConfig:
+  width: int
+  height: int
+  focal_length: float
+
+  @property
+  def size(self):
+    return (self.width, self.height)
+
+  @property
+  def intrinsics(self):
+    # aka 'K' aka camera_frame_from_view_frame
+    return np.array([
+      [self.focal_length,  0.0, float(self.width)/2],
+      [0.0, self.focal_length, float(self.height)/2],
+      [0.0,  0.0, 1.0]
+    ])
+
+  @property
+  def intrinsics_inv(self):
+    # aka 'K_inv' aka view_frame_from_camera_frame
+    return np.linalg.inv(self.intrinsics)
+
+@dataclass(frozen=True)
+class _NoneCameraConfig(CameraConfig):
+  width: int = 0
+  height: int = 0
+  focal_length: float = 0
+
+@dataclass(frozen=True)
+class DeviceCameraConfig:
+  fcam: CameraConfig
+  dcam: CameraConfig
+  ecam: CameraConfig
+
+  def all_cams(self):
+    for cam in ['fcam', 'dcam', 'ecam']:
+      if not isinstance(getattr(self, cam), _NoneCameraConfig):
+        yield cam, getattr(self, cam)
+
+_ar_ox_fisheye = CameraConfig(1928, 1208, 567.0)  # focal length probably wrong? magnification is not consistent across frame
+_os_fisheye = CameraConfig(2688 // 2, 1520 // 2, 567.0 / 4 * 3)
+_ar_ox_config = DeviceCameraConfig(CameraConfig(1928, 1208, 2648.0), _ar_ox_fisheye, _ar_ox_fisheye)
+_os_config = DeviceCameraConfig(CameraConfig(2688 // 2, 1520 // 2, 1522.0 * 3 / 4), _os_fisheye, _os_fisheye)
+_neo_config = DeviceCameraConfig(CameraConfig(1164, 874, 910.0), CameraConfig(816, 612, 650.0), _NoneCameraConfig())
+
+DEVICE_CAMERAS = {
+  # A "device camera" is defined by a device type and sensor
+
+  # sensor type was never set on eon/neo/two
+  ("neo", "unknown"): _neo_config,
+  # unknown here is AR0231, field was added with OX03C10 support
+  ("tici", "unknown"): _ar_ox_config,
+
+  # before deviceState.deviceType was set, assume tici AR config
+  ("unknown", "ar0231"): _ar_ox_config,
+  ("unknown", "ox03c10"): _ar_ox_config,
+
+  # simulator (emulates a tici)
+  ("pc", "unknown"): _ar_ox_config,
+}
+prods = itertools.product(('tici', 'tizi', 'mici'), (('ar0231', _ar_ox_config), ('ox03c10', _ar_ox_config), ('os04c10', _os_config)))
+DEVICE_CAMERAS.update({(d, c[0]): c[1] for d, c in prods})
+
+# device/mesh : x->forward, y-> right, z->down
+# view : x->right, y->down, z->forward
+device_frame_from_view_frame = np.array([
+  [ 0.,  0.,  1.],
+  [ 1.,  0.,  0.],
+  [ 0.,  1.,  0.]
+])
+view_frame_from_device_frame = device_frame_from_view_frame.T
+
+
+# aka 'extrinsic_matrix'
+# road : x->forward, y -> left, z->up
+def get_view_frame_from_road_frame(roll, pitch, yaw, height):
+  device_from_road = orient.rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1]))
+  view_from_road = view_frame_from_device_frame.dot(device_from_road)
+  return np.hstack((view_from_road, [[0], [height], [0]]))
+
+
+
+# aka 'extrinsic_matrix'
+def get_view_frame_from_calib_frame(roll, pitch, yaw, height):
+  device_from_calib= orient.rot_from_euler([roll, pitch, yaw])
+  view_from_calib = view_frame_from_device_frame.dot(device_from_calib)
+  return np.hstack((view_from_calib, [[0], [height], [0]]))
+
+
+def vp_from_ke(m):
+  """
+  Computes the vanishing point from the product of the intrinsic and extrinsic
+  matrices C = KE.
+
+  The vanishing point is defined as lim x->infinity C (x, 0, 0, 1).T
+  """
+  return (m[0, 0]/m[2, 0], m[1, 0]/m[2, 0])
+
+
+def roll_from_ke(m):
+  # note: different from calibration.h/RollAnglefromKE: i think that one's just wrong
+  return np.arctan2(-(m[1, 0] - m[1, 1] * m[2, 0] / m[2, 1]),
+                    -(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1]))
+
+
+def normalize(img_pts, intrinsics):
+  # normalizes image coordinates
+  # accepts single pt or array of pts
+  intrinsics_inv = np.linalg.inv(intrinsics)
+  img_pts = np.array(img_pts)
+  input_shape = img_pts.shape
+  img_pts = np.atleast_2d(img_pts)
+  img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0], 1))))
+  img_pts_normalized = img_pts.dot(intrinsics_inv.T)
+  img_pts_normalized[(img_pts < 0).any(axis=1)] = np.nan
+  return img_pts_normalized[:, :2].reshape(input_shape)
+
+
+def denormalize(img_pts, intrinsics, width=np.inf, height=np.inf):
+  # denormalizes image coordinates
+  # accepts single pt or array of pts
+  img_pts = np.array(img_pts)
+  input_shape = img_pts.shape
+  img_pts = np.atleast_2d(img_pts)
+  img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0], 1), dtype=img_pts.dtype)))
+  img_pts_denormalized = img_pts.dot(intrinsics.T)
+  if np.isfinite(width):
+    img_pts_denormalized[img_pts_denormalized[:, 0] > width] = np.nan
+    img_pts_denormalized[img_pts_denormalized[:, 0] < 0] = np.nan
+  if np.isfinite(height):
+    img_pts_denormalized[img_pts_denormalized[:, 1] > height] = np.nan
+    img_pts_denormalized[img_pts_denormalized[:, 1] < 0] = np.nan
+  return img_pts_denormalized[:, :2].reshape(input_shape)
+
+
+def get_calib_from_vp(vp, intrinsics):
+  vp_norm = normalize(vp, intrinsics)
+  yaw_calib = np.arctan(vp_norm[0])
+  pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))
+  roll_calib = 0
+  return roll_calib, pitch_calib, yaw_calib
+
+
+def device_from_ecef(pos_ecef, orientation_ecef, pt_ecef):
+  # device from ecef frame
+  # device frame is x -> forward, y-> right, z -> down
+  # accepts single pt or array of pts
+  input_shape = pt_ecef.shape
+  pt_ecef = np.atleast_2d(pt_ecef)
+  ecef_from_device_rot = orient.rotations_from_quats(orientation_ecef)
+  device_from_ecef_rot = ecef_from_device_rot.T
+  pt_ecef_rel = pt_ecef - pos_ecef
+  pt_device = np.einsum('jk,ik->ij', device_from_ecef_rot, pt_ecef_rel)
+  return pt_device.reshape(input_shape)
+
+
+def img_from_device(pt_device):
+  # img coordinates from pts in device frame
+  # first transforms to view frame, then to img coords
+  # accepts single pt or array of pts
+  input_shape = pt_device.shape
+  pt_device = np.atleast_2d(pt_device)
+  pt_view = np.einsum('jk,ik->ij', view_frame_from_device_frame, pt_device)
+
+  # This function should never return negative depths
+  pt_view[pt_view[:, 2] < 0] = np.nan
+
+  pt_img = pt_view/pt_view[:, 2:3]
+  return pt_img.reshape(input_shape)[:, :2]
+

iqpilot/common/transformations/coordinates.cc

@@ -0,0 +1,100 @@
+#define _USE_MATH_DEFINES
+
+#include "iqpilot/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);
+}

iqpilot/common/transformations/coordinates.hpp

@@ -0,0 +1,43 @@
+#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);
+};

iqpilot/common/transformations/coordinates.py

@@ -0,0 +1,18 @@
+from iqpilot.common.transformations.orientation import numpy_wrap
+from iqpilot.common.transformations.transformations import (ecef2geodetic_single,
+                                                    geodetic2ecef_single)
+from iqpilot.common.transformations.transformations import LocalCoord as LocalCoord_single
+
+
+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,))
+
+
+geodetic2ecef = numpy_wrap(geodetic2ecef_single, (3,), (3,))
+ecef2geodetic = numpy_wrap(ecef2geodetic_single, (3,), (3,))
+
+geodetic_from_ecef = ecef2geodetic
+ecef_from_geodetic = geodetic2ecef

iqpilot/common/transformations/model.py

@@ -0,0 +1,70 @@
+import numpy as np
+
+from iqpilot.common.transformations.orientation import rot_from_euler
+from iqpilot.common.transformations.camera import get_view_frame_from_calib_frame, view_frame_from_device_frame, _ar_ox_fisheye
+
+# segnet
+SEGNET_SIZE = (512, 384)
+
+# MED model
+MEDMODEL_INPUT_SIZE = (512, 256)
+MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2)
+MEDMODEL_CY = 47.6
+
+medmodel_fl = 910.0
+medmodel_intrinsics = np.array([
+  [medmodel_fl,  0.0,  0.5 * MEDMODEL_INPUT_SIZE[0]],
+  [0.0,  medmodel_fl,                   MEDMODEL_CY],
+  [0.0,  0.0,                                   1.0]])
+
+
+# BIG model
+BIGMODEL_INPUT_SIZE = (1024, 512)
+BIGMODEL_YUV_SIZE = (BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1] * 3 // 2)
+
+bigmodel_fl = 910.0
+bigmodel_intrinsics = np.array([
+  [bigmodel_fl,  0.0,  0.5 * BIGMODEL_INPUT_SIZE[0]],
+  [0.0,  bigmodel_fl,             256 + MEDMODEL_CY],
+  [0.0,  0.0,                                   1.0]])
+
+
+# SBIG model (big model with the size of small model)
+SBIGMODEL_INPUT_SIZE = (512, 256)
+SBIGMODEL_YUV_SIZE = (SBIGMODEL_INPUT_SIZE[0], SBIGMODEL_INPUT_SIZE[1] * 3 // 2)
+
+sbigmodel_fl = 455.0
+sbigmodel_intrinsics = np.array([
+  [sbigmodel_fl,  0.0,  0.5 * SBIGMODEL_INPUT_SIZE[0]],
+  [0.0,  sbigmodel_fl,      0.5 * (256 + MEDMODEL_CY)],
+  [0.0,  0.0,                                     1.0]])
+
+DM_INPUT_SIZE = (1440, 960)
+dmonitoringmodel_fl = _ar_ox_fisheye.focal_length
+dmonitoringmodel_intrinsics = np.array([
+  [dmonitoringmodel_fl,  0.0, DM_INPUT_SIZE[0]/2],
+  [0.0, dmonitoringmodel_fl, DM_INPUT_SIZE[1]/2 - (_ar_ox_fisheye.height - DM_INPUT_SIZE[1])/2],
+  [0.0,  0.0, 1.0]])
+
+bigmodel_frame_from_calib_frame = np.dot(bigmodel_intrinsics,
+  get_view_frame_from_calib_frame(0, 0, 0, 0))
+
+
+sbigmodel_frame_from_calib_frame = np.dot(sbigmodel_intrinsics,
+  get_view_frame_from_calib_frame(0, 0, 0, 0))
+
+medmodel_frame_from_calib_frame = np.dot(medmodel_intrinsics,
+  get_view_frame_from_calib_frame(0, 0, 0, 0))
+
+medmodel_frame_from_bigmodel_frame = np.dot(medmodel_intrinsics, np.linalg.inv(bigmodel_intrinsics))
+
+calib_from_medmodel = np.linalg.inv(medmodel_frame_from_calib_frame[:, :3])
+calib_from_sbigmodel = np.linalg.inv(sbigmodel_frame_from_calib_frame[:, :3])
+
+# This function is verified to give similar results to xx.uncommon.utils.transform_img
+def get_warp_matrix(device_from_calib_euler: np.ndarray, intrinsics: np.ndarray, bigmodel_frame: bool = False) -> np.ndarray:
+  calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel
+  device_from_calib = rot_from_euler(device_from_calib_euler)
+  camera_from_calib = intrinsics @ view_frame_from_device_frame @ device_from_calib
+  warp_matrix: np.ndarray = camera_from_calib @ calib_from_model
+  return warp_matrix

iqpilot/common/transformations/orientation.cc

@@ -0,0 +1,143 @@
+#define _USE_MATH_DEFINES
+
+#include <iostream>
+#include <cmath>
+#include <eigen3/Eigen/Dense>
+
+#include "iqpilot/common/transformations/orientation.hpp"
+#include "iqpilot/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};
+}

iqpilot/common/transformations/orientation.hpp

@@ -0,0 +1,17 @@
+#pragma once
+#include <eigen3/Eigen/Dense>
+#include "iqpilot/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);

iqpilot/common/transformations/orientation.py

@@ -0,0 +1,52 @@
+import numpy as np
+from collections.abc import Callable
+
+from iqpilot.common.transformations.transformations import (ecef_euler_from_ned_single,
+                                                    euler2quat_single,
+                                                    euler2rot_single,
+                                                    ned_euler_from_ecef_single,
+                                                    quat2euler_single,
+                                                    quat2rot_single,
+                                                    rot2euler_single,
+                                                    rot2quat_single)
+
+
+def numpy_wrap(function, input_shape, output_shape) -> Callable[..., np.ndarray]:
+  """Wrap a function to take either an input or list of inputs and return the correct shape"""
+  def f(*inps):
+    *args, inp = inps
+    inp = np.array(inp)
+    shape = inp.shape
+
+    if len(shape) == len(input_shape):
+      out_shape = output_shape
+    else:
+      out_shape = (shape[0],) + output_shape
+
+    # Add empty dimension if inputs is not a list
+    if len(shape) == len(input_shape):
+      inp.shape = (1, ) + inp.shape
+
+    result = np.asarray([function(*args, i) for i in inp])
+    result.shape = out_shape
+    return result
+  return f
+
+
+euler2quat = numpy_wrap(euler2quat_single, (3,), (4,))
+quat2euler = numpy_wrap(quat2euler_single, (4,), (3,))
+quat2rot = numpy_wrap(quat2rot_single, (4,), (3, 3))
+rot2quat = numpy_wrap(rot2quat_single, (3, 3), (4,))
+euler2rot = numpy_wrap(euler2rot_single, (3,), (3, 3))
+rot2euler = numpy_wrap(rot2euler_single, (3, 3), (3,))
+ecef_euler_from_ned = numpy_wrap(ecef_euler_from_ned_single, (3,), (3,))
+ned_euler_from_ecef = numpy_wrap(ned_euler_from_ecef_single, (3,), (3,))
+
+quats_from_rotations = rot2quat
+quat_from_rot = rot2quat
+rotations_from_quats = quat2rot
+rot_from_quat = quat2rot
+euler_from_rot = rot2euler
+euler_from_quat = quat2euler
+rot_from_euler = euler2rot
+quat_from_euler = euler2quat

iqpilot/common/transformations/tests/test_coordinates.py

@@ -0,0 +1,137 @@
+import numpy as np
+
+import iqpilot.common.transformations.coordinates as coord
+
+geodetic_positions = np.array([[37.7610403, -122.4778699, 115],
+                                 [27.4840915, -68.5867592, 2380],
+                                 [32.4916858, -113.652821, -6],
+                                 [15.1392514, 103.6976037, 24],
+                                 [24.2302229, 44.2835412, 1650]])
+
+ecef_positions = np.array([[-2711076.55270557, -4259167.14692758,  3884579.87669935],
+                          [ 2068042.69652729, -5273435.40316622,  2927004.89190746],
+                          [-2160412.60461669, -4932588.89873832,  3406542.29652851],
+                          [-1458247.92550567,  5983060.87496612,  1654984.6099885 ],
+                          [ 4167239.10867871,  4064301.90363223,  2602234.6065749 ]])
+
+ecef_positions_offset = np.array([[-2711004.46961115, -4259099.33540613,  3884605.16002147],
+                                  [ 2068074.30639499, -5273413.78835412,  2927012.48741131],
+                                  [-2160344.53748176, -4932586.20092211,  3406636.2962545 ],
+                                  [-1458211.98517094,  5983151.11161276,  1655077.02698447],
+                                  [ 4167271.20055269,  4064398.22619263,  2602238.95265847]])
+
+
+ned_offsets = np.array([[78.722153649976391, 24.396208657446344, 60.343017506838436],
+                       [10.699003365155221, 37.319278617604269, 4.1084100025050407],
+                       [95.282646251726959, 61.266689955574428, -25.376506058505054],
+                       [68.535769283630003, -56.285970011848889, -100.54840137956515],
+                       [-33.066609321880179, 46.549821994306861, -84.062540548335591]])
+
+ecef_init_batch = np.array([2068042.69652729, -5273435.40316622,  2927004.89190746])
+ecef_positions_offset_batch = np.array([[ 2068089.41454771, -5273434.46829148,  2927074.04783672],
+                                        [ 2068103.31628647, -5273393.92275431,  2927102.08725987],
+                                        [ 2068108.49939636, -5273359.27047121,  2927045.07091581],
+                                        [ 2068075.12395611, -5273381.69432566,  2927041.08207992],
+                                        [ 2068060.72033399, -5273430.6061505,  2927094.54928305]])
+
+ned_offsets_batch = np.array([[  53.88103168,   43.83445935,  -46.27488057],
+                              [  93.83378995,   71.57943024,  -30.23113187],
+                              [  57.26725796,   89.05602684,   23.02265814],
+                              [  49.71775195,   49.79767572,   17.15351015],
+                              [  78.56272609,   18.53100158,  -43.25290759]])
+
+
+class TestNED:
+  def test_small_distances(self):
+    start_geodetic = np.array([33.8042184, -117.888593, 0.0])
+    local_coord = coord.LocalCoord.from_geodetic(start_geodetic)
+
+    start_ned = local_coord.geodetic2ned(start_geodetic)
+    np.testing.assert_array_equal(start_ned, np.zeros(3,))
+
+    west_geodetic = start_geodetic + [0, -0.0005, 0]
+    west_ned = local_coord.geodetic2ned(west_geodetic)
+    assert np.abs(west_ned[0]) < 1e-3
+    assert west_ned[1] < 0
+
+    southwest_geodetic = start_geodetic + [-0.0005, -0.002, 0]
+    southwest_ned = local_coord.geodetic2ned(southwest_geodetic)
+    assert southwest_ned[0] < 0
+    assert southwest_ned[1] < 0
+
+  def test_ecef_geodetic(self):
+    # testing single
+    np.testing.assert_allclose(ecef_positions[0], coord.geodetic2ecef(geodetic_positions[0]), rtol=1e-9)
+    np.testing.assert_allclose(geodetic_positions[0, :2], coord.ecef2geodetic(ecef_positions[0])[:2], rtol=1e-9)
+    np.testing.assert_allclose(geodetic_positions[0, 2], coord.ecef2geodetic(ecef_positions[0])[2], rtol=1e-9, atol=1e-4)
+
+    np.testing.assert_allclose(geodetic_positions[:, :2], coord.ecef2geodetic(ecef_positions)[:, :2], rtol=1e-9)
+    np.testing.assert_allclose(geodetic_positions[:, 2], coord.ecef2geodetic(ecef_positions)[:, 2], rtol=1e-9, atol=1e-4)
+    np.testing.assert_allclose(ecef_positions, coord.geodetic2ecef(geodetic_positions), rtol=1e-9)
+
+
+  def test_ned(self):
+    for ecef_pos in ecef_positions:
+      converter = coord.LocalCoord.from_ecef(ecef_pos)
+      ecef_pos_moved = ecef_pos + [25, -25, 25]
+      ecef_pos_moved_double_converted = converter.ned2ecef(converter.ecef2ned(ecef_pos_moved))
+      np.testing.assert_allclose(ecef_pos_moved, ecef_pos_moved_double_converted, rtol=1e-9)
+
+    for geo_pos in geodetic_positions:
+      converter = coord.LocalCoord.from_geodetic(geo_pos)
+      geo_pos_moved = geo_pos + np.array([0, 0, 10])
+      geo_pos_double_converted_moved = converter.ned2geodetic(converter.geodetic2ned(geo_pos) + np.array([0, 0, -10]))
+      np.testing.assert_allclose(geo_pos_moved[:2], geo_pos_double_converted_moved[:2], rtol=1e-9, atol=1e-6)
+      np.testing.assert_allclose(geo_pos_moved[2], geo_pos_double_converted_moved[2], rtol=1e-9, atol=1e-4)
+
+  def test_ned_saved_results(self):
+    for i, ecef_pos in enumerate(ecef_positions):
+      converter = coord.LocalCoord.from_ecef(ecef_pos)
+      np.testing.assert_allclose(converter.ned2ecef(ned_offsets[i]),
+                                 ecef_positions_offset[i],
+                                 rtol=1e-9, atol=1e-4)
+      np.testing.assert_allclose(converter.ecef2ned(ecef_positions_offset[i]),
+                                 ned_offsets[i],
+                                 rtol=1e-9, atol=1e-4)
+
+  def test_ned_batch(self):
+    converter = coord.LocalCoord.from_ecef(ecef_init_batch)
+    np.testing.assert_allclose(converter.ecef2ned(ecef_positions_offset_batch),
+                                                           ned_offsets_batch,
+                                                           rtol=1e-9, atol=1e-7)
+    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)

iqpilot/common/transformations/tests/test_orientation.py

@@ -0,0 +1,91 @@
+import numpy as np
+import pytest
+
+from iqpilot.common.transformations.orientation import euler2quat, quat2euler, euler2rot, rot2euler, \
+                                               rot2quat, quat2rot, \
+                                               ned_euler_from_ecef
+
+eulers = np.array([[ 1.46520501,  2.78688383,  2.92780854],
+       [ 4.86909526,  3.60618161,  4.30648981],
+       [ 3.72175965,  2.68763705,  5.43895988],
+       [ 5.92306687,  5.69573614,  0.81100357],
+       [ 0.67838374,  5.02402037,  2.47106426]])
+
+quats = np.array([[ 0.66855182, -0.71500939,  0.19539353,  0.06017818],
+       [ 0.43163717,  0.70013301,  0.28209145,  0.49389021],
+       [ 0.44121991, -0.08252646,  0.34257534,  0.82532207],
+       [ 0.88578382, -0.04515356, -0.32936046,  0.32383617],
+       [ 0.06578165,  0.61282835,  0.07126891,  0.78424163]])
+
+ecef_positions = np.array([[-2711076.55270557, -4259167.14692758,  3884579.87669935],
+                          [ 2068042.69652729, -5273435.40316622,  2927004.89190746],
+                          [-2160412.60461669, -4932588.89873832,  3406542.29652851],
+                          [-1458247.92550567,  5983060.87496612,  1654984.6099885 ],
+                          [ 4167239.10867871,  4064301.90363223,  2602234.6065749 ]])
+
+ned_eulers = np.array([[ 0.46806039, -0.4881889 ,  1.65697808],
+                    [-2.14525969, -0.36533066,  0.73813479],
+                    [-1.39523364, -0.58540761, -1.77376356],
+                    [-1.84220435,  0.61828016, -1.03310421],
+                    [ 2.50450101,  0.36304151,  0.33136365]])
+
+
+class TestOrientation:
+  def test_quat_euler(self):
+    for i, eul in enumerate(eulers):
+      np.testing.assert_allclose(quats[i], euler2quat(eul), rtol=1e-7)
+      np.testing.assert_allclose(quats[i], euler2quat(quat2euler(quats[i])), rtol=1e-6)
+    for i, eul in enumerate(eulers):
+      np.testing.assert_allclose(quats[i], euler2quat(list(eul)), rtol=1e-7)
+      np.testing.assert_allclose(quats[i], euler2quat(quat2euler(list(quats[i]))), rtol=1e-6)
+    np.testing.assert_allclose(quats, euler2quat(eulers), rtol=1e-7)
+    np.testing.assert_allclose(quats, euler2quat(quat2euler(quats)), rtol=1e-6)
+
+  def test_rot_euler(self):
+    for eul in eulers:
+      np.testing.assert_allclose(euler2quat(eul), euler2quat(rot2euler(euler2rot(eul))), rtol=1e-7)
+    for eul in eulers:
+      np.testing.assert_allclose(euler2quat(eul), euler2quat(rot2euler(euler2rot(list(eul)))), rtol=1e-7)
+    np.testing.assert_allclose(euler2quat(eulers), euler2quat(rot2euler(euler2rot(eulers))), rtol=1e-7)
+
+  def test_rot_quat(self):
+    for quat in quats:
+      np.testing.assert_allclose(quat, rot2quat(quat2rot(quat)), rtol=1e-7)
+    for quat in quats:
+      np.testing.assert_allclose(quat, rot2quat(quat2rot(list(quat))), rtol=1e-7)
+    np.testing.assert_allclose(quats, rot2quat(quat2rot(quats)), rtol=1e-7)
+
+  def test_euler_ned(self):
+    for i in range(len(eulers)):
+      np.testing.assert_allclose(ned_eulers[i], ned_euler_from_ecef(ecef_positions[i], eulers[i]), rtol=1e-7)
+      #np.testing.assert_allclose(eulers[i], ecef_euler_from_ned(ecef_positions[i], ned_eulers[i]), rtol=1e-7)
+    # 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)

iqpilot/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])