mawei
/
openpilot0
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

transformations: standardize parameter passing by reference (#33469) 

pass parameters by reference
This commit is contained in:
Dean Lee 2024-09-08 08:06:57 +08:00 committed by GitHub
parent 606943010e
commit b286a2f0e5
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 56 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
common/transformations/coordinates.cc
View File

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

22
common/transformations/coordinates.hpp
View File

@ -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) {}
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(ECEF e);
ECEF ned2ecef(NED n);
NED geodetic2ned(Geodetic g);
Geodetic ned2geodetic(NED n);
NED ecef2ned(const ECEF &e);
ECEF ned2ecef(const NED &n);
NED geodetic2ned(const Geodetic &g);
Geodetic ned2geodetic(const NED &n);
};

22
common/transformations/orientation.cc
View File

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

16
common/transformations/orientation.hpp
View File

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

34
common/transformations/transformations.pxd
View File

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