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

.editorconfig

@@ -5,7 +5,7 @@ end_of_line = lf
 insert_final_newline = true
 trim_trailing_whitespace = true
 
-[{*.py, *.pyx, *pxd}]
+[{*.py, *.pyx, *.pxd}]
 charset = utf-8
 indent_style = space
 indent_size = 2

.gitignore

@@ -62,3 +62,4 @@ htmlcov
 pandaextra
 
 .mypy_cache/
+flycheck_*

SConstruct

@@ -212,6 +212,7 @@ SConscript(['opendbc/can/SConscript'])
 
 SConscript(['common/SConscript'])
 SConscript(['common/kalman/SConscript'])
+SConscript(['common/transformations/SConscript'])
 SConscript(['phonelibs/SConscript'])
 
 if arch != "Darwin":

cereal

@@ -1 +1 @@
-Subproject commit 1aaf1bfd7c07e1c5184e8f13823e8ed02e2c7af2
+Subproject commit 0021fa241994cd9d94945ba305b0f3f1c43feaae

common/transformations/.gitignore

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

common/transformations/SConscript

@@ -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')

common/transformations/coordinates.cc

@@ -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);
+}

common/transformations/coordinates.hpp

@@ -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);
+};

common/transformations/coordinates.py

@@ -1,113 +1,19 @@
-"""
+# pylint: skip-file
-Coordinate transformation module. All methods accept arrays as input
+from common.transformations.orientation import numpy_wrap
-with each row as a position.
+from common.transformations.transformations import (ecef2geodetic_single,
-"""
+                                                    geodetic2ecef_single)
-import numpy as np
+from common.transformations.transformations import LocalCoord as LocalCoord_single
 
 
-a = 6378137
+class LocalCoord(LocalCoord_single):
-b = 6356752.3142
+  ecef2ned = numpy_wrap(LocalCoord_single.ecef2ned_single, (3,), (3,))
-esq = 6.69437999014 * 0.001
+  ned2ecef = numpy_wrap(LocalCoord_single.ned2ecef_single, (3,), (3,))
-e1sq = 6.73949674228 * 0.001
+  geodetic2ned = numpy_wrap(LocalCoord_single.geodetic2ned_single, (3,), (3,))
+  ned2geodetic = numpy_wrap(LocalCoord_single.ned2geodetic_single, (3,), (3,))
 
 
-def geodetic2ecef(geodetic, radians=False):
+geodetic2ecef = numpy_wrap(geodetic2ecef_single, (3,), (3,))
-  geodetic = np.array(geodetic)
+ecef2geodetic = numpy_wrap(ecef2geodetic_single, (3,), (3,))
-  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)
-
 
 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)

common/transformations/orientation.cc

@@ -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){
+}

common/transformations/orientation.hpp

@@ -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);

common/transformations/orientation.py

@@ -1,125 +1,47 @@
-'''
+# pylint: skip-file
-Vectorized functions that transform between
-rotation matrices, euler angles and quaternions.
-All support lists, array or array of arrays as inputs.
-Supports both x2y and y_from_x format (y_from_x preferred!).
-'''
-
 import numpy as np
