transformations: standardize parameter passing by reference (commaai#…

…33469)

pass parameters by reference

common/transformations/coordinates.cc

@@ -25,16 +25,16 @@ static Geodetic to_radians(Geodetic geodetic){
 }
 
 
-ECEF geodetic2ecef(Geodetic g){
-  g = to_radians(g);
+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(ECEF e){
+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;
@@ -61,10 +61,10 @@ Geodetic ecef2geodetic(ECEF e){
   return to_degrees({lat, lon, h});
 }
 
-LocalCoord::LocalCoord(Geodetic g, ECEF e){
+LocalCoord::LocalCoord(const Geodetic &geodetic, const ECEF &e) {
   init_ecef <<  e.x, e.y, e.z;
 
-  g = to_radians(g);
+  auto g = to_radians(geodetic);
 
   ned2ecef_matrix <<
     -sin(g.lat)*cos(g.lon), -sin(g.lon), -cos(g.lat)*cos(g.lon),
@@ -73,28 +73,28 @@ LocalCoord::LocalCoord(Geodetic g, ECEF e){
   ecef2ned_matrix = ned2ecef_matrix.transpose();
 }
 
-NED LocalCoord::ecef2ned(ECEF e) {
+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(NED n) {
+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(Geodetic g) {
+NED LocalCoord::geodetic2ned(const Geodetic &g) {
   ECEF e = ::geodetic2ecef(g);
   return ecef2ned(e);
 }
 
-Geodetic LocalCoord::ned2geodetic(NED n){
+Geodetic LocalCoord::ned2geodetic(const NED &n) {
   ECEF e = ned2ecef(n);
   return ::ecef2geodetic(e);
 }

common/transformations/coordinates.hpp

@@ -7,14 +7,14 @@
 
 struct ECEF {
   double x, y, z;
-  Eigen::Vector3d to_vector(){
+  Eigen::Vector3d to_vector() const {
     return Eigen::Vector3d(x, y, z);
   }
 };
 
 struct NED {
   double n, e, d;
-  Eigen::Vector3d to_vector(){
+  Eigen::Vector3d to_vector() const {
     return Eigen::Vector3d(n, e, d);
   }
 };
@@ -24,20 +24,20 @@ struct Geodetic {
   bool radians=false;
 };
 
-ECEF geodetic2ecef(Geodetic g);
-Geodetic ecef2geodetic(ECEF e);
+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(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);
+  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);
 };

common/transformations/orientation.cc

@@ -7,15 +7,15 @@
 #include "common/transformations/orientation.hpp"
 #include "common/transformations/coordinates.hpp"
 
-Eigen::Quaterniond ensure_unique(Eigen::Quaterniond quat){
+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(Eigen::Vector3d euler){
+Eigen::Quaterniond euler2quat(const Eigen::Vector3d &euler) {
   Eigen::Quaterniond q;
 
   q = Eigen::AngleAxisd(euler(2), Eigen::Vector3d::UnitZ())
@@ -25,7 +25,7 @@ Eigen::Quaterniond euler2quat(Eigen::Vector3d euler){
 }
 
 
-Eigen::Vector3d quat2euler(Eigen::Quaterniond quat){
+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)};
@@ -36,34 +36,34 @@ Eigen::Vector3d quat2euler(Eigen::Quaterniond quat){
   return {gamma, theta, psi};
 }
 
-Eigen::Matrix3d quat2rot(Eigen::Quaterniond quat){
+Eigen::Matrix3d quat2rot(const Eigen::Quaterniond &quat) {
   return quat.toRotationMatrix();
 }
 
-Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot){
+Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot) {
   return ensure_unique(Eigen::Quaterniond(rot));
 }
 
-Eigen::Matrix3d euler2rot(Eigen::Vector3d euler){
+Eigen::Matrix3d euler2rot(const Eigen::Vector3d &euler) {
   return quat2rot(euler2quat(euler));
 }
 
-Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot){
+Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot) {
   return quat2euler(rot2quat(rot));
 }
 
-Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw){
+Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw) {
   return euler2rot({roll, pitch, yaw});
 }
 
-Eigen::Matrix3d rot(Eigen::Vector3d axis, double angle){
+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(ECEF ecef_init, Eigen::Vector3d ned_pose) {
+Eigen::Vector3d ecef_euler_from_ned(const ECEF &ecef_init, const Eigen::Vector3d &ned_pose) {
   /*
     Using Rotations to Build Aerospace Coordinate Systems
     Don Koks
@@ -103,7 +103,7 @@ Eigen::Vector3d ecef_euler_from_ned(ECEF ecef_init, Eigen::Vector3d ned_pose) {
   return {phi, theta, psi};
 }
 
-Eigen::Vector3d ned_euler_from_ecef(ECEF ecef_init, Eigen::Vector3d ecef_pose){
+Eigen::Vector3d ned_euler_from_ecef(const ECEF &ecef_init, const Eigen::Vector3d &ecef_pose) {
   /*
     Using Rotations to Build Aerospace Coordinate Systems
     Don Koks

common/transformations/orientation.hpp

@@ -3,15 +3,15 @@
 #include "common/transformations/coordinates.hpp"
 
 
-Eigen::Quaterniond ensure_unique(Eigen::Quaterniond quat);
+Eigen::Quaterniond ensure_unique(const Eigen::Quaterniond &quat);
 
-Eigen::Quaterniond euler2quat(Eigen::Vector3d euler);
-Eigen::Vector3d quat2euler(Eigen::Quaterniond quat);
-Eigen::Matrix3d quat2rot(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(Eigen::Vector3d euler);
+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(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);
+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);

common/transformations/transformations.pxd

@@ -24,15 +24,15 @@ cdef extern from "orientation.hpp":
 
     double operator()(int, int)
 
-  Quaternion euler2quat(Vector3)
-  Vector3 quat2euler(Quaternion)
-  Matrix3 quat2rot(Quaternion)
-  Quaternion rot2quat(Matrix3)
-  Vector3 rot2euler(Matrix3)
-  Matrix3 euler2rot(Vector3)
+  Quaternion euler2quat(const Vector3 &)
+  Vector3 quat2euler(const Quaternion &)
+  Matrix3 quat2rot(const Quaternion &)
+  Quaternion rot2quat(const Matrix3 &)
+  Vector3 rot2euler(const Matrix3 &)
+  Matrix3 euler2rot(const Vector3 &)
   Matrix3 rot_matrix(double, double, double)
-  Vector3 ecef_euler_from_ned(ECEF, Vector3)
-  Vector3 ned_euler_from_ecef(ECEF, Vector3)
+  Vector3 ecef_euler_from_ned(const ECEF &, const Vector3 &)
+  Vector3 ned_euler_from_ecef(const ECEF &, const Vector3 &)
 
 
 cdef extern from "coordinates.cc":
@@ -52,21 +52,21 @@ cdef extern from "coordinates.cc":
     double alt
     bool radians
 
-  ECEF geodetic2ecef(Geodetic)
-  Geodetic ecef2geodetic(ECEF)
+  ECEF geodetic2ecef(const Geodetic &)
+  Geodetic ecef2geodetic(const ECEF &)
 
   cdef cppclass LocalCoord_c "LocalCoord":
     Matrix3 ned2ecef_matrix
     Matrix3 ecef2ned_matrix
 
-    LocalCoord_c(Geodetic, ECEF)
-    LocalCoord_c(Geodetic)
-    LocalCoord_c(ECEF)
+    LocalCoord_c(const Geodetic &, const ECEF &)
+    LocalCoord_c(const Geodetic &)
+    LocalCoord_c(const ECEF &)
 
-    NED ecef2ned(ECEF)
-    ECEF ned2ecef(NED)
-    NED geodetic2ned(Geodetic)
-    Geodetic ned2geodetic(NED)
+    NED ecef2ned(const ECEF &)
+    ECEF ned2ecef(const NED &)
+    NED geodetic2ned(const Geodetic &)
+    Geodetic ned2geodetic(const NED &)
 
 cdef extern from "coordinates.hpp":
   pass