Merge remote-tracking branch 'origin/dev'

cloud_common/utility/station.cpp

+#include <ogr_spatialref.h>
+#include "station.h"
+
+namespace jf {
+  Eigen::Matrix3d Station::CalcRtStationToEpsg4978(double lon, double lat)
+  { 
+    double radianL = lon;
+    Wgs84::Instance().Degree2Rad(&radianL);
+
+    double radianB = lat;
+    Wgs84::Instance().Degree2Rad(&radianB);
+
+    double sinB = std::sin(radianB);
+    double cosB = std::cos(radianB);
+    double sinL = std::sin(radianL);
+    double cosL = std::cos(radianL);
+
+    Eigen::Matrix3d result_m;
+    result_m <<
+      -sinL, -sinB * cosL, cosB* cosL,
+      cosL, -sinB * sinL, cosB* sinL,
+      0, cosB, sinB;
+    return result_m;
+  }
+
+  Eigen::Vector3d Station::CalcTranslateStationToEpsg4978(double lon, double lat, double height)
+  { 
+    double xyz[3]{ lat,lon,height };
+    Wgs84::Instance().BLH2ECEF(&xyz[0], &xyz[1], &xyz[2]);
+
+    return Eigen::Vector3d(xyz[1], xyz[0], xyz[2]);
+  }
+
+  Station::Station(double lon, double lat, double high):lon_(lon), lat_(lat), height_(high) 
+  {
+      Eigen::Matrix3d rref = CalcRtStationToEpsg4978(lon, lat);
+    Eigen::Vector3d tref = CalcTranslateStationToEpsg4978(lon, lat, high);
+
+    r_enutoepsg_ = rref;
+    t_enutoepsg_ = tref;
+
+    rt_enutoepsg_ <<
+      rref(0, 0), rref(0, 1), rref(0, 2), tref.x(),
+      rref(1, 0), rref(1, 1), rref(1, 2), tref.y(),
+      rref(2, 0), rref(2, 1), rref(2, 2), tref.z(),
+      0.0, 0.0, 0.0, 1.0;
+
+    rt_enutoepsg_inverse_ = rt_enutoepsg_.inverse();
+  }
+
+  bool Station::TransToEpsg4978(double* e2x, double* n2y, double* u2z, int count)
+  {
+    for (int i = 0; i < count; ++i)
+    { 
+      Eigen::Vector4d enuhomo(e2x[i], n2y[i], u2z[i], 1.0);
+      // auto res = rt_enutoepsg_ * enuhomo;
+      // e2x[i] = res.x();
+      // n2y[i] = res.y();
+      // u2z[i] = res.z();
+      double x = rt_enutoepsg_(0,0)*e2x[i] + rt_enutoepsg_(0,1)*e2x[i] + rt_enutoepsg_(0,2)*e2x[i] + rt_enutoepsg_(0,3);
+      double y = rt_enutoepsg_(1,0)*n2y[i] + rt_enutoepsg_(1,1)*n2y[i] + rt_enutoepsg_(1,2)*n2y[i] + rt_enutoepsg_(1,3);
+      double z = rt_enutoepsg_(2,0)*u2z[i] + rt_enutoepsg_(2,1)*u2z[i] + rt_enutoepsg_(2,2)*u2z[i] + rt_enutoepsg_(2,3);
+      e2x[i] = x;
+      n2y[i] = y;
+      u2z[i] = z;
+    }
+
+    return true;
+  }
+
+  bool Station::TransFromEpsg4978(double* x2e, double* y2n, double* z2u, int count)
+  { 
+    for (int i = 0; i < count; ++i)
+    { 
+      Eigen::Vector4d enuhomo(x2e[i], y2n[i], z2u[i], 1.0);
+      auto res = rt_enutoepsg_inverse_ * enuhomo;
+      x2e[i] = res.x();
+      y2n[i] = res.y();
+      z2u[i] = res.z();
+    }
+
+    return true;
+  }
+
+  bool Station::TransToEpsg4326(double* e2lon, double* n2lat, double* u2h, int count)
+  { 
+    for (int i = 0; i < count; ++i)
+    { 
+      Eigen::Vector4d enuhomo(e2lon[i], n2lat[i], u2h[i], 1.0);
+      auto res = rt_enutoepsg_ * enuhomo;
+
+      e2lon[i] = res.x();
+      n2lat[i] = res.y();
+      u2h[i] = res.z();
+
+      Wgs84::Instance().ECEF2BLH(&(e2lon[i]), &(n2lat[i]), &(u2h[i]));
+    }
+
+    return true;
+  }
+
+  bool Station::TransFromEpsg4326(double* lon2e, double* lat2n, double* h2u, int count)
+  { 
+    for (int i = 0; i < count; ++i)
+    { 
+      double x = lon2e[i];
+      double y = lat2n[i];
+      double z = h2u[i];
+      Wgs84::Instance().BLH2ECEF(&y, &x, &z);
+
+//      Eigen::Vector4d epsg4978homo(x, y, z, 1.0);
+//      Eigen::Vector4d res = rt_enutoepsg_inverse_ * epsg4978homo;
+//      lon2e[i] = res.x();
+//      lat2n[i] = res.y();
+//      h2u[i] = res.z();
+
+      double x1 = rt_enutoepsg_inverse_(0,0)*x + rt_enutoepsg_inverse_(0,1)*y + rt_enutoepsg_inverse_(0,2)*z + rt_enutoepsg_inverse_(0,3);
+      double y1 = rt_enutoepsg_inverse_(1,0)*x + rt_enutoepsg_inverse_(1,1)*y + rt_enutoepsg_inverse_(1,2)*z + rt_enutoepsg_inverse_(1,3);
+      double z1 = rt_enutoepsg_inverse_(2,0)*x + rt_enutoepsg_inverse_(2,1)*y + rt_enutoepsg_inverse_(2,2)*z + rt_enutoepsg_inverse_(2,3);
+
+      lon2e[i] = x1;
+      lat2n[i] = y1;
+      h2u[i] = z1;
+    }
+
+    return true;
+  }
+
+  bool Station::EnuToECEF(double* x, double* y, double* z, int count)
+  {
+    for (int i = 0; i < count; ++i)
+    { 
+      Eigen::Vector4d enuhomo(x[i], y[i], z[i], 1.0);
+      auto res = rt_enutoepsg_ * enuhomo;
+
+      x[i] = res.x();
+      y[i] = res.y();
+      z[i] = res.z();
+    }
+
+    return true;
+  }
+
+  bool Station::ECEFToWGS84(double* x, double* y, double* z, int count)
+  {
+    for (int i = 0; i < count; ++i)
+      Wgs84::Instance().ECEF2BLH(&(x[i]), &(y[i]), &(z[i]));
+    return true;
+  }
+
+  bool Station::WGS84ToECEF(double* x, double* y, double* z, int count)
+  {
+    for (int i = 0; i < count; ++i)
+      Wgs84::Instance().BLH2ECEF(&(x[i]), &(y[i]), &(z[i]));
+    return true;
+  }
+
+  void Station::resetNEH() {
+    OGRSpatialReference* res4326 = OGRSpatialReference::GetWGS84SRS();
+    OGRSpatialReference* resutm51 = new OGRSpatialReference();
+
+    int zone_code = 31 + std::floor(lon_ / 6.0);
+    resutm51->SetUTM(zone_code, 1);
+
+    OGRCoordinateTransformation* trans = OGRCreateCoordinateTransformation(res4326, resutm51);
+    double E0 = lon_;
+    double N0 = lat_;
+    double H0 = height_;
+    int nCount = 1;
+    trans->Transform(nCount, &E0, &N0, &H0);
+    UTM_E0_ = floor(E0);
+    UTM_N0_ = floor(N0);
+    UTM_H0_ = 0.0;
+    OGRCoordinateTransformation::DestroyCT(trans);
+    trans = nullptr;
+  }
+}

