Fixposition SDK 0.0.0-heads/main-0-gde8559b
Collection of c++ libraries and apps for use with Fixposition products on Linux
Loading...
Searching...
No Matches
trafo.hpp
Go to the documentation of this file.
1/**
2 * \verbatim
3 * ___ ___
4 * \ \ / /
5 * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors
6 * / /\ \ License: see the LICENSE file
7 * /__/ \__\
8 * \endverbatim
9 *
10 * @file
11 * @brief Fixposition SDK: Transformation utilities
12 *
13 * @page FPSDK_COMMON_TRAFO Transformation utilities
14 *
15 * **API**: fpsdk_common/trafo.hpp and fpsdk::common::trafo
16 *
17 */
18#ifndef __FPSDK_COMMON_TRAFO_HPP__
19#define __FPSDK_COMMON_TRAFO_HPP__
20
21/* LIBC/STL */
22#include <memory>
23#include <string>
24
25/* EXTERNAL */
26#include "fpsdk_common/ext/eigen_core.hpp"
27#include "fpsdk_common/ext/eigen_geometry.hpp"
28
29/* PACKAGE */
30#include "types.hpp"
31
32namespace fpsdk {
33namespace common {
34/**
35 * @brief Transformation utilities
36 */
37namespace trafo {
38/* ****************************************************************************************************************** */
39
40/**
41 * @name WGS-84 constants
42 *
43 * https://en.wikipedia.org/wiki/World_Geodetic_System#Defining_Parameters
44 *
45 * @{
46 */
47static constexpr double WGS84_A = 6378137.0; //!< WGS-84 Earth radius major axis [m]
48static constexpr double WGS84_B = 6356752.314245; //!< WGS-84 Earth radius minor axis [m]
49static constexpr double WGS84_C = 40075016.6855785; //!< Circumference at equator (= 2π * WGS84_A) [m]
50static constexpr double WGS84_1F = 298.257223563; //!< WGS-84 1/f inverse of flattening parameter
51static constexpr double WGS84_E2 = 6.69437999014e-3; //!< WGS-84 first eccentricity squared
52static constexpr double WGS84_A2 = WGS84_A * WGS84_A; //!< WGS-84 a^2
53static constexpr double WGS84_B2 = WGS84_B * WGS84_B; //!< WGS-84 b^2
54static constexpr double WGS84_EE2 = (WGS84_A2 / WGS84_B2) - 1.0; //!< WGS-84 e'^2 second eccentricity squared
55///@}
56
57/**
58 * @brief Calculate the rotation matrix from ECEF to ENU for a given reference latitude/longitude
59 *
60 * @param[in] lat Reference latitude [rad]
61 * @param[in] lon Reference longitude [rad]
62 *
63 * @returns the rotation matrix from ECEF to ENU
64 *
65 * Reference https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates
66 */
67Eigen::Matrix3d RotEnuEcef(const double lat, const double lon);
68
69/**
70 * @brief Calculate the rotation matrix from ECEF to ENU for a given reference position
71 *
72 * @note The reference position is projected to the WGS84 ellipsoid
73 *
74 * @param[in] ecef Reference position in ECEF [m]
75 *
76 * @returns the rotation matrix from ECEF to ENU
77 */
78Eigen::Matrix3d RotEnuEcef(const Eigen::Vector3d& ecef);
79
80/**
81 * @brief Returns rotation matrix between NED and ENU
82 *
83 * | 0, 1, 0 |
84 * | 1, 0, 0 |
85 * | 0, 0, -1 |
86 *
87 * @returns the rotation matrix between NED and ENU
88 */
89Eigen::Matrix3d RotNedEnu();
90
91/**
92 * @brief Calculate the rotation matrix from ECEF to NED for a given reference latitude/longitude
93 *
94 * @param[in] lat Reference latitude [rad]
95 * @param[in] lon Reference longitude [rad]
96 *
97 * @returns the rotation matrix from ECEF to NED
98 */
99Eigen::Matrix3d RotNedEcef(const double lat, const double lon);
100
101/**
102 * @brief Calculate the rotation matrix from ECEF to NED for a given reference origin
103 *
104 * @note The reference position is projected to the WGS84 ellipsoid
105 *
106 * @param[in] ecef Reference position in ECEF [m]
107 *
108 * @returns the rRotation matrix from ECEF to NED
109 */
110Eigen::Matrix3d RotNedEcef(const Eigen::Vector3d& ecef);
111
112/**
113 * @brief Transform ECEF coordinate to ENU with specified ENU-origin
114 *
115 * @param[in] ecef ECEF position to be transformed [m]
116 * @param[in] wgs84llh_ref ENU-origin in geodetic coordinates (lat [rad], lon [rad], height [m])
117 *
118 * @returns the position in ENU coordinates
119 */
120Eigen::Vector3d TfEnuEcef(const Eigen::Vector3d& ecef, const Eigen::Vector3d& wgs84llh_ref);
121
122/**
123 * @brief Transform ENU coordinate to ECEF with specified ENU-origin
124 *
125 * @param[in] enu ENU position to be transformed [m]
126 * @param[in] wgs84llh_ref ENU-origin in geodetic coordinates (lat [rad], lon [rad], height [m])
127 *
128 * @returns @todo documentation
129 */
130Eigen::Vector3d TfEcefEnu(const Eigen::Vector3d& enu, const Eigen::Vector3d& wgs84llh_ref);
131
132/**
133 * @brief Convert ECEF coordinates to ENU with specified ENU origin
134 *
135 * @param[in] ecef ECEF position to be converted
136 * @param[in] wgs84llh_ref ENU-origin input given in geodetic coordinates
137 *
138 * @returns the position in ENU coordinates
139 */
140Eigen::Vector3d TfNedEcef(const Eigen::Vector3d& ecef, const Eigen::Vector3d& wgs84llh_ref);
141
142/**
143 * @todo documentation
144 */
145Eigen::Vector3d TfEcefNed(const Eigen::Vector3d& ned, const Eigen::Vector3d& wgs84llh_ref);
146
147/**
148 * @brief Convert geodetic coordinates (latitude, longitude, height) to ECEF (x, y, z).
149 *
150 * @param[in] wgs84llh Geodetic coordinates (lat [rad], lon [rad], height [m])
151 *
152 * @returns the ECEF coordinates [m]
153 */
154Eigen::Vector3d TfEcefWgs84Llh(const Eigen::Vector3d& wgs84llh);
155
156/**
157 * @brief Convert ECEF (x, y, z) coordinates to geodetic coordinates (latitude, longitude, height)
158 *
159 * @param[in] ecef ECEF coordinates [m]
160 *
161 * @returns the geodetic coordinates (lat [rad], lon [rad], height [m])
162 */
163Eigen::Vector3d TfWgs84LlhEcef(const Eigen::Vector3d& ecef);
164
165/**
166 * @brief Calculate yaw, pitch and roll in ENU from a given pose in ECEF
167 *
168 * Yaw is -Pi/2 when X points North, because ENU starts with East
169 *
170 * @param[in] ecef_p 3D position vector in ECEF
171 * @param[in] ecef_r 3x3 rotation matrix representing rotation from body to ECEF
172 *
173 * @returns yaw, pitch and roll in ENU
174 */
175Eigen::Vector3d EcefPoseToEnuEul(const Eigen::Vector3d& ecef_p, const Eigen::Matrix3d& ecef_r);
176
177/**
178 * @brief Vector4 quaternion to intrinsic Euler angles in ZYX (yaw, pitch, roll)
179 *
180 * @param[in] quat Quaternion (w, i, j, k)
181 *
182 * @returns the intrinsic Euler angles in ZYX (yaw, pitch, roll) order
183 */
184Eigen::Vector3d QuatToEul(const Eigen::Quaterniond& quat);
185
186/**
187 * @brief Rotation Matrix to intrinsic Euler angles in ZYX (yaw, pitch, roll) order in radian
188 *
189 * @param[in] rot Eigen::Matrix3d
190 *
191 * @returns intrinsic euler angle in ZYX (Yaw-Pitch-Roll) order in radian
192 */
193Eigen::Vector3d RotToEul(const Eigen::Matrix3d& rot);
194
195/**
196 * @brief Convert llh from deg to rad, height component unchanged
197 *
198 * @param[in] llh_deg llh in degrees
199 *
200 * @returns llh in radian
201 */
202Eigen::Vector3d LlhDegToRad(const Eigen::Vector3d& llh_deg);
203
204/**
205 * @brief Convert llh from rad to deg, height component unchanged
206 *
207 * @param[in] llh_rad llh in radian
208 *
209 * @returns llh in degrees
210 */
211Eigen::Vector3d LlhRadToDeg(const Eigen::Vector3d& llh_rad);
212
213#if FPSDK_USE_PROJ
214/**
215 * @brief "Universal" coordinate transformer, backed by PROJ
216 */
218{
219 public:
220 /**
221 * @brief Constructor
222 *
223 * @param[in] name Optional name, for debugging
224 */
225 Transformer(const std::string& name = "");
226
227 ~Transformer();
228
229 /**
230 * @brief Initialise transformer
231 *
232 * The source and target CRS specification can be anything that proj_create_crs_to_crs() understands (see
233 * https://proj.org/en/9.4/development/reference/functions.html#c.proj_create_crs_to_crs). You may have to change
234 * the PROJ configuration (proj.ini) and/or update some PROJ data yourself to support some CRS. See the PROJ
235 * documentation for details (see https://proj.org/en/9.4/resource_files.html).
236 *
237 * @param[in] source_crs Source coordinate reference system, e.g. "EPSG:4326"
238 * @param[in] target_crs Target coordinate reference system, e.g. "EPSG:2056"
239 *
240 * @returns true on success, false otherwise (bad params, missing data, ...)
241 */
242 bool Init(const std::string& source_crs, const std::string& target_crs);
243
244 /**
245 * @brief Transform coordinates
246 *
247 * @param[in,out] inout Coordinates to transform, replaced with result
248 * @param[in] inv Do the inverse transformation (true), default is forward (false)
249 *
250 * @returns true on success, false otherwise
251 */
252 bool Transform(Eigen::Vector3d& inout, const bool inv = false);
253
254 /**
255 * @brief Transform coordinates
256 *
257 * @param[in] in Coordinates to transform
258 * @param[out] out Transformed coordinates
259 * @param[in] inv Do the inverse transformation (true), default is forward (false)
260 *
261 * @returns true on success, false otherwise
262 */
263 bool Transform(const Eigen::Vector3d& in, Eigen::Vector3d& out, const bool inv = false);
264
265 /**
266 * @brief Transform coordinates (with time)
267 *
268 * @param[in] in Coordinates to transform (with time)
269 * @param[out] out Transformed coordinates (with time)
270 * @param[in] inv Do the inverse transformation (true), default is forward (false)
271 *
272 * @returns true on success, false otherwise
273 */
274 bool Transform(const Eigen::Vector4d& in, Eigen::Vector4d& out, const bool inv = false);
275
276 private:
277 const std::string name_; //!< Name, for debugging
278 bool pj_init_; //!< Proj is initialised
279 void* pj_ctx_; //!< Proj context
280 void* pj_tf_; //!< Proj transformation object
281};
282#endif // FPSDK_USE_PROJ
283
284/* ****************************************************************************************************************** */
285} // namespace trafo
286} // namespace common
287} // namespace fpsdk
288#endif // __FPSDK_COMMON_TRAFO_HPP__
"Universal" coordinate transformer, backed by PROJ
Definition trafo.hpp:218
Transformer(const std::string &name="")
Constructor.
bool Transform(Eigen::Vector3d &inout, const bool inv=false)
Transform coordinates.
bool Transform(const Eigen::Vector3d &in, Eigen::Vector3d &out, const bool inv=false)
Transform coordinates.
bool Transform(const Eigen::Vector4d &in, Eigen::Vector4d &out, const bool inv=false)
Transform coordinates (with time)
bool Init(const std::string &source_crs, const std::string &target_crs)
Initialise transformer.
Base class to prevent copy or move.
Definition types.hpp:97
Eigen::Matrix3d RotEnuEcef(const double lat, const double lon)
Calculate the rotation matrix from ECEF to ENU for a given reference latitude/longitude.
Eigen::Matrix3d RotNedEcef(const double lat, const double lon)
Calculate the rotation matrix from ECEF to NED for a given reference latitude/longitude.
Eigen::Vector3d LlhDegToRad(const Eigen::Vector3d &llh_deg)
Convert llh from deg to rad, height component unchanged.
Eigen::Vector3d LlhRadToDeg(const Eigen::Vector3d &llh_rad)
Convert llh from rad to deg, height component unchanged.
static constexpr double WGS84_1F
WGS-84 1/f inverse of flattening parameter.
Definition trafo.hpp:50
Eigen::Vector3d TfEcefNed(const Eigen::Vector3d &ned, const Eigen::Vector3d &wgs84llh_ref)
static constexpr double WGS84_B2
WGS-84 b^2.
Definition trafo.hpp:53
Eigen::Vector3d TfNedEcef(const Eigen::Vector3d &ecef, const Eigen::Vector3d &wgs84llh_ref)
Convert ECEF coordinates to ENU with specified ENU origin.
static constexpr double WGS84_E2
WGS-84 first eccentricity squared.
Definition trafo.hpp:51
Eigen::Vector3d QuatToEul(const Eigen::Quaterniond &quat)
Vector4 quaternion to intrinsic Euler angles in ZYX (yaw, pitch, roll)
static constexpr double WGS84_C
Circumference at equator (= 2π * WGS84_A) [m].
Definition trafo.hpp:49
Eigen::Vector3d RotToEul(const Eigen::Matrix3d &rot)
Rotation Matrix to intrinsic Euler angles in ZYX (yaw, pitch, roll) order in radian.
static constexpr double WGS84_B
WGS-84 Earth radius minor axis [m].
Definition trafo.hpp:48
Eigen::Matrix3d RotNedEnu()
Returns rotation matrix between NED and ENU.
Eigen::Vector3d TfWgs84LlhEcef(const Eigen::Vector3d &ecef)
Convert ECEF (x, y, z) coordinates to geodetic coordinates (latitude, longitude, height)
Eigen::Vector3d TfEcefEnu(const Eigen::Vector3d &enu, const Eigen::Vector3d &wgs84llh_ref)
Transform ENU coordinate to ECEF with specified ENU-origin.
static constexpr double WGS84_A
WGS-84 Earth radius major axis [m].
Definition trafo.hpp:47
Eigen::Vector3d EcefPoseToEnuEul(const Eigen::Vector3d &ecef_p, const Eigen::Matrix3d &ecef_r)
Calculate yaw, pitch and roll in ENU from a given pose in ECEF.
Eigen::Vector3d TfEnuEcef(const Eigen::Vector3d &ecef, const Eigen::Vector3d &wgs84llh_ref)
Transform ECEF coordinate to ENU with specified ENU-origin.
Eigen::Vector3d TfEcefWgs84Llh(const Eigen::Vector3d &wgs84llh)
Convert geodetic coordinates (latitude, longitude, height) to ECEF (x, y, z).
static constexpr double WGS84_A2
WGS-84 a^2.
Definition trafo.hpp:52
static constexpr double WGS84_EE2
WGS-84 e'^2 second eccentricity squared.
Definition trafo.hpp:54
Fixposition SDK.
Fixposition SDK: Common types and type helpers.