Branch data     Line data    Source code

       1                 :            : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
       2                 :            : //
       3                 :            : // Copyright (C) 2018-2021 Intel Corporation
       4                 :            : //
       5                 :            : // SPDX-License-Identifier: MIT
       6                 :            : //
       7                 :            : // ----------------- END LICENSE BLOCK -----------------------------------
       8                 :            : 
       9                 :            : #include "ad/map/point/CoordinateTransform.hpp"
      10                 :            : #include "ad/map/access/Logging.hpp"
      11                 :            : 
      12                 :            : #include <cmath>
      13                 :            : #include <cstring>
      14                 :            : #include "ad/map/point/Operation.hpp"
      15                 :            : 
      16                 :            : namespace ad {
      17                 :            : namespace map {
      18                 :            : namespace point {
      19                 :            : 
      20                 :            : //////////////////////////
      21                 :            : // Constructor/Destructor
      22                 :            : 
      23                 :       3113 : CoordinateTransform::CoordinateTransform()
      24                 :            : {
      25                 :       3113 :   enu_ref_ = instance_counter_;
      26                 :       3113 :   instance_counter_ += 0x00100000;
      27                 :       3113 :   enu_h_ = 0;
      28                 :       3113 :   enu_tmp1_ = 0;
      29                 :       3113 :   enu_tmp1_3_ = 0;
      30                 :       3113 :   enu_cp_ = 0;
      31                 :       3113 :   enu_sp_ = 0;
      32                 :       3113 :   enu_cp_2_ = 0;
      33                 :       3113 :   enu_sp_2_ = 0;
      34                 :       3113 :   enu_phi_ = 0;
      35                 :       3113 :   enu_lam_ = 0;
      36                 :       3113 :   std::memset(ecef_enu_, 0, sizeof(ecef_enu_));
      37                 :            : 
      38                 :       3113 :   projPtr_ = nullptr;
      39                 :       3113 : }
      40                 :            : 
      41                 :       6226 : CoordinateTransform::~CoordinateTransform()
      42                 :            : {
      43         [ +  + ]:       3113 :   if (projPtr_ != nullptr)
      44                 :            :   {
      45                 :         33 :     pj_dalloc(projPtr_);
      46                 :         33 :     projPtr_ = nullptr;
      47                 :            :   }
      48                 :       3113 : }
      49                 :            : 
      50                 :         44 : bool CoordinateTransform::setGeoProjection(std::string const &geo_projection)
      51                 :            : {
      52         [ -  + ]:         44 :   if (projPtr_ != nullptr)
      53                 :            :   {
      54                 :          0 :     pj_dalloc(projPtr_);
      55                 :          0 :     projPtr_ = nullptr;
      56                 :            :   }
      57                 :         44 :   projPtr_ = pj_init_plus(geo_projection.c_str());
      58         [ +  + ]:         44 :   if (projPtr_ != nullptr)
      59                 :            :   {
      60                 :         33 :     enu_ref_++;
      61         [ +  - ]:         33 :     auto enu_zero = createENUPoint(0, 0, 0);
      62                 :            :     // create dummy reference point to allow calculation of actual one
      63         [ +  - ]:         33 :     enu_ref_point_ = createGeoPoint(Longitude(0), Latitude(0), Altitude(0));
      64         [ +  - ]:         33 :     enu_ref_point_ = ENU2Geo(enu_zero);
      65         [ +  - ]:         33 :     ecef_ref_point_ = Geo2ECEF(enu_ref_point_);
      66                 :         33 :     return true;
      67                 :            :   }
      68                 :         11 :   return false;
      69                 :            : }
      70                 :            : 
      71                 :      72110 : bool CoordinateTransform::isGeoProjectionValid() const
      72                 :            : {
      73                 :      72110 :   return projPtr_ != nullptr;
      74                 :            : }
      75                 :            : 
      76                 :       2863 : void CoordinateTransform::setENUReferencePoint(const GeoPoint &enu_ref_point)
      77                 :            : {
      78         [ +  + ]:       2863 :   if (isGeoProjectionValid())
      79                 :            :   {
      80         [ +  - ]:          1 :     access::getLogger()->warn("Set ENU Reference Point ignored in geo projection mode!");
      81                 :            :   }
      82         [ +  + ]:       2862 :   else if (isValid(enu_ref_point))
      83                 :            :   {
      84                 :       2861 :     enu_ref_++;
      85                 :       2861 :     enu_ref_point_ = enu_ref_point;
      86                 :       2861 :     ecef_ref_point_ = Geo2ECEF(enu_ref_point);
      87                 :            : 
      88                 :       2861 :     enu_phi_ = toRadians(enu_ref_point_.latitude);
      89                 :       2861 :     enu_lam_ = toRadians(enu_ref_point_.longitude);
      90                 :       2861 :     double sin_lam = std::sin(enu_lam_);
      91                 :       2861 :     double cos_lam = std::cos(enu_lam_);
      92                 :       2861 :     double sin_phi = std::sin(enu_phi_);
      93                 :       2861 :     double cos_phi = std::cos(enu_phi_);
      94                 :            : 
      95                 :       2861 :     enu_h_ = static_cast<double>(enu_ref_point_.altitude);
      96                 :       2861 :     enu_tmp1_ = std::sqrt(1 - E2 * sin_phi * sin_phi);
      97                 :       2861 :     enu_tmp1_3_ = enu_tmp1_ * enu_tmp1_ * enu_tmp1_;
      98                 :       2861 :     enu_cp_ = cos_phi;
      99                 :       2861 :     enu_sp_ = sin_phi;
     100                 :       2861 :     enu_cp_2_ = enu_cp_ * enu_cp_;
     101                 :       2861 :     enu_sp_2_ = enu_sp_ * enu_sp_;
     102                 :            : 
     103                 :       2861 :     ecef_enu_[0] = -sin_lam;
     104                 :       2861 :     ecef_enu_[1] = cos_lam;
     105                 :       2861 :     ecef_enu_[2] = 0;
     106                 :       2861 :     ecef_enu_[3] = -cos_lam * sin_phi;
     107                 :       2861 :     ecef_enu_[4] = -sin_lam * sin_phi;
     108                 :       2861 :     ecef_enu_[5] = cos_phi;
     109                 :       2861 :     ecef_enu_[6] = cos_lam * cos_phi;
     110                 :       2861 :     ecef_enu_[7] = sin_lam * cos_phi;
     111                 :       2861 :     ecef_enu_[8] = sin_phi;
     112                 :            :   }
     113                 :            :   else
     114                 :            :   {
     115         [ +  - ]:          1 :     access::getLogger()->error("Invalid ENU Reference Point provided!");
     116         [ +  - ]:          1 :     throw std::invalid_argument("Invalid ENU Reference Point provided!");
     117                 :            :   }
     118                 :       2862 : }
     119                 :            : 
     120                 :        470 : const GeoPoint &CoordinateTransform::getENUReferencePoint() const
     121                 :            : {
     122                 :        470 :   return enu_ref_point_;
     123                 :            : }
     124                 :            : 
     125                 :         49 : size_t CoordinateTransform::getENURef() const
     126                 :            : {
     127                 :         49 :   return enu_ref_;
     128                 :            : }
     129                 :            : 
     130                 :      69483 : bool CoordinateTransform::isENUValid() const
     131                 :            : {
     132                 :      69483 :   return isValid(enu_ref_point_, false);
     133                 :            : }
     134                 :            : 
     135                 :            : ////////////////////////////////
     136                 :            : // ENU Reference Point Management
     137                 :            : 
     138                 :      20542 : ENUPoint CoordinateTransform::Geo2ENU(const GeoPoint &pt) const
     139                 :            : {
     140         [ +  + ]:      20542 :   if (isENUValid())
     141                 :            :   {
     142         [ +  + ]:      20541 :     if (isValid(pt))
     143                 :            :     {
     144         [ +  + ]:      20540 :       if (isGeoProjectionValid())
     145                 :            :       {
     146                 :            :         projXY pjGeoPoint;
     147                 :      20522 :         pjGeoPoint.u = toRadians(pt.longitude);
     148                 :      20522 :         pjGeoPoint.v = toRadians(pt.latitude);
     149                 :            : 
     150         [ +  - ]:      20522 :         auto pjEnuPoint = pj_fwd(pjGeoPoint, projPtr_);
     151         [ +  - ]:      20522 :         return createENUPoint(pjEnuPoint.u, pjEnuPoint.v, static_cast<double>(pt.altitude));
     152                 :            :       }
     153                 :            :       else
     154                 :            :       {
     155                 :         18 :         double dphi = toRadians(pt.latitude) - enu_phi_;
     156                 :         18 :         double dlam = toRadians(pt.longitude) - enu_lam_;
     157                 :         18 :         double dh = static_cast<double>(pt.altitude) - enu_h_;
     158                 :         18 :         double dlam_2 = dlam * dlam;
     159                 :         18 :         double dphi_2 = dphi * dphi;
     160                 :         18 :         double de = (A / enu_tmp1_ + enu_h_) * enu_cp_ * dlam
     161                 :         18 :           - (A * (1 - E2) / enu_tmp1_3_ + enu_h_) * enu_sp_ * dphi * dlam + enu_cp_ * dlam * dh;
     162                 :         18 :         double dn = (A * (1 - E2) / enu_tmp1_3_ + enu_h_) * dphi + 1.5 * enu_cp_ * enu_sp_ * A * E2 * dphi_2
     163                 :         18 :           + enu_sp_2_ * dh * dphi + 0.5 * enu_sp_ * enu_cp_ * (A / enu_tmp1_ + enu_h_) * dlam_2;
     164                 :         18 :         double du = dh - 0.5 * (A - 1.5 * A * E2 * enu_cp_2_ + 0.5 * A * E2 + enu_h_) * dphi_2
     165                 :         18 :           - 0.5 * enu_cp_2_ * (A / enu_tmp1_ - enu_h_) * dlam_2;
     166                 :         18 :         return createENUPoint(de, dn, du);
     167                 :            :       }
     168                 :            :     }
     169                 :            :     else
     170                 :            :     {
     171         [ +  - ]:          1 :       access::getLogger()->error("Cannot convert from Geo to ENU: Input Point invalid.");
     172         [ +  - ]:          1 :       throw std::invalid_argument("Cannot convert from Geo to ENU: Input Point invalid.");
     173                 :            :     }
     174                 :            :   }
     175                 :            :   else
     176                 :            :   {
     177         [ +  - ]:          1 :     access::getLogger()->error("Cannot convert from Geo to ENU: Reference Point not defined.");
     178         [ +  - ]:          1 :     throw std::invalid_argument("Cannot convert from Geo to ENU: Reference Point not defined.");
     179                 :            :   }
     180                 :            : }
     181                 :            : 
     182                 :      21606 : GeoPoint CoordinateTransform::ENU2Geo(const ENUPoint &pt) const
     183                 :            : {
     184         [ +  + ]:      21606 :   if (isENUValid())
     185                 :            :   {
     186         [ +  + ]:      21605 :     if (isValid(pt))
     187                 :            :     {
     188         [ +  + ]:      21604 :       if (isGeoProjectionValid())
     189                 :            :       {
     190                 :            :         projXY pjEnuPoint;
     191                 :      21593 :         pjEnuPoint.u = static_cast<double>(pt.x);
     192                 :      21593 :         pjEnuPoint.v = static_cast<double>(pt.y);
     193                 :            : 
     194         [ +  - ]:      21593 :         auto pjGeoPoint = pj_inv(pjEnuPoint, projPtr_);
     195                 :            :         return createGeoPoint(Longitude(radians2degree(pjGeoPoint.u)),
     196                 :            :                               Latitude(radians2degree(pjGeoPoint.v)),
     197         [ +  - ]:      21593 :                               Altitude(static_cast<double>(pt.z)));
     198                 :            :       }
     199                 :            :       else
     200                 :            :       {
     201                 :            :         //* \todo Direct conversion ENU2Geo!
     202         [ +  - ]:         11 :         ECEFPoint ecef = ENU2ECEF(pt);
     203         [ +  - ]:         11 :         return ECEF2Geo(ecef);
     204                 :            :       }
     205                 :            :     }
     206                 :            :     else
     207                 :            :     {
     208         [ +  - ]:          1 :       access::getLogger()->error("Cannot convert from ENU to Geo: Input Point invalid.");
     209         [ +  - ]:          1 :       throw std::invalid_argument("Cannot convert from ENU to Geo: Input Point invalid.");
     210                 :            :     }
     211                 :            :   }
     212                 :            :   else
     213                 :            :   {
     214         [ +  - ]:          1 :     access::getLogger()->error("Cannot convert from ENU to Geo: Reference Point not defined.");
     215         [ +  - ]:          1 :     throw std::invalid_argument("Cannot convert from ENU to Geo: Reference Point not defined.");
     216                 :            :   }
     217                 :            : }
     218                 :            : 
     219                 :            : ///////////////////////
     220                 :            : // ECEF/Geo Conversions
     221                 :            : 
     222                 :     461705 : ECEFPoint CoordinateTransform::Geo2ECEF(const GeoPoint &pt) const
     223                 :            : {
     224         [ +  + ]:     461705 :   if (isValid(pt))
     225                 :            :   {
     226                 :     461704 :     double lat = toRadians(pt.latitude);
     227                 :     461704 :     double lon = toRadians(pt.longitude);
     228                 :     461704 :     double alt = static_cast<double>(pt.altitude);
     229                 :     461704 :     double sinlat = std::sin(lat);
     230                 :     461704 :     double coslat = std::cos(lat);
     231                 :     461704 :     double n = A / std::sqrt(1 - E2 * sinlat * sinlat);
     232                 :     461704 :     double x = (n + alt) * coslat * std::cos(lon);
     233                 :     461704 :     double y = (n + alt) * coslat * std::sin(lon);
     234                 :     461704 :     double z = (n * (1 - E2) + alt) * sinlat;
     235                 :     461704 :     return createECEFPoint(x, y, z);
     236                 :            :   }
     237                 :            :   else
     238                 :            :   {
     239         [ +  - ]:          1 :     access::getLogger()->error("Cannot convert from Geo to ECEF: Input point invalid.");
     240         [ +  - ]:          1 :     throw std::invalid_argument("Cannot convert from Geo to ECEF: Input point invalid.");
     241                 :            :   }
     242                 :            : }
     243                 :            : 
     244                 :      41505 : GeoPoint CoordinateTransform::ECEF2Geo(const ECEFPoint &pt) const
     245                 :            : {
     246         [ +  + ]:      41505 :   if (isValid(pt))
     247                 :            :   {
     248                 :      41504 :     double x = static_cast<double>(pt.x);
     249                 :      41504 :     double y = static_cast<double>(pt.y);
     250                 :      41504 :     double z = static_cast<double>(pt.z);
     251                 :      41504 :     double zp = std::abs(z);
     252                 :      41504 :     double w2 = x * x + y * y;
     253                 :      41504 :     double w = std::sqrt(w2);
     254                 :      41504 :     double r2 = w2 + z * z;
     255                 :      41504 :     double r = std::sqrt(r2);
     256                 :      41504 :     double lon = std::atan2(y, x);
     257                 :      41504 :     double s2 = z * z / r2;
     258                 :      41504 :     double c2 = w2 / r2;
     259                 :      41504 :     double u = A2 / r;
     260                 :      41504 :     double v = A3 - A4 / r;
     261                 :      41504 :     double lat = 0;
     262                 :      41504 :     double c = 0;
     263                 :      41504 :     double ss = 0;
     264                 :      41504 :     double s = 0;
     265         [ +  + ]:      41504 :     if (c2 > 0.3)
     266                 :            :     {
     267                 :      41465 :       s = (zp / r) * (1.0 + c2 * (A1 + u + s2 * v) / r);
     268                 :      41465 :       lat = std::asin(s);
     269                 :      41465 :       ss = s * s;
     270                 :      41465 :       c = std::sqrt(1.0 - ss);
     271                 :            :     }
     272                 :            :     else
     273                 :            :     {
     274                 :         39 :       c = (w / r) * (1.0 - s2 * (A5 - u - c2 * v) / r);
     275                 :         39 :       lat = std::acos(c);
     276                 :         39 :       ss = 1.0 - c * c;
     277                 :         39 :       s = std::sqrt(ss);
     278                 :            :     }
     279                 :      41504 :     double g = 1.0 - E2 * ss;
     280                 :      41504 :     double rg = A / std::sqrt(g);
     281                 :      41504 :     double rf = A6 * rg;
     282                 :      41504 :     u = w - rg * c;
     283                 :      41504 :     v = zp - rf * s;
     284                 :      41504 :     double f = c * u + s * v;
     285                 :      41504 :     double m = c * v - s * u;
     286                 :      41504 :     double p = m / (rf / g + f);
     287                 :      41504 :     lat = lat + p;
     288                 :      41504 :     double alt = f + m * p / 2.0;
     289         [ +  + ]:      41504 :     if (z < 0.0)
     290                 :            :     {
     291                 :      18698 :       lat *= -1.0;
     292                 :            :     }
     293         [ +  - ]:      41504 :     return createGeoPoint(Longitude(radians2degree(lon)), Latitude(radians2degree(lat)), Altitude(alt));
     294                 :            :   }
     295                 :            :   else
     296                 :            :   {
     297         [ +  - ]:          1 :     access::getLogger()->error("Cannot convert from ECEF to Geo: Input point invalid.");
     298         [ +  - ]:          1 :     throw std::invalid_argument("Cannot convert from ECEF to Geo: Input point invalid.");
     299                 :            :   }
     300                 :            : }
     301                 :            : 
     302                 :            : ///////////////////////
     303                 :            : // ECEF/ENU Conversions
     304                 :            : 
     305                 :       3815 : ECEFPoint CoordinateTransform::ENU2ECEF(const ENUPoint &pt) const
     306                 :            : {
     307         [ +  + ]:       3815 :   if (isENUValid())
     308                 :            :   {
     309         [ +  + ]:       3814 :     if (isValid(pt))
     310                 :            :     {
     311         [ +  + ]:       3813 :       if (isGeoProjectionValid())
     312                 :            :       {
     313         [ +  - ]:       1158 :         GeoPoint geo = ENU2Geo(pt);
     314         [ +  - ]:       1158 :         return Geo2ECEF(geo);
     315                 :            :       }
     316                 :            :       else
     317                 :            :       {
     318                 :       2655 :         double x = static_cast<double>(pt.x);
     319                 :       2655 :         double y = static_cast<double>(pt.y);
     320                 :       2655 :         double z = static_cast<double>(pt.z);
     321                 :       2655 :         double ecef_x = ecef_enu_[0] * x + ecef_enu_[3] * y + ecef_enu_[6] * z;
     322                 :       2655 :         double ecef_y = ecef_enu_[1] * x + ecef_enu_[4] * y + ecef_enu_[7] * z;
     323                 :       2655 :         double ecef_z = ecef_enu_[5] * y + ecef_enu_[8] * z;
     324         [ +  - ]:       2655 :         auto const ecef_base = createECEFPoint(ecef_x, ecef_y, ecef_z);
     325         [ +  - ]:       2655 :         auto const ecef_result = ecef_base + ecef_ref_point_;
     326                 :       2655 :         return ecef_result;
     327                 :            :       }
     328                 :            :     }
     329                 :            :     else
     330                 :            :     {
     331         [ +  - ]:          1 :       access::getLogger()->error("Cannot convert from ENU to ECEF: Input Point invalid.");
     332         [ +  - ]:          1 :       throw std::invalid_argument("Cannot convert from ENU to ECEF: Input Point invalid.");
     333                 :            :     }
     334                 :            :   }
     335                 :            :   else
     336                 :            :   {
     337         [ +  - ]:          1 :     access::getLogger()->error("Cannot convert from ENU to ECEF: ENU Reference Point invalid.");
     338         [ +  - ]:          1 :     throw std::invalid_argument("Cannot convert from ENU to ECEF: ENU Reference Point invalid.");
     339                 :            :   }
     340                 :            : }
     341                 :            : 
     342                 :      23294 : ENUPoint CoordinateTransform::ECEF2ENU(const ECEFPoint &pt) const
     343                 :            : {
     344         [ +  + ]:      23294 :   if (isENUValid())
     345                 :            :   {
     346         [ +  + ]:      23291 :     if (isValid(pt))
     347                 :            :     {
     348         [ +  + ]:      23290 :       if (isGeoProjectionValid())
     349                 :            :       {
     350         [ +  - ]:      20520 :         GeoPoint geo = ECEF2Geo(pt);
     351         [ +  - ]:      20520 :         return Geo2ENU(geo);
     352                 :            :       }
     353                 :            :       else
     354                 :            :       {
     355         [ +  - ]:       2770 :         auto const ecef_d = pt - ecef_ref_point_;
     356                 :       2770 :         double x = static_cast<double>(ecef_d.x);
     357                 :       2770 :         double y = static_cast<double>(ecef_d.y);
     358                 :       2770 :         double z = static_cast<double>(ecef_d.z);
     359                 :       2770 :         double enu_x = ecef_enu_[0] * x + ecef_enu_[1] * y;
     360                 :       2770 :         double enu_y = ecef_enu_[3] * x + ecef_enu_[4] * y + ecef_enu_[5] * z;
     361                 :       2770 :         double enu_z = ecef_enu_[6] * x + ecef_enu_[7] * y + ecef_enu_[8] * z;
     362         [ +  - ]:       2770 :         return createENUPoint(enu_x, enu_y, enu_z);
     363                 :            :       }
     364                 :            :     }
     365                 :            :     else
     366                 :            :     {
     367         [ +  - ]:          1 :       access::getLogger()->error("Cannot convert from ECEF to ENU: Input Point invalid.");
     368         [ +  - ]:          1 :       throw std::invalid_argument("Cannot convert from ECEF to ENU: Input Point invalid.");
     369                 :            :     }
     370                 :            :   }
     371                 :            :   else
     372                 :            :   {
     373         [ +  - ]:          3 :     access::getLogger()->error("Cannot convert from ECEF to ENU: ENU Reference Point invalid.");
     374         [ +  - ]:          3 :     throw std::invalid_argument("Cannot convert from ECEF to ENU: ENU Reference Point invalid.");
     375                 :            :   }
     376                 :            : }
     377                 :            : 
     378                 :            : /////////////////
     379                 :            : // Useful methods
     380                 :            : 
     381                 :        100 : physics::Distance CoordinateTransform::WGS84_R(Latitude const &lat)
     382                 :            : {
     383                 :        100 :   double lat_rad = toRadians(lat);
     384                 :        100 :   double cos_lat = std::cos(lat_rad);
     385                 :        100 :   double sin_lat = std::sin(lat_rad);
     386                 :        100 :   double t1 = A * A * cos_lat;
     387                 :        100 :   double t2 = B * B * sin_lat;
     388                 :        100 :   double t3 = A * cos_lat;
     389                 :        100 :   double t4 = B * sin_lat;
     390                 :        100 :   double d = std::sqrt((t1 * t1 + t2 * t2) / (t3 * t3 + t4 * t4));
     391                 :        100 :   return physics::Distance(d);
     392                 :            : }
     393                 :            : 
     394                 :        100 : double CoordinateTransform::geocentricLatitude(Latitude const &lat)
     395                 :            : {
     396                 :        100 :   double lat_rad = toRadians(lat);
     397                 :        100 :   double gclat = std::atan((1.0 - E2) * std::tan(lat_rad));
     398                 :        100 :   return gclat;
     399                 :            : }
     400                 :            : 
     401                 :            : //////////////
     402                 :            : // Static data
     403                 :            : 
     404                 :            : size_t CoordinateTransform::instance_counter_ = 0;
     405                 :            : 
     406                 :            : } // namespace point
     407                 :            : } // namespace map
     408                 :            : } // namespace ad