CoordinateTransform.hpp

Go to the documentation of this file.

// ----------------- BEGIN LICENSE BLOCK ---------------------------------
//
// Copyright (C) 2018-2021 Intel Corporation
//
// SPDX-License-Identifier: MIT
//
// ----------------- END LICENSE BLOCK -----------------------------------
#pragma once
 
#define ACCEPT_USE_OF_DEPRECATED_PROJ_API_H
#include <memory>
#include <proj_api.h>
#include <vector>
#include "ad/map/point/Types.hpp"
 
namespace ad {
namespace map {
namespace point {
 
class CoordinateTransform
{
public:
  typedef std::shared_ptr<CoordinateTransform> Ptr;            
  typedef std::shared_ptr<CoordinateTransform const> ConstPtr; 
 
public: // Constructor/Destructor
  CoordinateTransform();
 
  ~CoordinateTransform();
 
public: // Basic Conversions
public: // ENU Reference Point Managment
  bool setGeoProjection(std::string const &geo_projection);
 
  void setENUReferencePoint(const GeoPoint &enu_ref_point);
 
  const GeoPoint &getENUReferencePoint() const;
 
  bool isENUValid() const;
 
  size_t getENURef() const;
 
public: // ENU/Geo Conversions
  ENUPoint Geo2ENU(const GeoPoint &pt) const;
 
  GeoPoint ENU2Geo(const ENUPoint &pt) const;
 
public: // ECEF/Geo Conversions
  ECEFPoint Geo2ECEF(const GeoPoint &pt) const;
 
  GeoPoint ECEF2Geo(const ECEFPoint &pt) const;
 
public: // ECEF/ENU Conversions
  ECEFPoint ENU2ECEF(const ENUPoint &pt) const;
 
  ENUPoint ECEF2ENU(const ECEFPoint &pt) const;
 
public: // Useful methods
  static physics::Distance WGS84_R(const Latitude &lat);
 
  static double geocentricLatitude(const Latitude &lat);
 
public: // Conversions
  void convert(const GeoPoint &x, ENUPoint &y) const
  {
    y = Geo2ENU(x);
  }
 
  void convert(const ENUPoint &x, GeoPoint &y) const
  {
    y = ENU2Geo(x);
  }
 
  void convert(const GeoPoint &x, ECEFPoint &y) const
  {
    y = Geo2ECEF(x);
  }
 
  void convert(const ECEFPoint &x, GeoPoint &y) const
  {
    y = ECEF2Geo(x);
  }
 
  void convert(const ECEFPoint &x, ENUPoint &y) const
  {
    y = ECEF2ENU(x);
  }
 
  void convert(const ENUPoint &x, ECEFPoint &y) const
  {
    y = ENU2ECEF(x);
  }
 
  template <typename SourceC, typename TargetC>
  void convert(const std::vector<SourceC> &xs, std::vector<TargetC> &ys) const
  {
    ys.clear();
    ys.reserve(xs.size());
    for (auto x : xs)
    {
      TargetC y;
      convert(x, y);
      ys.push_back(y);
    }
  }
 
private:                     // ENU Conversion parameters
  size_t enu_ref_;           
  GeoPoint enu_ref_point_;   
  ECEFPoint ecef_ref_point_; 
  double enu_phi_;           
  double enu_lam_;           
  double enu_h_;             
  double enu_tmp1_;          
  double enu_tmp1_3_;        
  double enu_cp_;            
  double enu_sp_;            
  double enu_cp_2_;          
  double enu_sp_2_;          
  double ecef_enu_[9];       
 
  projPJ projPtr_; 
 
  bool isGeoProjectionValid() const;
 
private:                                              // Constants
  static constexpr double PI = 3.141592654;           
  static constexpr double A = 6378137.0;              
  static constexpr double B = 6356752.3;              
  static constexpr double E2 = 6.6943799901377997e-3; 
  static constexpr double A1 = A * E2;                
  static constexpr double A2 = A1 * A1;               
  static constexpr double A3 = A1 * E2 * 0.5;         
  static constexpr double A4 = 2.5 * A2;              
  static constexpr double A5 = A1 + A3;               
  static constexpr double A6 = 9.9330562000986220e-1; 
 
private:                           // Static Data
  static size_t instance_counter_; 
};
 
} // namespace point
} // namespace map
} // namespace ad