MRPT  2.0.4
conversions.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "topography-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/geometry.h>
13 #include <mrpt/math/utils.h>
14 #include <mrpt/poses/CPoint3D.h>
15 #include <mrpt/poses/CPose3D.h>
17 
18 using namespace std;
19 using namespace mrpt;
20 using namespace mrpt::math;
21 using namespace mrpt::poses;
22 
24 {
25  return a.decimal_value == o.decimal_value;
26 }
28 {
29  return !(a == o);
30 }
32  const TGeodeticCoords& a, const TGeodeticCoords& o)
33 {
34  return a.lat == o.lat && a.lon == o.lon && a.height == o.height;
35 }
37  const TGeodeticCoords& a, const TGeodeticCoords& o)
38 {
39  return !(a == o);
40 }
41 
42 //#define M_PI_TOPO 3.141592653589793
43 // inline double DEG2RAD(const double x) { return x*M_PI_TOPO/180.0; }
44 
45 // Define a generic type for "precission numbers":
46 #ifdef HAVE_LONG_DOUBLE
47 using precnum_t = long double;
48 #else
49 using precnum_t = double;
50 #endif
51 
52 std::ostream& mrpt::topography::operator<<(std::ostream& out, const TCoords& o)
53 {
54  return out << o.getAsString();
55 }
56 
57 /*---------------------------------------------------------------
58  geocentricToENU_WGS84
59  ---------------------------------------------------------------*/
61  const mrpt::math::TPoint3D& P_geocentric_,
62  mrpt::math::TPoint3D& out_ENU_point,
63  const TGeodeticCoords& in_coords_origin)
64 {
65  // Generate reference 3D point:
66  TPoint3D P_geocentric_ref;
67  geodeticToGeocentric_WGS84(in_coords_origin, P_geocentric_ref);
68 
69  const double clat = cos(DEG2RAD(in_coords_origin.lat)),
70  slat = sin(DEG2RAD(in_coords_origin.lat));
71  const double clon = cos(DEG2RAD(in_coords_origin.lon)),
72  slon = sin(DEG2RAD(in_coords_origin.lon));
73 
74  // Compute the resulting relative coordinates:
75  // For using smaller numbers:
76  const mrpt::math::TPoint3D P_geocentric = P_geocentric_ - P_geocentric_ref;
77 
78  // Optimized calculation: Local transformed coordinates of P_geo(x,y,z)
79  // after rotation given by the transposed rotation matrix from ENU ->
80  // ECEF.
81  out_ENU_point.x = -slon * P_geocentric.x + clon * P_geocentric.y;
82  out_ENU_point.y = -clon * slat * P_geocentric.x -
83  slon * slat * P_geocentric.y + clat * P_geocentric.z;
84  out_ENU_point.z = clon * clat * P_geocentric.x +
85  slon * clat * P_geocentric.y + slat * P_geocentric.z;
86 }
87 
89  const std::vector<mrpt::math::TPoint3D>& in_geocentric_points,
90  std::vector<mrpt::math::TPoint3D>& out_ENU_points,
91  const TGeodeticCoords& in_coords_origin)
92 {
93  // Generate reference 3D point:
94  TPoint3D P_geocentric_ref;
95  geodeticToGeocentric_WGS84(in_coords_origin, P_geocentric_ref);
96 
97  const double clat = cos(DEG2RAD(in_coords_origin.lat)),
98  slat = sin(DEG2RAD(in_coords_origin.lat));
99  const double clon = cos(DEG2RAD(in_coords_origin.lon)),
100  slon = sin(DEG2RAD(in_coords_origin.lon));
101 
102  const size_t N = in_geocentric_points.size();
103  out_ENU_points.resize(N);
104  for (size_t i = 0; i < N; i++)
105  {
106  // Compute the resulting relative coordinates for using smaller numbers:
107  const mrpt::math::TPoint3D P_geocentric =
108  in_geocentric_points[i] - P_geocentric_ref;
109 
110  // Optimized calculation: Local transformed coordinates of P_geo(x,y,z)
111  // after rotation given by the transposed rotation matrix from ENU ->
112  // ECEF.
113  out_ENU_points[i].x = -slon * P_geocentric.x + clon * P_geocentric.y;
114  out_ENU_points[i].y = -clon * slat * P_geocentric.x -
115  slon * slat * P_geocentric.y +
116  clat * P_geocentric.z;
117  out_ENU_points[i].z = clon * clat * P_geocentric.x +
118  slon * clat * P_geocentric.y +
119  slat * P_geocentric.z;
120  }
121 }
122 
123 /*---------------------------------------------------------------
124  geodeticToENU_WGS84
125  ---------------------------------------------------------------*/
127  const TGeodeticCoords& in_coords, mrpt::math::TPoint3D& out_ENU_point,
128  const TGeodeticCoords& in_coords_origin)
129 {
130  // --------------------------------------------------------------------
131  // Explanation: We compute the earth-centric coordinates first,
132  // then make a system transformation to local XYZ coordinates
133  // using a system of three orthogonal vectors as local reference.
134  //
135  // See: http://en.wikipedia.org/wiki/Reference_ellipsoid
136  // (JLBC 21/DEC/2006) (Fixed: JLBC 9/JUL/2008)
137  // - Oct/2013, Emilio Sanjurjo: Fixed UP vector pointing exactly normal to
138  // ellipsoid surface.
139  // --------------------------------------------------------------------
140  // Generate 3D point:
141  TPoint3D P_geocentric;
142  geodeticToGeocentric_WGS84(in_coords, P_geocentric);
143 
144  geocentricToENU_WGS84(P_geocentric, out_ENU_point, in_coords_origin);
145 }
146 
147 /*---------------------------------------------------------------
148  ENU_axes_from_WGS84
149  ---------------------------------------------------------------*/
151  double in_longitude_reference_degrees, double in_latitude_reference_degrees,
152  double in_height_reference_meters, mrpt::math::TPose3D& out_ENU,
153  bool only_angles)
154 {
155  // See "coordinatesTransformation_WGS84" for more comments.
156  TPoint3D PPref;
159  in_latitude_reference_degrees, in_longitude_reference_degrees,
160  in_height_reference_meters),
161  PPref);
162 
163  const double clat = cos(DEG2RAD(in_latitude_reference_degrees)),
164  slat = sin(DEG2RAD(in_latitude_reference_degrees));
165  const double clon = cos(DEG2RAD(in_longitude_reference_degrees)),
166  slon = sin(DEG2RAD(in_longitude_reference_degrees));
167 
168  CMatrixDouble44 HM; // zeros by default
169  HM(0, 0) = -slon;
170  HM(0, 1) = -clon * slat;
171  HM(0, 2) = clon * clat;
172  HM(1, 0) = clon;
173  HM(1, 1) = -slon * slat;
174  HM(1, 2) = slon * clat;
175  HM(2, 0) = 0;
176  HM(2, 1) = clat;
177  HM(2, 2) = slat;
178  HM(3, 3) = 1;
179 
180  if (!only_angles)
181  {
182  HM(0, 3) = PPref.x;
183  HM(1, 3) = PPref.y;
184  HM(2, 3) = PPref.z;
185  }
186 
187  out_ENU = CPose3D(HM).asTPose();
188 }
189 
190 //*---------------------------------------------------------------
191 // geodeticToGeocentric_WGS84
192 // ---------------------------------------------------------------*/
194  const TGeodeticCoords& in_coords, mrpt::math::TPoint3D& out_point)
195 {
196  // --------------------------------------------------------------------
197  // See: http://en.wikipedia.org/wiki/Reference_ellipsoid
198  // Constants are for WGS84
199  // --------------------------------------------------------------------
200 
201  static const precnum_t a =
202  6378137L; // Semi-major axis of the Earth (meters)
203  static const precnum_t b = 6356752.3142L; // Semi-minor axis:
204 
205  static const precnum_t ae = acos(b / a); // eccentricity:
206  static const precnum_t cos2_ae_earth =
207  square(cos(ae)); // The cos^2 of the angular eccentricity of the Earth:
208  // // 0.993305619995739L;
209  static const precnum_t sin2_ae_earth =
210  square(sin(ae)); // The sin^2 of the angular eccentricity of the Earth:
211  // // 0.006694380004261L;
212 
213  const precnum_t lon = DEG2RAD(precnum_t(in_coords.lon));
214  const precnum_t lat = DEG2RAD(precnum_t(in_coords.lat));
215 
216  // The radius of curvature in the prime vertical:
217  const precnum_t N = a / std::sqrt(1 - sin2_ae_earth * square(sin(lat)));
218 
219  // Generate 3D point:
220  out_point.x =
221  static_cast<double>((N + in_coords.height) * cos(lat) * cos(lon));
222  out_point.y =
223  static_cast<double>((N + in_coords.height) * cos(lat) * sin(lon));
224  out_point.z =
225  static_cast<double>((cos2_ae_earth * N + in_coords.height) * sin(lat));
226 }
227 
228 /*---------------------------------------------------------------
229  geodeticToGeocentric_WGS84
230  ---------------------------------------------------------------*/
232  const TGeodeticCoords& in_coords, TGeocentricCoords& out_point,
233  const TEllipsoid& ellip)
234 {
235  static const precnum_t a =
236  ellip.sa; // Semi-major axis of the Earth (meters)
237  static const precnum_t b = ellip.sb; // Semi-minor axis:
238 
239  static const precnum_t ae = acos(b / a); // eccentricity:
240  static const precnum_t cos2_ae_earth =
241  square(cos(ae)); // The cos^2 of the angular eccentricity of the Earth:
242  // // 0.993305619995739L;
243  static const precnum_t sin2_ae_earth =
244  square(sin(ae)); // The sin^2 of the angular eccentricity of the Earth:
245  // // 0.006694380004261L;
246 
247  const precnum_t lon = DEG2RAD(precnum_t(in_coords.lon));
248  const precnum_t lat = DEG2RAD(precnum_t(in_coords.lat));
249 
250  // The radius of curvature in the prime vertical:
251  const precnum_t N = a / std::sqrt(1 - sin2_ae_earth * square(sin(lat)));
252 
253  // Generate 3D point:
254  out_point.x =
255  static_cast<double>((N + in_coords.height) * cos(lat) * cos(lon));
256  out_point.y =
257  static_cast<double>((N + in_coords.height) * cos(lat) * sin(lon));
258  out_point.z =
259  static_cast<double>((cos2_ae_earth * N + in_coords.height) * sin(lat));
260 }
261 
262 /*---------------------------------------------------------------
263  geocentricToGeodetic
264  ---------------------------------------------------------------*/
266  const TGeocentricCoords& in_point, TGeodeticCoords& out_coords,
267  const TEllipsoid& ellip)
268 {
269  const double sa2 = ellip.sa * ellip.sa;
270  const double sb2 = ellip.sb * ellip.sb;
271 
272  const double e2 = (sa2 - sb2) / sa2;
273  const double ep2 = (sa2 - sb2) / sb2;
274  const double p = sqrt(in_point.x * in_point.x + in_point.y * in_point.y);
275  const double theta = atan2(in_point.z * ellip.sa, p * ellip.sb);
276 
277  out_coords.lon = atan2(in_point.y, in_point.x);
278  out_coords.lat = atan2(
279  in_point.z + ep2 * ellip.sb * sin(theta) * sin(theta) * sin(theta),
280  p - e2 * ellip.sa * cos(theta) * cos(theta) * cos(theta));
281 
282  const double clat = cos(out_coords.lat);
283  const double slat = sin(out_coords.lat);
284  const double N = sa2 / sqrt(sa2 * clat * clat + sb2 * slat * slat);
285 
286  out_coords.height = p / clat - N;
287 
288  out_coords.lon = RAD2DEG(out_coords.lon);
289  out_coords.lat = RAD2DEG(out_coords.lat);
290 }
291 
292 /*---------------------------------------------------------------
293  UTMToGeodesic
294  ---------------------------------------------------------------*/
296  double X, double Y, int huso, char hem, double& out_lon /*degrees*/,
297  double& out_lat /*degrees*/, const TEllipsoid& ellip)
298 {
299  ASSERT_(hem == 's' || hem == 'S' || hem == 'n' || hem == 'N');
300 
301  X = X - 5e5;
302  if (hem == 's' || hem == 'S') Y = Y - 1e7;
303 
304  const precnum_t lon0 = huso * 6 - 183;
305  const precnum_t a2 = ellip.sa * ellip.sa;
306  const precnum_t b2 = ellip.sb * ellip.sb;
307  const precnum_t ep2 = (a2 - b2) / b2;
308  const precnum_t c = a2 / ellip.sb;
309  const precnum_t latp = Y / (6366197.724 * 0.9996);
310  const precnum_t clp2 = square(cos(latp));
311 
312  const precnum_t v = c * 0.9996 / sqrt(1 + ep2 * clp2);
313  const precnum_t na = X / v;
314  const precnum_t A1 = sin(2 * latp);
315  const precnum_t A2 = A1 * clp2;
316  const precnum_t J2 = latp + A1 * 0.5;
317  const precnum_t J4 = 0.75 * J2 + 0.25 * A2;
318  const precnum_t J6 = (5 * J4 + A2 * clp2) / 3;
319 
320  const precnum_t alp = 0.75 * ep2;
321  const precnum_t beta = (5.0 / 3.0) * alp * alp;
322  const precnum_t gam = (35.0 / 27.0) * alp * alp * alp;
323  const precnum_t B = 0.9996 * c * (latp - alp * J2 + beta * J4 - gam * J6);
324  const precnum_t nb = (Y - B) / v;
325  const precnum_t psi = (ep2 * square(na)) / 2 * clp2;
326  const precnum_t eps = na * (1 - psi / 3);
327  const precnum_t nu = nb * (1 - psi) + latp;
328  const precnum_t she = (exp(eps) - exp(-eps)) / 2;
329  const precnum_t dlon = atan2(she, cos(nu));
330  const precnum_t tau = atan2(cos(dlon) * tan(nu), 1);
331 
332  out_lon = static_cast<double>(RAD2DEG(dlon) + lon0);
333  out_lat = static_cast<double>(RAD2DEG(
334  latp +
335  (1 + ep2 * clp2 - 1.5 * ep2 * sin(latp) * cos(latp) * (tau - latp)) *
336  (tau - latp)));
337 }
338 
340  const TGeodeticCoords& GeodeticCoords, TUTMCoords& UTMCoords, int& UTMZone,
341  char& UTMLatitudeBand, const TEllipsoid& ellip)
342 {
343  const double la = GeodeticCoords.lat;
344  char Letra;
345  if (la < -72)
346  Letra = 'C';
347  else if (la < -64)
348  Letra = 'D';
349  else if (la < -56)
350  Letra = 'E';
351  else if (la < -48)
352  Letra = 'F';
353  else if (la < -40)
354  Letra = 'G';
355  else if (la < -32)
356  Letra = 'H';
357  else if (la < -24)
358  Letra = 'J';
359  else if (la < -16)
360  Letra = 'K';
361  else if (la < -8)
362  Letra = 'L';
363  else if (la < 0)
364  Letra = 'M';
365  else if (la < 8)
366  Letra = 'N';
367  else if (la < 16)
368  Letra = 'P';
369  else if (la < 24)
370  Letra = 'Q';
371  else if (la < 32)
372  Letra = 'R';
373  else if (la < 40)
374  Letra = 'S';
375  else if (la < 48)
376  Letra = 'T';
377  else if (la < 56)
378  Letra = 'U';
379  else if (la < 64)
380  Letra = 'V';
381  else if (la < 72)
382  Letra = 'W';
383  else
384  Letra = 'X';
385 
386  const precnum_t lat = DEG2RAD(GeodeticCoords.lat);
387  const precnum_t lon = DEG2RAD(GeodeticCoords.lon);
388  const int Huso = mrpt::fix((GeodeticCoords.lon / 6) + 31);
389  const precnum_t lon0 = DEG2RAD(Huso * 6 - 183);
390 
391  const precnum_t sa = ellip.sa;
392  const precnum_t sb = ellip.sb;
393  // const precnum_t e2 = (sa*sa-sb*sb)/(sa*sa);
394  const precnum_t ep2 = (sa * sa - sb * sb) / (sb * sb);
395  const precnum_t c = sa * sa / sb;
396  // const precnum_t alp = (sa-sb)/sa;
397 
398  const precnum_t Dlon = lon - lon0;
399  const precnum_t clat = cos(lat);
400  const precnum_t sDlon = sin(Dlon);
401  const precnum_t cDlon = cos(Dlon);
402 
403  const precnum_t A = clat * sDlon;
404  const precnum_t eps = 0.5 * log((1 + A) / (1 - A));
405  const precnum_t nu = atan2(tan(lat), cDlon) - lat;
406  const precnum_t v = 0.9996 * c / sqrt(1 + ep2 * clat * clat);
407  const precnum_t psi = 0.5 * ep2 * eps * eps * clat * clat;
408  const precnum_t A1 = sin(2 * lat);
409  const precnum_t A2 = A1 * clat * clat;
410  const precnum_t J2 = lat + 0.5 * A1;
411  const precnum_t J4 = 0.75 * J2 + 0.25 * A2;
412  const precnum_t J6 = (5.0 * J4 + A2 * clat * clat) / 3;
413  const precnum_t nalp = 0.75 * ep2;
414  const precnum_t nbet = (5.0 / 3.0) * nalp * nalp;
415  const precnum_t ngam = (35.0 / 27.0) * nalp * nalp * nalp;
416  const precnum_t B = 0.9996 * c * (lat - nalp * J2 + nbet * J4 - ngam * J6);
417 
418  UTMCoords.x = static_cast<double>(eps * v * (1 + psi / 3.0) + 500000);
419  UTMCoords.y = static_cast<double>(nu * v * (1 + psi) + B);
420  UTMCoords.z = static_cast<double>(GeodeticCoords.height);
421 
422  UTMZone = Huso;
423  UTMLatitudeBand = Letra;
424 }
425 
426 /*---------------------------------------------------------------
427  LatLonToUTM
428  ---------------------------------------------------------------*/
430  double la, double lo, double& xx, double& yy, int& out_UTM_zone,
431  char& out_UTM_latitude_band, const TEllipsoid& ellip)
432 {
433  // This method is based on public code by Gabriel Ruiz Martinez and Rafael
434  // Palacios.
435  // http://www.mathworks.com/matlabcentral/fileexchange/10915
436 
437  const double sa = ellip.sa;
438  const double sb = ellip.sb;
439  const double e2 = (sqrt((sa * sa) - (sb * sb))) / sb;
440 
441  const double e2cuadrada = e2 * e2;
442 
443  const double c = (sa * sa) / sb;
444 
445  const double lat = DEG2RAD(la);
446  const double lon = DEG2RAD(lo);
447 
448  const int Huso = mrpt::fix((lo / 6) + 31);
449  double S = ((Huso * 6) - 183);
450  double deltaS = lon - DEG2RAD(S);
451 
452  char Letra;
453 
454  if (la < -72)
455  Letra = 'C';
456  else if (la < -64)
457  Letra = 'D';
458  else if (la < -56)
459  Letra = 'E';
460  else if (la < -48)
461  Letra = 'F';
462  else if (la < -40)
463  Letra = 'G';
464  else if (la < -32)
465  Letra = 'H';
466  else if (la < -24)
467  Letra = 'J';
468  else if (la < -16)
469  Letra = 'K';
470  else if (la < -8)
471  Letra = 'L';
472  else if (la < 0)
473  Letra = 'M';
474  else if (la < 8)
475  Letra = 'N';
476  else if (la < 16)
477  Letra = 'P';
478  else if (la < 24)
479  Letra = 'Q';
480  else if (la < 32)
481  Letra = 'R';
482  else if (la < 40)
483  Letra = 'S';
484  else if (la < 48)
485  Letra = 'T';
486  else if (la < 56)
487  Letra = 'U';
488  else if (la < 64)
489  Letra = 'V';
490  else if (la < 72)
491  Letra = 'W';
492  else
493  Letra = 'X';
494 
495  const double a = cos(lat) * sin(deltaS);
496  const double epsilon = 0.5 * log((1 + a) / (1 - a));
497  const double nu = atan(tan(lat) / cos(deltaS)) - lat;
498  const double v = (c / sqrt((1 + (e2cuadrada * square(cos(lat)))))) * 0.9996;
499  const double ta = 0.5 * e2cuadrada * square(epsilon) * square(cos(lat));
500  const double a1 = sin(2 * lat);
501  const double a2 = a1 * square(cos(lat));
502  const double j2 = lat + 0.5 * a1;
503  const double j4 = ((3.0 * j2) + a2) / 4.0;
504  const double j6 = ((5.0 * j4) + (a2 * square(cos(lat)))) / 3.0;
505  const double alfa = 0.75 * e2cuadrada;
506  const double beta = (5.0 / 3.0) * pow(alfa, 2.0);
507  const double gama = (35.0 / 27.0) * pow(alfa, 3.0);
508  const double Bm = 0.9996 * c * (lat - alfa * j2 + beta * j4 - gama * j6);
509 
510  xx = epsilon * v * (1 + (ta / 3.0)) + 500000;
511  yy = nu * v * (1 + ta) + Bm;
512 
513  if (yy < 0) yy += 9999999;
514 
515  out_UTM_zone = Huso;
516  out_UTM_latitude_band = Letra;
517 }
518 
519 /** 7-parameter Bursa-Wolf transformation:
520  * [ X Y Z ]_WGS84 = [ dX dY dZ ] + ( 1 + dS ) [ 1 RZ -RY; -RZ 1 RX; RY -RX 1
521  * ] [ X Y Z ]_local
522  * \sa transform10params
523  */
525  const mrpt::math::TPoint3D& p, const TDatum7Params& d,
527 {
528  const double scale = (1 + d.dS);
529 
530  o.x = d.dX + scale * (p.x + p.y * d.Rz - p.z * d.Ry);
531  o.y = d.dY + scale * (-p.x * d.Rz + p.y + p.z * d.Rx);
532  o.z = d.dZ + scale * (p.x * d.Ry - p.y * d.Rx + p.z);
533 }
534 
535 /** 7-parameter Bursa-Wolf transformation TOPCON:
536  * [ X Y Z ]_WGS84 = [ dX dY dZ ] + ( 1 + dS ) [ 1 RZ -RY; -RZ 1 RX; RY -RX 1
537  * ] [ X Y Z ]_local
538  * \sa transform10params
539  */
541  const mrpt::math::TPoint3D& p, const TDatum7Params_TOPCON& d,
543 {
544  const double scale = (1 + d.dS);
545 
546  o.x = d.dX + scale * (d.m11 * p.x + d.m12 * p.y + d.m13 * p.z);
547  o.y = d.dY + scale * (d.m21 * p.x + d.m22 * p.y + d.m23 * p.z);
548  o.z = d.dZ + scale * (d.m31 * p.x + d.m32 * p.y + d.m33 * p.z);
549 }
550 
551 /** 10-parameter Molodensky-Badekas transformation:
552  * [ X Y Z ]_WGS84 = [ dX dY dZ ] + ( 1 + dS ) [ 1 RZ -RY; -RZ 1 RX; RY -RX 1
553  * ] [ X-Xp Y-Yp Z-Zp ]_local + [Xp Yp Zp]
554  * \sa transform7params
555  */
557  const mrpt::math::TPoint3D& p, const TDatum10Params& d,
559 {
560  const double scale = (1 + d.dS);
561 
562  const double px = p.x - d.Xp;
563  const double py = p.y - d.Yp;
564  const double pz = p.z - d.Zp;
565 
566  o.x = d.dX + scale * (px + py * d.Rz - pz * d.Ry) + d.Xp;
567  o.y = d.dY + scale * (-px * d.Rz + py + pz * d.Rx) + d.Yp;
568  o.z = d.dZ + scale * (px * d.Ry - py * d.Rx + pz) + d.Zp;
569 }
570 
571 /** Helmert 2D transformation:
572  * [ X Y ]_WGS84 = [ dX dY ] + ( 1 + dS ) [ cos(alpha) -sin(alpha);
573  * sin(alpha) cos(alpha) ] [ X-Xp Y-Yp Z-Zp ]_local + [Xp Yp Zp]
574  * \sa transformHelmert3D
575  */
577  const mrpt::math::TPoint2D& p, const TDatumHelmert2D& d,
579 {
580  const double scale = (1 + d.dS);
581 
582  const double px = p.x - d.Xp;
583  const double py = p.y - d.Yp;
584 
585  o.x = d.dX + scale * (px * cos(d.alpha) - py * sin(d.alpha)) + d.Xp;
586  o.y = d.dY + scale * (px * sin(d.alpha) + py * cos(d.alpha)) + d.Yp;
587 }
588 
589 /** Helmert 2D transformation:
590  * [ X Y ]_WGS84 = [ dX dY ] + ( 1 + dS ) [ cos(alpha) -sin(alpha);
591  * sin(alpha) cos(alpha) ] [ X-Xp Y-Yp Z-Zp ]_local + [Xp Yp Zp]
592  * \sa transformHelmert3D
593  */
595  const mrpt::math::TPoint2D& p, const TDatumHelmert2D_TOPCON& d,
597 {
598  o.x = d.a * p.x + d.b * p.y + d.c;
599  o.y = -d.b * p.x + d.a * p.y + d.d;
600 }
601 
602 /** Helmert 3D transformation:
603  * [ X Y ]_WGS84 = [ dX dY ] + ( 1 + dS ) [ cos(alpha) -sin(alpha);
604  * sin(alpha) cos(alpha) ] [ X-Xp Y-Yp Z-Zp ]_local + [Xp Yp Zp]
605  * \sa transformHelmert3D
606  */
608  const mrpt::math::TPoint3D& p, const TDatumHelmert3D& d,
610 {
611  TDatum7Params d2(d.dX, d.dY, d.dZ, -1 * d.Rx, -1 * d.Ry, -1 * d.Rz, d.dS);
612  transform7params(p, d2, o);
613 }
614 
615 /** Helmert 3D transformation:
616  * [ X Y ]_WGS84 = [ dX dY ] + ( 1 + dS ) [ cos(alpha) -sin(alpha);
617  * sin(alpha) cos(alpha) ] [ X-Xp Y-Yp Z-Zp ]_local + [Xp Yp Zp]
618  * \sa transformHelmert3D
619  */
621  const mrpt::math::TPoint3D& p, const TDatumHelmert3D_TOPCON& d,
623 {
624  o.x = d.a * p.x + d.b * p.y + d.c;
625  o.y = d.d * p.x + d.e * p.y + d.f;
626  o.z = p.z + d.g;
627 }
628 
629 /** 1D transformation:
630  * [ Z ]_WGS84 = (dy * X - dx * Y + Z)*(1+e)+DZ
631  * \sa transformHelmert3D
632  */
634  const mrpt::math::TPoint3D& p, const TDatum1DTransf& d,
636 {
637  o.x = p.x;
638  o.y = p.y;
639  o.z = (d.dY * p.x - d.dX * p.y + p.z) * (1 + d.dS) + d.DZ;
640 }
641 
642 /** 1D transformation:
643  * [ X;Y ]_WGS84 = [X;Y]_locales+[1 -sin(d.beta);0
644  * cos(d.beta)]*[x*d.dSx;y*d.dSy ]
645  * \sa transformHelmert3D
646  */
650 {
651  o.x = d.dX + p.x * d.dSx - p.y * d.dSy * sin(d.beta);
652  o.y = d.dY + p.y * d.dSy * cos(d.beta);
653  o.z = p.z;
654 }
655 
656 /** ENU to geocentric coordinates.
657  * \sa geodeticToENU_WGS84
658  */
660  const mrpt::math::TPoint3D& p, const TGeodeticCoords& in_coords_origin,
661  TGeocentricCoords& out_coords, const TEllipsoid& ellip)
662 {
663  // Generate reference 3D point:
664  TPoint3D P_geocentric_ref;
666  in_coords_origin, P_geocentric_ref, ellip);
667 
668  CVectorDouble P_ref(3);
669  P_ref[0] = P_geocentric_ref.x;
670  P_ref[1] = P_geocentric_ref.y;
671  P_ref[2] = P_geocentric_ref.z;
672 
673  // Z axis -> In direction out-ward the center of the Earth:
674  CVectorDouble REF_X(3), REF_Y(3), REF_Z(3);
675  math::normalize(P_ref, REF_Z);
676 
677  // 1st column: Starting at the reference point, move in the tangent
678  // direction
679  // east-ward: I compute this as the derivative of P_ref wrt "longitude":
680  // A_east[0] =-(N+in_height_meters)*cos(lat)*sin(lon); --> -Z[1]
681  // A_east[1] = (N+in_height_meters)*cos(lat)*cos(lon); --> Z[0]
682  // A_east[2] = 0; --> 0
683  // ---------------------------------------------------------------------------
684  CVectorDouble AUX_X(3);
685  AUX_X[0] = -REF_Z[1];
686  AUX_X[1] = REF_Z[0];
687  AUX_X[2] = 0;
688  math::normalize(AUX_X, REF_X);
689 
690  // 2nd column: The cross product:
691  math::crossProduct3D(REF_Z, REF_X, REF_Y);
692 
693  out_coords.x =
694  REF_X[0] * p.x + REF_Y[0] * p.y + REF_Z[0] * p.z + P_geocentric_ref.x;
695  out_coords.y =
696  REF_X[1] * p.x + REF_Y[1] * p.y + REF_Z[1] * p.z + P_geocentric_ref.y;
697  out_coords.z =
698  REF_X[2] * p.x + REF_Y[2] * p.y + REF_Z[2] * p.z + P_geocentric_ref.z;
699 }
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:759
void transformHelmert2D_TOPCON(const mrpt::math::TPoint2D &p, const TDatumHelmert2D_TOPCON &d, mrpt::math::TPoint2D &o)
Helmert 2D transformation: [ X Y ]_WGS84 = [ dX dY ] + ( 1 + dS ) [ cos(alpha) -sin(alpha); sin(alpha...
double sa
largest semiaxis of the reference ellipsoid (in meters)
Definition: data_types.h:89
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
double dX
Deltas (X,Y,Z)
Definition: data_types.h:223
double dX
Deltas (X,Y,Z)
Definition: data_types.h:277
int fix(T x)
Rounds toward zero.
void transformHelmert2D(const mrpt::math::TPoint2D &p, const TDatumHelmert2D &d, mrpt::math::TPoint2D &o)
Helmert 2D transformation: [ X Y ]_WGS84 = [ dX dY ] + ( 1 + dS ) [ cos(alpha) -sin(alpha); sin(alpha...
double dS
Scale factor (in ppm) (Scale is 1+dS/1e6)
Definition: data_types.h:247
Parameters for a topographic transfomation.
Definition: data_types.h:333
Parameters for a topographic transfomation.
Definition: data_types.h:301
T x
X,Y coordinates.
Definition: TPoint2D.h:25
double Rx
Rotation components.
Definition: data_types.h:281
void transform7params(const mrpt::math::TPoint3D &in_point, const TDatum7Params &in_datum, mrpt::math::TPoint3D &out_point)
7-parameter Bursa-Wolf transformation: [ X Y Z ]_WGS84 = [ dX dY dZ ] + ( 1 + dS ) [ 1 RZ -RY; -RZ 1 ...
void ENU_axes_from_WGS84(double in_longitude_reference_degrees, double in_latitude_reference_degrees, double in_height_reference_meters, mrpt::math::TPose3D &out_ENU, bool only_angles=false)
Returns the East-North-Up (ENU) coordinate system associated to the given point.
A set of geodetic coordinates: latitude, longitude and height, defined over a given geoid (typically...
Definition: data_types.h:192
double dS
Scale factor (in ppm) (Scale is 1+dS/1e6)
Definition: data_types.h:227
bool operator!=(const TCoords &a, const TCoords &o)
Definition: conversions.cpp:27
bool operator==(const TCoords &a, const TCoords &o)
Definition: conversions.cpp:23
double dS
Scale factor (Scale is 1+dS)
Definition: data_types.h:377
STL namespace.
double Xp
To be substracted to the input point.
Definition: data_types.h:279
void transform1D(const mrpt::math::TPoint3D &p, const TDatum1DTransf &d, mrpt::math::TPoint3D &o)
1D transformation: [ Z ]_WGS84 = (dy * X - dx * Y + Z)*(1+e)+DZ
void geodeticToENU_WGS84(const TGeodeticCoords &in_coords, mrpt::math::TPoint3D &out_ENU_point, const TGeodeticCoords &in_coords_origin)
Coordinates transformation from longitude/latitude/height to ENU (East-North-Up) X/Y/Z coordinates Th...
void geodeticToGeocentric(const TGeodeticCoords &in_coords, TGeocentricCoords &out_point, const TEllipsoid &ellip)
Coordinates transformation from longitude/latitude/height to geocentric X/Y/Z coordinates (with an sp...
double Rx
Rotation components.
Definition: data_types.h:338
A coordinate that is stored as a simple "decimal" angle in degrees, but can be retrieved/set in the f...
Definition: data_types.h:24
void geocentricToENU_WGS84(const mrpt::math::TPoint3D &in_geocentric_point, mrpt::math::TPoint3D &out_ENU_point, const TGeodeticCoords &in_coords_origin)
ENU to EFEC (Geocentric) coordinates.
Definition: conversions.cpp:60
void transformHelmert3D_TOPCON(const mrpt::math::TPoint3D &p, const TDatumHelmert3D_TOPCON &d, mrpt::math::TPoint3D &o)
Helmert 3D transformation: [ X Y ]_WGS84 = [ dX dY ] + ( 1 + dS ) [ cos(alpha) -sin(alpha); sin(alpha...
void transform10params(const mrpt::math::TPoint3D &in_point, const TDatum10Params &in_datum, mrpt::math::TPoint3D &out_point)
10-parameter Molodensky-Badekas transformation: [ X Y Z ]_WGS84 = [ dX dY dZ ] + ( 1 + dS ) [ 1 RZ -R...
void crossProduct3D(const T &v0, const U &v1, V &vOut)
Computes the cross product of two 3D vectors, returning a vector normal to both.
Definition: geometry.h:765
double dSx
Scale factor in X and Y.
Definition: data_types.h:395
std::string getAsString() const
Return a std::string in the format "DEGdeg MIN&#39; SEC&#39;&#39;".
Definition: data_types.h:65
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
double Rx
Rotation components (in secs)
Definition: data_types.h:225
Parameters for a topographic transfomation.
Definition: data_types.h:357
void transfInterpolation(const mrpt::math::TPoint3D &p, const TDatumTransfInterpolation &d, mrpt::math::TPoint3D &o)
Interpolation: [ Z ]_WGS84 = (dy * X - dx * Y + Z)*(1+e)+DZ.
double sb
smallest semiaxis of the reference ellipsoid (in meters)
Definition: data_types.h:91
Parameters for a topographic transfomation.
Definition: data_types.h:372
This base provides a set of functions for maths stuff.
TCoords lon
Longitude (in degrees)
Definition: data_types.h:209
void geodeticToUTM(const TGeodeticCoords &GeodeticCoords, TUTMCoords &UTMCoords, int &UTMZone, char &UTMLatitudeBand, const TEllipsoid &ellip=TEllipsoid::Ellipsoid_WGS84())
void normalize(CONTAINER &c, Scalar valMin, Scalar valMax)
Scales all elements such as the minimum & maximum values are shifted to the given values...
void geocentricToGeodetic(const TGeocentricCoords &in_point, TGeodeticCoords &out_coords, const TEllipsoid &ellip=TEllipsoid::Ellipsoid_WGS84())
Coordinates transformation from geocentric X/Y/Z coordinates to longitude/latitude/height.
double lat
[deg], [deg], hgt over sea level[m]
const double eps
double dS
Scale factor (Scale is 1+dS)
Definition: data_types.h:283
double dS
Scale factor (Scale is 1+dS)
Definition: data_types.h:340
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void UTMToGeodetic(double X, double Y, int zone, char hem, double &out_lon, double &out_lat, const TEllipsoid &ellip=TEllipsoid::Ellipsoid_WGS84())
Returns the Geodetic coordinates of the UTM input point.
return_t square(const num_t x)
Inline function for the square of a number.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
double precnum_t
Definition: conversions.cpp:49
Parameters for a topographic transfomation.
Definition: data_types.h:274
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
void ENUToGeocentric(const mrpt::math::TPoint3D &in_ENU_point, const TGeodeticCoords &in_coords_origin, TGeocentricCoords &out_coords, const TEllipsoid &ellip)
ENU to geocentric coordinates.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
std::ostream & operator<<(std::ostream &out, const TCoords &o)
Definition: conversions.cpp:52
double dX
Deltas (X,Y,Z)
Definition: data_types.h:336
double height
Geodetic height (in meters)
Definition: data_types.h:211
void geodeticToGeocentric_WGS84(const TGeodeticCoords &in_coords, mrpt::math::TPoint3D &out_point)
Coordinates transformation from longitude/latitude/height to geocentric X/Y/Z coordinates (with a WGS...
double decimal_value
Also obtained directly through the double(void) operator using a TCoords anywhere were a double is ex...
Definition: data_types.h:29
void GeodeticToUTM(double in_latitude_degrees, double in_longitude_degrees, double &out_UTM_x, double &out_UTM_y, int &out_UTM_zone, char &out_UTM_latitude_band, const TEllipsoid &ellip=TEllipsoid::Ellipsoid_WGS84())
Convert latitude and longitude coordinates into UTM coordinates, computing the corresponding UTM zone...
Parameters for a topographic transfomation.
Definition: data_types.h:220
Parameters for a topographic transfomation.
Definition: data_types.h:390
void transformHelmert3D(const mrpt::math::TPoint3D &p, const TDatumHelmert3D &d, mrpt::math::TPoint3D &o)
Helmert3D transformation: [ X Y Z ]_WGS84 = [ dX dY dZ ] + ( 1 + dS ) [ 1 -RZ RY; RZ 1 -RX; -RY RX 1 ...
TCoords lat
Latitude (in degrees)
Definition: data_types.h:207
void transform7params_TOPCON(const mrpt::math::TPoint3D &in_point, const TDatum7Params_TOPCON &in_datum, mrpt::math::TPoint3D &out_point)
7-parameter Bursa-Wolf transformation TOPCON: [ X Y Z ]_WGS84 = [ dX dY dZ ] + ( 1 + dS ) [ 1 RZ -RY;...
double dX
Deltas (X,Y,Z)
Definition: data_types.h:375



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 5711e29ae Wed May 27 14:29:47 2020 +0200 at miƩ may 27 14:30:10 CEST 2020