Compile FrogPilot

common/transformations/SConscript

@@ -1,5 +0,0 @@
-Import('env', 'envCython')
-
-transformations = env.Library('transformations', ['orientation.cc', 'coordinates.cc'])
-transformations_python = envCython.Program('transformations.so', 'transformations.pyx')
-Export('transformations', 'transformations_python')

common/transformations/coordinates.cc

@@ -1,100 +0,0 @@
-#define _USE_MATH_DEFINES
-
-#include "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(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/orientation.cc

@@ -1,144 +0,0 @@
-#define _USE_MATH_DEFINES
-
-#include <iostream>
-#include <cmath>
-#include <eigen3/Eigen/Dense>
-
-#include "common/transformations/orientation.hpp"
-#include "common/transformations/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 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(Eigen::Quaterniond quat){
-  return quat.toRotationMatrix();
-}
-
-Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot){
-  return ensure_unique(Eigen::Quaterniond(rot));
-}
-
-Eigen::Matrix3d euler2rot(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(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};
-}
-