cloud_common/utility/station.h

+#ifndef JF_HD_DEVICE_DEVICES_STATION_H_
+#define JF_HD_DEVICE_DEVICES_STATION_H_
+// #include <device/export.h>
+// #include "device.h"
+
+#include <Eigen/Dense>
+#include <memory>
+#include "wgs84.h"
+
+namespace jf {
+
+  /**
+  站心坐标系的站心， EPSG:4326 的经纬度坐标，单位度，高，单位米
+  Enu 东北天（xyz) 的站心坐标系， 右手系
+  */
+  class Station
+  {
+  public:
+    explicit Station (double lon, double lat, double high);
+    Eigen::Matrix3d CalcRtStationToEpsg4978(double lon, double lat);
+    Eigen::Vector3d CalcTranslateStationToEpsg4978(double lon, double lat, double height);
+
+    bool TransToEpsg4978(double* e2x, double* n2y, double* u2z, int count = 1);
+    bool TransFromEpsg4978(double* x2e, double* y2n, double* z2u, int count = 1);
+
+    bool TransToEpsg4326(double* e2lon, double* n2lat, double* u2h, int count = 1);
+    bool TransFromEpsg4326(double* lon2e, double* lat2n, double* h2u, int count = 1);
+    bool EnuToECEF(double* x, double* y, double* z, int count = 1);
+    bool ECEFToWGS84(double* x, double* y, double* z, int count = 1);
+    bool WGS84ToECEF(double* x, double* y, double* z, int count = 1);
+
+    double lon() {return lon_;}
+    double lat() {return lat_;}
+    double height() {return height_;}
+
+    double UTM_E0() {return UTM_E0_;}
+    double UTM_N0() {return UTM_N0_;}
+    double UTM_H0() {return UTM_H0_;}
+
+    void resetNEH();
+
+    void setENH(double E0, double N0, double H0) {
+        UTM_E0_ = E0;
+        UTM_N0_ = N0;
+        UTM_H0_ = H0;
+    }
+
+    int getSRID() {
+        // 根据经纬度计算utm 6度分带的代号
+        //int zone_code = 31 + math.floor(lon / 6.0)
+        int zone_code = 31 + std::floor(lon_ / 6.0);
+
+        // 南半球的EPSG SRID
+        // int zone_code_base = 32600;
+        // if (lat < 0.0 && lat >= -80.0)
+        // {
+        //   zone_code_base = 32700;
+        // }
+        // else if (lat >= 0.0 && lat <= 84.0)
+        // {
+        //   zone_code_base = 32600;
+        // }
+        // else
+        //   return false;
+
+        // *srid = zone_code + zone_code_base;
+        // return true;
+        return zone_code;
+    }
+
+  private:
+    double lon_;
+    double lat_;
+    double height_;
+    double UTM_E0_;
+    double UTM_N0_;
+    double UTM_H0_;
+
+    Eigen::Matrix3d r_enutoepsg_;
+    Eigen::Vector3d t_enutoepsg_;
+
+    Eigen::Matrix4d rt_enutoepsg_;
+    Eigen::Matrix4d rt_enutoepsg_inverse_;
+  };
+}
+
+#endif // !JF_HD_DEVICE_DEVICES_STATION_H_

