Write orientation & transform in C++ (commaai#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
old-commit-hash: c18e7da

.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":

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