GeodeticCoords.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 /*
29  * The methods in this file were modified from the originals of the MRPT toolkit (see notice below):
30  * https://github.com/MRPT/mrpt/blob/master/libs/topography/src/conversions.cpp
31  */
32 
33 /* +---------------------------------------------------------------------------+
34  | Mobile Robot Programming Toolkit (MRPT) |
35  | http://www.mrpt.org/ |
36  | |
37  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
38  | See: http://www.mrpt.org/Authors - All rights reserved. |
39  | Released under BSD License. See details in http://www.mrpt.org/License |
40  +---------------------------------------------------------------------------+ */
41 
42 
44 
45 #include <cmath>
46 #ifndef M_PI
47 #define M_PI 3.14159265358979323846
48 #endif
49 
50 namespace rtabmap {
51 
52 
53 inline double DEG2RAD(const double x) { return x*M_PI/180.0;}
54 inline double RAD2DEG(const double x) { return x*180.0/M_PI;}
55 inline double square(const double & value) {return value*value;}
56 
58  latitude_(0.0),
59  longitude_(0.0),
60  altitude_(0.0)
61 {
62 
63 }
65  latitude_(latitude),
66  longitude_(longitude),
67  altitude_(altitude)
68 {
69 
70 }
71 
72 //*---------------------------------------------------------------
73 // geodeticToGeocentric_WGS84
74 // ---------------------------------------------------------------*/
76 {
77  // --------------------------------------------------------------------
78  // See: http://en.wikipedia.org/wiki/Reference_ellipsoid
79  // Constants are for WGS84
80  // --------------------------------------------------------------------
81 
82  static const double a = 6378137; // Semi-major axis of the Earth (meters)
83  static const double b = 6356752.3142; // Semi-minor axis:
84 
85  static const double ae = acos(b/a); // eccentricity:
86  static const double cos2_ae_earth = square(cos(ae)); // The cos^2 of the angular eccentricity of the Earth: // 0.993305619995739L;
87  static const double sin2_ae_earth = square(sin(ae)); // The sin^2 of the angular eccentricity of the Earth: // 0.006694380004261L;
88 
89  const double lon = DEG2RAD( double(this->longitude()) );
90  const double lat = DEG2RAD( double(this->latitude()) );
91 
92  // The radius of curvature in the prime vertical:
93  const double N = a / std::sqrt( 1.0 - sin2_ae_earth*square( sin(lat) ) );
94 
95  // Generate 3D point:
96  cv::Point3d out;
97  out.x = (N+this->altitude())*cos(lat)*cos(lon);
98  out.y = (N+this->altitude())*cos(lat)*sin(lon);
99  out.z = (cos2_ae_earth*N+this->altitude())*sin(lat);
100 
101  return out;
102 }
103 
104 
105 /*---------------------------------------------------------------
106  geodeticToENU_WGS84
107  ---------------------------------------------------------------*/
108 cv::Point3d GeodeticCoords::toENU_WGS84(const GeodeticCoords &origin) const
109 {
110  // Generate 3D point:
111  cv::Point3d P_geocentric = this->toGeocentric_WGS84();
112 
113  // Generate reference 3D point:
114  cv::Point3d P_geocentric_ref = origin.toGeocentric_WGS84();
115 
116  return Geocentric_WGS84ToENU_WGS84(P_geocentric, P_geocentric_ref, origin);
117 }
118 
120  const cv::Point3d & geocentric_WGS84,
121  const cv::Point3d & origin_geocentric_WGS84,
122  const GeodeticCoords & origin)
123 {
124  // --------------------------------------------------------------------
125  // Explanation: We compute the earth-centric coordinates first,
126  // then make a system transformation to local XYZ coordinates
127  // using a system of three orthogonal vectors as local reference.
128  //
129  // See: http://en.wikipedia.org/wiki/Reference_ellipsoid
130  // (JLBC 21/DEC/2006) (Fixed: JLBC 9/JUL/2008)
131  // - Oct/2013, Emilio Sanjurjo: Fixed UP vector pointing exactly normal to ellipsoid surface.
132  // --------------------------------------------------------------------
133 
134  const double clat = cos(DEG2RAD(origin.latitude())), slat = sin(DEG2RAD(origin.latitude()));
135  const double clon = cos(DEG2RAD(origin.longitude())), slon = sin(DEG2RAD(origin.longitude()));
136 
137  // Compute the resulting relative coordinates:
138  // For using smaller numbers:
139  cv::Point3d geocentric_WGS84_rel = geocentric_WGS84-origin_geocentric_WGS84;
140 
141  // Optimized calculation: Local transformed coordinates of P_geo(x,y,z)
142  // after rotation given by the transposed rotation matrix from ENU -> ECEF.
143  cv::Point3d out;
144  out.x = -slon*geocentric_WGS84_rel.x + clon*geocentric_WGS84_rel.y;
145  out.y = -clon*slat*geocentric_WGS84_rel.x -slon*slat*geocentric_WGS84_rel.y + clat*geocentric_WGS84_rel.z;
146  out.z = clon*clat*geocentric_WGS84_rel.x + slon*clat*geocentric_WGS84_rel.y +slat*geocentric_WGS84_rel.z;
147 
148  return out;
149 }
150 
151 void GeodeticCoords::fromGeocentric_WGS84(const cv::Point3d& geocentric)
152 {
153  static const double a = 6378137; // Semi-major axis of the Earth (meters)
154  static const double b = 6356752.3142; // Semi-minor axis:
155 
156  const double sa2 = a*a;
157  const double sb2 = b*b;
158 
159  const double e2 = (sa2 - sb2) / sa2;
160  const double ep2 = (sa2 - sb2) / sb2;
161  const double p = std::sqrt(geocentric.x * geocentric.x + geocentric.y * geocentric.y);
162  const double theta = atan2(geocentric.z * a, p * b);
163 
164  longitude_ = atan2(geocentric.y, geocentric.x);
165  latitude_ = atan2(
166  geocentric.z + ep2 * b * sin(theta) * sin(theta) * sin(theta),
167  p - e2 * a * cos(theta) * cos(theta) * cos(theta));
168 
169  const double clat = cos(latitude_);
170  const double slat = sin(latitude_);
171  const double N = sa2 / std::sqrt(sa2 * clat * clat + sb2 * slat * slat);
172 
173  altitude_ = p / clat - N;
176 }
177 
178 void GeodeticCoords::fromENU_WGS84(const cv::Point3d& enu, const GeodeticCoords& origin)
179 {
181 }
182 
183 cv::Point3d GeodeticCoords::ENU_WGS84ToGeocentric_WGS84(const cv::Point3d& enu, const GeodeticCoords& origin)
184 {
185  // Generate reference 3D point:
186  cv::Point3f originGeocentric;
187  originGeocentric = origin.toGeocentric_WGS84();
188 
189  cv::Vec3d P_ref(originGeocentric.x, originGeocentric.y, originGeocentric.z);
190 
191  // Z axis -> In direction out-ward the center of the Earth:
192  cv::Vec3d REF_X, REF_Y, REF_Z;
193  REF_Z = cv::normalize(P_ref);
194 
195  // 1st column: Starting at the reference point, move in the tangent
196  // direction
197  // east-ward: I compute this as the derivative of P_ref wrt "longitude":
198  // A_east[0] =-(N+in_height_meters)*cos(lat)*sin(lon); --> -Z[1]
199  // A_east[1] = (N+in_height_meters)*cos(lat)*cos(lon); --> Z[0]
200  // A_east[2] = 0; --> 0
201  // ---------------------------------------------------------------------------
202  cv::Vec3d AUX_X(-REF_Z[1], REF_Z[0], 0);
203  REF_X = cv::normalize(AUX_X);
204 
205  // 2nd column: The cross product:
206  REF_Y = REF_Z.cross(REF_X);
207 
208  cv::Point3d out_coords;
209  out_coords.x =
210  REF_X[0] * enu.x + REF_Y[0] * enu.y + REF_Z[0] * enu.z + originGeocentric.x;
211  out_coords.y =
212  REF_X[1] * enu.x + REF_Y[1] * enu.y + REF_Z[1] * enu.z + originGeocentric.y;
213  out_coords.z =
214  REF_X[2] * enu.x + REF_Y[2] * enu.y + REF_Z[2] * enu.z + originGeocentric.z;
215 
216  return out_coords;
217 }
218 
219 }
const double & latitude() const
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
double DEG2RAD(const double x)
double square(const double &value)
static cv::Point3d ENU_WGS84ToGeocentric_WGS84(const cv::Point3d &enu, const GeodeticCoords &origin)
void fromGeocentric_WGS84(const cv::Point3d &geocentric)
GLM_FUNC_DECL genType cos(genType const &angle)
GLM_FUNC_DECL genType normalize(genType const &x)
GLM_FUNC_DECL genType sin(genType const &angle)
cv::Point3d toENU_WGS84(const GeodeticCoords &origin) const
const double & altitude() const
static cv::Point3d Geocentric_WGS84ToENU_WGS84(const cv::Point3d &geocentric_WGS84, const cv::Point3d &origin_geocentric_WGS84, const GeodeticCoords &origin)
#define M_PI
double RAD2DEG(const double x)
GLM_FUNC_QUALIFIER T atan2(T x, T y)
Arc tangent. Returns an angle whose tangent is y/x. The signs of x and y are used to determine what q...
GLM_FUNC_DECL genType acos(genType const &x)
const double & longitude() const
cv::Point3d toGeocentric_WGS84() const
void fromENU_WGS84(const cv::Point3d &enu, const GeodeticCoords &origin)


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:28