cloud_common/utility/wgs84.h

+#ifndef JF_HDMAP_DEVICE_WGS_ECEG_H_
+#define JF_HDMAP_DEVICE_WGS_ECEG_H_
+#include <cmath>
+
+class Wgs84
+{
+public:
+  static Wgs84& Instance();
+
+  /// <summary>
+  /// 获得某纬度处的卯酉圈曲率半径N （70/269）页 （11-13）公式
+  /// </summary>
+  /// <param name="B_radian">纬度值，以弧度位单位</param>
+  /// <returns></returns>
+  double GetN(double B_radian);
+
+  /// <summary>
+  /// 获得某纬度处的子午线弧长，（76/269）页，（11-49）
+  /// </summary>
+  /// <param name="B_radian">纬度值，以弧度为单位</param>
+  /// <returns></returns>
+  double GetX(double B_radian);
+
+
+  /// <summary>
+  /// 由子午圈弧长获得纬度，GetX()方法的反向计算
+  /// （75/269）页，公式（11-46）
+  /// </summary>
+  /// <param name="X">子午圈弧长</param>
+  /// <returns>返回的纬度值以弧度为单位</returns>
+  double GetLatByX(double X);
+
+  /// <summary>
+  /// V为第二基本纬度函数,（38/269）页，（6-6）公式
+  /// </summary>
+  /// <param name="lat">纬度值，以弧度为单位</param>
+  /// <returns></returns>
+  double GetV(double lat);
+
+  //List<GeoPoint> ECEFList2BLH(IReadOnlyList<PointECEF> ecefs);
+
+    // 经纬高转空间直角坐标
+  // b 纬度 l经度 h 高 单位 度 度 米
+  void BLH2ECEF(double* b2y, double* l2x, double* h2z);
+
+  /// 空间直角坐标转经纬高
+  // 单位米
+  void ECEF2BLH(double* x2l, double* y2b, double* z2h);
+
+       
+  //获取给定距离对应的角度差，大略计算，距离百米以内
+  double GetAngleDif(double distance); 
+
+  void Rad2Degree(double* rad2degree);
+  void Degree2Rad(double* degree2rad);
+
+private: 
+  Wgs84();
+};
+
+#endif// define JF_HDMAP_DEVICE_WGS_ECEG_H_