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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019