-from numpy import dot, inner, array, linalg
-from common.transformations.coordinates import LocalCoord
-
-
-def euler2quat(eulers):
-  eulers = array(eulers)
-  if len(eulers.shape) > 1:
-    output_shape = (-1, 4)
-  else:
-    output_shape = (4,)
-  eulers = np.atleast_2d(eulers)
-  gamma, theta, psi = eulers[:, 0],  eulers[:, 1],  eulers[:, 2]
-
-  q0 = np.cos(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) + \
-       np.sin(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
-  q1 = np.sin(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) - \
-       np.cos(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
-  q2 = np.cos(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2) + \
-       np.sin(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2)
-  q3 = np.cos(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2) - \
-       np.sin(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2)
-
-  quats = array([q0, q1, q2, q3]).T
-  for i in range(len(quats)):
-    if quats[i, 0] < 0:
-      quats[i] = -quats[i]
-  return quats.reshape(output_shape)
-
 
-def quat2euler(quats):
+from common.transformations.transformations import (ecef_euler_from_ned_single,
-  quats = array(quats)
+                                                    euler2quat_single,
-  if len(quats.shape) > 1:
+                                                    euler2rot_single,
-    output_shape = (-1, 3)
+                                                    ned_euler_from_ecef_single,
+                                                    quat2euler_single,
+                                                    quat2rot_single,
+                                                    rot2euler_single,
+                                                    rot2quat_single,
+                                                    rot_matrix)
+
+
+def numpy_wrap(function, input_shape, output_shape):
+  """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:
-    output_shape = (3,)
+      out_shape = (shape[0],) + output_shape
-  quats = np.atleast_2d(quats)
-  q0, q1, q2, q3 = quats[:, 0], quats[:, 1], quats[:, 2], quats[:, 3]
 
-  gamma = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2))
+    # Add empty dimension if inputs is not a list
-  theta = np.arcsin(2 * (q0 * q2 - q3 * q1))
+    if len(shape) == len(input_shape):
-  psi = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2))
+      inp.shape = (1, ) + inp.shape
 
-  eulers = array([gamma, theta, psi]).T
+    result = np.asarray([function(*args, i) for i in inp])
-  return eulers.reshape(output_shape)
+    result.shape = out_shape
+    return result
+  return f
 
 
-def quat2rot(quats):
+euler2quat = numpy_wrap(euler2quat_single, (3,), (4,))
-  quats = array(quats)
+quat2euler = numpy_wrap(quat2euler_single, (4,), (3,))
-  input_shape = quats.shape
+quat2rot = numpy_wrap(quat2rot_single, (4,), (3, 3))
-  quats = np.atleast_2d(quats)
+rot2quat = numpy_wrap(rot2quat_single, (3, 3), (4,))
-  Rs = np.zeros((quats.shape[0], 3, 3))
+euler2rot = numpy_wrap(euler2rot_single, (3,), (3, 3))
-  q0 = quats[:, 0]
+rot2euler = numpy_wrap(rot2euler_single, (3, 3), (3,))
-  q1 = quats[:, 1]
+ecef_euler_from_ned = numpy_wrap(ecef_euler_from_ned_single, (3,), (3,))
-  q2 = quats[:, 2]
+ned_euler_from_ecef = numpy_wrap(ned_euler_from_ecef_single, (3,), (3,))
-  q3 = quats[:, 3]
-  Rs[:, 0, 0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3
-  Rs[:, 0, 1] = 2 * (q1 * q2 - q0 * q3)
-  Rs[:, 0, 2] = 2 * (q0 * q2 + q1 * q3)
-  Rs[:, 1, 0] = 2 * (q1 * q2 + q0 * q3)
-  Rs[:, 1, 1] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3
-  Rs[:, 1, 2] = 2 * (q2 * q3 - q0 * q1)
-  Rs[:, 2, 0] = 2 * (q1 * q3 - q0 * q2)
-  Rs[:, 2, 1] = 2 * (q0 * q1 + q2 * q3)
-  Rs[:, 2, 2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3
-
-  if len(input_shape) < 2:
-    return Rs[0]
-  else:
-    return Rs
-
-
-def rot2quat(rots):
-  input_shape = rots.shape
-  if len(input_shape) < 3:
-    rots = array([rots])
-  K3 = np.empty((len(rots), 4, 4))
-  K3[:, 0, 0] = (rots[:, 0, 0] - rots[:, 1, 1] - rots[:, 2, 2]) / 3.0
-  K3[:, 0, 1] = (rots[:, 1, 0] + rots[:, 0, 1]) / 3.0
-  K3[:, 0, 2] = (rots[:, 2, 0] + rots[:, 0, 2]) / 3.0
-  K3[:, 0, 3] = (rots[:, 1, 2] - rots[:, 2, 1]) / 3.0
-  K3[:, 1, 0] = K3[:, 0, 1]
-  K3[:, 1, 1] = (rots[:, 1, 1] - rots[:, 0, 0] - rots[:, 2, 2]) / 3.0
-  K3[:, 1, 2] = (rots[:, 2, 1] + rots[:, 1, 2]) / 3.0
-  K3[:, 1, 3] = (rots[:, 2, 0] - rots[:, 0, 2]) / 3.0
-  K3[:, 2, 0] = K3[:, 0, 2]
-  K3[:, 2, 1] = K3[:, 1, 2]
-  K3[:, 2, 2] = (rots[:, 2, 2] - rots[:, 0, 0] - rots[:, 1, 1]) / 3.0
-  K3[:, 2, 3] = (rots[:, 0, 1] - rots[:, 1, 0]) / 3.0
-  K3[:, 3, 0] = K3[:, 0, 3]
-  K3[:, 3, 1] = K3[:, 1, 3]
-  K3[:, 3, 2] = K3[:, 2, 3]
-  K3[:, 3, 3] = (rots[:, 0, 0] + rots[:, 1, 1] + rots[:, 2, 2]) / 3.0
-  q = np.empty((len(rots), 4))
-  for i in range(len(rots)):
-    _, eigvecs = linalg.eigh(K3[i].T)
-    eigvecs = eigvecs[:, 3:]
-    q[i, 0] = eigvecs[-1]
-    q[i, 1:] = -eigvecs[:-1].flatten()
-    if q[i, 0] < 0:
-      q[i] = -q[i]
-
-  if len(input_shape) < 3:
-    return q[0]
-  else:
-    return q
-
-
-def euler2rot(eulers):
-  return rotations_from_quats(euler2quat(eulers))
-
-
-def rot2euler(rots):
-  return quat2euler(quats_from_rotations(rots))
-
 
 quats_from_rotations = rot2quat
 quat_from_rot = rot2quat
@@ -130,162 +52,3 @@ euler_from_rot = rot2euler
 euler_from_quat = quat2euler
 rot_from_euler = euler2rot
 quat_from_euler = euler2quat
-
-
-'''
-Random helpers below
-'''
-
-
-def quat_product(q, r):
-  t = np.zeros(4)
-  t[0] = r[0] * q[0] - r[1] * q[1] - r[2] * q[2] - r[3] * q[3]
-  t[1] = r[0] * q[1] + r[1] * q[0] - r[2] * q[3] + r[3] * q[2]
-  t[2] = r[0] * q[2] + r[1] * q[3] + r[2] * q[0] - r[3] * q[1]
-  t[3] = r[0] * q[3] - r[1] * q[2] + r[2] * q[1] + r[3] * q[0]
-  return t
-
-
-def rot_matrix(roll, pitch, yaw):
-  cr, sr = np.cos(roll), np.sin(roll)
-  cp, sp = np.cos(pitch), np.sin(pitch)
-  cy, sy = np.cos(yaw), np.sin(yaw)
-  rr = array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]])
-  rp = array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]])
-  ry = array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]])
-  return ry.dot(rp.dot(rr))
-
-
-def rot(axis, angle):
-  # Rotates around an arbitrary axis
-  ret_1 = (1 - np.cos(angle)) * array([[axis[0]**2, axis[0] * axis[1], axis[0] * axis[2]], [
-    axis[1] * axis[0], axis[1]**2, axis[1] * axis[2]
-  ], [axis[2] * axis[0], axis[2] * axis[1], axis[2]**2]])
-  ret_2 = np.cos(angle) * np.eye(3)
-  ret_3 = np.sin(angle) * array([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]],
-                              [-axis[1], axis[0], 0]])
-  return ret_1 + ret_2 + ret_3
-
-
-def ecef_euler_from_ned(ned_ecef_init, ned_pose):
-  '''
-  Got it from here:
-  Using Rotations to Build Aerospace Coordinate Systems
-  -Don Koks
-  '''
-  converter = LocalCoord.from_ecef(ned_ecef_init)
-  x0 = converter.ned2ecef([1, 0, 0]) - converter.ned2ecef([0, 0, 0])
-  y0 = converter.ned2ecef([0, 1, 0]) - converter.ned2ecef([0, 0, 0])
-  z0 = converter.ned2ecef([0, 0, 1]) - converter.ned2ecef([0, 0, 0])
-
-  x1 = rot(z0, ned_pose[2]).dot(x0)
-  y1 = rot(z0, ned_pose[2]).dot(y0)
-  z1 = rot(z0, ned_pose[2]).dot(z0)
-
-  x2 = rot(y1, ned_pose[1]).dot(x1)
-  y2 = rot(y1, ned_pose[1]).dot(y1)
-  z2 = rot(y1, ned_pose[1]).dot(z1)
-
-  x3 = rot(x2, ned_pose[0]).dot(x2)
-  y3 = rot(x2, ned_pose[0]).dot(y2)
-  #z3 = rot(x2, ned_pose[0]).dot(z2)
-
-  x0 = array([1, 0, 0])
-  y0 = array([0, 1, 0])
-  z0 = array([0, 0, 1])
-
-  psi = np.arctan2(inner(x3, y0), inner(x3, x0))
-  theta = np.arctan2(-inner(x3, z0), np.sqrt(inner(x3, x0)**2 + inner(x3, y0)**2))
-  y2 = rot(z0, psi).dot(y0)
-  z2 = rot(y2, theta).dot(z0)
-  phi = np.arctan2(inner(y3, z2), inner(y3, y2))
-
-  ret = array([phi, theta, psi])
-  return ret
-
-
-def ned_euler_from_ecef(ned_ecef_init, ecef_poses):
-  '''
-  Got the math from here:
-  Using Rotations to Build Aerospace Coordinate Systems
-  -Don Koks
-
-  Also accepts array of ecef_poses and array of ned_ecef_inits.
-  Where each row is a pose and an ecef_init.
-  '''
-  ned_ecef_init = array(ned_ecef_init)
-  ecef_poses = array(ecef_poses)
-  output_shape = ecef_poses.shape
-  ned_ecef_init = np.atleast_2d(ned_ecef_init)
-  if ned_ecef_init.shape[0] == 1:
-    ned_ecef_init = np.tile(ned_ecef_init[0], (output_shape[0], 1))
-  ecef_poses = np.atleast_2d(ecef_poses)
-
-  ned_poses = np.zeros(ecef_poses.shape)
-  for i, ecef_pose in enumerate(ecef_poses):
-    converter = LocalCoord.from_ecef(ned_ecef_init[i])
-    x0 = array([1, 0, 0])
-    y0 = array([0, 1, 0])
-    z0 = array([0, 0, 1])
-
-    x1 = rot(z0, ecef_pose[2]).dot(x0)
-    y1 = rot(z0, ecef_pose[2]).dot(y0)
-    z1 = rot(z0, ecef_pose[2]).dot(z0)
-
-    x2 = rot(y1, ecef_pose[1]).dot(x1)
-    y2 = rot(y1, ecef_pose[1]).dot(y1)
-    z2 = rot(y1, ecef_pose[1]).dot(z1)
-
-    x3 = rot(x2, ecef_pose[0]).dot(x2)
-    y3 = rot(x2, ecef_pose[0]).dot(y2)
-    #z3 = rot(x2, ecef_pose[0]).dot(z2)
-
-    x0 = converter.ned2ecef([1, 0, 0]) - converter.ned2ecef([0, 0, 0])
-    y0 = converter.ned2ecef([0, 1, 0]) - converter.ned2ecef([0, 0, 0])
-    z0 = converter.ned2ecef([0, 0, 1]) - converter.ned2ecef([0, 0, 0])
-
-    psi = np.arctan2(inner(x3, y0), inner(x3, x0))
-    theta = np.arctan2(-inner(x3, z0), np.sqrt(inner(x3, x0)**2 + inner(x3, y0)**2))
-    y2 = rot(z0, psi).dot(y0)
-    z2 = rot(y2, theta).dot(z0)
-    phi = np.arctan2(inner(y3, z2), inner(y3, y2))
-    ned_poses[i] = array([phi, theta, psi])
-
-  return ned_poses.reshape(output_shape)
-
-
-def ecef2car(car_ecef, psi, theta, points_ecef, ned_converter):
-  """
-  TODO: add roll rotation
-  Converts an array of points in ecef coordinates into
-  x-forward, y-left, z-up coordinates
-  Parameters
-  ----------
-  psi: yaw, radian
-  theta: pitch, radian
-  Returns
-  -------
-  [x, y, z] coordinates in car frame
-  """
-
-  # input is an array of points in ecef cocrdinates
-  # output is an array of points in car's coordinate (x-front, y-left, z-up)
-
-  # convert points to NED
-  points_ned = []
-  for p in points_ecef:
-    points_ned.append(ned_converter.ecef2ned_matrix.dot(array(p) - car_ecef))
-
-  points_ned = np.vstack(points_ned).T
-
-  # n, e, d -> x, y, z
-  # Calculate relative postions and rotate wrt to heading and pitch of car
-  invert_R = array([[1., 0., 0.], [0., -1., 0.], [0., 0., -1.]])
-
-  c, s = np.cos(psi), np.sin(psi)
-  yaw_R = array([[c, s, 0.], [-s, c, 0.], [0., 0., 1.]])
-
-  c, s = np.cos(theta), np.sin(theta)
-  pitch_R = array([[c, 0., -s], [0., 1., 0.], [s, 0., c]])
-
-  return dot(pitch_R, dot(yaw_R, dot(invert_R, points_ned)))

common/transformations/setup.py

@@ -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()],
+  )
+))

common/transformations/tests/test_coordinates.py

@@ -11,12 +11,6 @@ geodetic_positions = np.array([[37.7610403, -122.4778699, 115],
                                  [15.1392514, 103.6976037, 24],
                                  [24.2302229, 44.2835412, 1650]])
 
-geodetic_positions_radians = np.array([[0.65905448, -2.13764209, 115],
-                                       [0.47968789, -1.19706477, 2380],
-                                       [0.5670869, -1.98361593, -6],
-                                       [0.26422978, 1.80986461, 24],
-                                       [0.42289717, 0.7728936, 1650]])
-
 ecef_positions = np.array([[-2711076.55270557, -4259167.14692758,  3884579.87669935],
                           [ 2068042.69652729, -5273435.40316622,  2927004.89190746],
                           [-2160412.60461669, -4932588.89873832,  3406542.29652851],
@@ -78,9 +72,6 @@ class TestNED(unittest.TestCase):
     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)
 
-    np.testing.assert_allclose(geodetic_positions_radians[0], coord.ecef2geodetic(ecef_positions[0], radians=True), rtol=1e-5)
-    np.testing.assert_allclose(geodetic_positions_radians[:, :2], coord.ecef2geodetic(ecef_positions, radians=True)[:, :2], rtol=1e-7)
-    np.testing.assert_allclose(geodetic_positions_radians[:, 2], coord.ecef2geodetic(ecef_positions, radians=True)[:, 2], rtol=1e-7, atol=1e-4)
 
   def test_ned(self):
     for ecef_pos in ecef_positions:

common/transformations/tests/test_orientation.py

@@ -61,7 +61,7 @@ class TestOrientation(unittest.TestCase):
     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)
+    # np.testing.assert_allclose(ned_eulers, ned_euler_from_ecef(ecef_positions, eulers), rtol=1e-7)
 
 
 if __name__ == "__main__":

common/transformations/transformations.pxd

@@ -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

common/transformations/transformations.pyx

@@ -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

release/files_common

@@ -43,9 +43,18 @@ common/kalman/*
 
 common/transformations/__init__.py
 common/transformations/camera.py
-common/transformations/coordinates.py
 common/transformations/model.py
+
+common/transformations/SConscript
+common/transformations/setup.py
+common/transformations/coordinates.py
+common/transformations/coordinates.cc
+common/transformations/coordinates.hpp
 common/transformations/orientation.py
+common/transformations/orientation.cc
+common/transformations/orientation.hpp
+common/transformations/transformations.pxd
+common/transformations/transformations.pyx
 
 common/api/__init__.py
 

selfdrive/locationd/locationd.py

@@ -21,7 +21,6 @@ from sympy.utilities.lambdify import lambdify
 from rednose.helpers.sympy_helpers import euler_rotate
 
 
-OUTPUT_DECIMATION = 2
 VISION_DECIMATION = 2
 SENSOR_DECIMATION = 10
 
@@ -194,7 +193,7 @@ class Localizer():
 
     self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude])
     ecef_pos = self.converter.ned2ecef([0, 0, 0])
-    ecef_vel = self.converter.ned2ecef_matrix.dot(np.array(log.vNED))
+    ecef_vel = self.converter.ned2ecef(np.array(log.vNED)) - ecef_pos
     ecef_pos_R = np.diag([(3*log.verticalAccuracy)**2]*3)
     ecef_vel_R = np.diag([(log.speedAccuracy)**2]*3)
 
@@ -263,6 +262,7 @@ class Localizer():
           self.update_kalman(current_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]])
 
   def handle_live_calib(self, current_time, log):
+    if len(log.rpyCalib):
       self.calib = log.rpyCalib
       self.device_from_calib = rot_from_euler(self.calib)
       self.calib_from_device = self.device_from_calib.T
@@ -270,7 +270,7 @@ class Localizer():
 
   def reset_kalman(self, current_time=None, init_orient=None):
     self.filter_time = current_time
-    init_x = LiveKalman.initial_x
+    init_x = LiveKalman.initial_x.copy()
     # too nonlinear to init on completely wrong
     if init_orient is not None:
       init_x[3:7] = init_orient
@@ -295,7 +295,6 @@ def locationd_thread(sm, pm, disabled_logs=None):
     pm = messaging.PubMaster(['liveLocationKalman'])
 
   localizer = Localizer(disabled_logs=disabled_logs)
-  camera_odometry_cnt = 0
 
   while True:
     sm.update()
@@ -315,9 +314,6 @@ def locationd_thread(sm, pm, disabled_logs=None):
           localizer.handle_live_calib(t, sm[sock])
 
     if sm.updated['cameraOdometry']:
-      camera_odometry_cnt += 1
-
-      if camera_odometry_cnt % OUTPUT_DECIMATION == 0:
       t = sm.logMonoTime['cameraOdometry']
       msg = messaging.new_message('liveLocationKalman')
       msg.logMonoTime = t

selfdrive/test/process_replay/compare_logs.py

@@ -70,7 +70,7 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None):
     if msg1_bytes != msg2_bytes:
       msg1_dict = msg1.to_dict(verbose=True)
       msg2_dict = msg2.to_dict(verbose=True)
-      dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields, tolerance=0)
+      dd = dictdiffer.diff(msg1_dict, msg2_dict, ignore=ignore_fields)
       diff.extend(dd)
   return diff
 

selfdrive/test/process_replay/process_replay.py

@@ -66,7 +66,11 @@ class FakeSocket:
 class DumbSocket:
   def __init__(self, s=None):
     if s is not None:
+      try:
         dat = messaging.new_message(s)
+      except capnp.lib.capnp.KjException:  # pylint: disable=c-extension-no-member
+        # lists
+        dat = messaging.new_message(s, 0)
       self.data = dat.to_bytes()
 
   def receive(self, non_blocking=False):
@@ -255,6 +259,17 @@ CONFIGS = [
     init_callback=get_car_params,
     should_recv_callback=None,
   ),
+  ProcessConfig(
+    proc_name="locationd",
+    pub_sub={
+      "cameraOdometry": ["liveLocationKalman"],
+      "sensorEvents": [], "gpsLocationExternal": [], "liveCalibration": [], "carState": [],
+    },
+    ignore=["logMonoTime", "valid"],
+    init_callback=get_car_params,
+    should_recv_callback=None,
+  ),
+
 ]
 
 def replay_process(cfg, lr):

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-0533f640ab27f7b5af57aa4ebf4a29200550b3e8
+834f4cd7e90ff266ced8ea142d7d7d05076186aa