diff --git a/common/transformations/coordinates.cc b/common/transformations/coordinates.cc index 5b00b53a4f4928..f3f10e547f5105 100644 --- a/common/transformations/coordinates.cc +++ b/common/transformations/coordinates.cc @@ -25,8 +25,8 @@ 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); @@ -34,7 +34,7 @@ ECEF geodetic2ecef(Geodetic g){ 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,7 +73,7 @@ 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; @@ -81,7 +81,7 @@ NED LocalCoord::ecef2ned(ECEF e) { 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; @@ -89,12 +89,12 @@ ECEF LocalCoord::ned2ecef(NED n) { 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); } diff --git a/common/transformations/coordinates.hpp b/common/transformations/coordinates.hpp index 32ec2ff66e3058..dc8ff7a4b6a4f1 100644 --- a/common/transformations/coordinates.hpp +++ b/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); }; diff --git a/common/transformations/orientation.cc b/common/transformations/orientation.cc index 00888c3a926080..fb5e47a86a3ea2 100644 --- a/common/transformations/orientation.cc +++ b/common/transformations/orientation.cc @@ -7,7 +7,7 @@ #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 { @@ -15,7 +15,7 @@ Eigen::Quaterniond ensure_unique(Eigen::Quaterniond quat){ } } -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 diff --git a/common/transformations/orientation.hpp b/common/transformations/orientation.hpp index 150b12cadee1d1..0874a0a814b282 100644 --- a/common/transformations/orientation.hpp +++ b/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); diff --git a/common/transformations/transformations.pxd b/common/transformations/transformations.pxd index 964adf06ece9e8..fe32e18deac88b 100644 --- a/common/transformations/transformations.pxd +++ b/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