Skip to content

D
denpends
Commit 32041247 authored by xueyimeng's avatar xueyimeng
Browse files
Options

Merge remote-tracking branch 'origin/dev'

parents a24ce41d 12f13daa
Show whitespace changes
Inline Side-by-side
Showing with 736 additions and 0 deletions
cloud_common/utility/station.cpp 0 → 100644
View file @ 32041247
#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 0 → 100644
View file @ 32041247
#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.cpp 0 → 100644
View file @ 32041247
#include "wgs84.h"
constexpr double pi = 3.1415926535897931l; // C#中的定义
//#define M_PI 3.14159265358979323846 // c++中的定义
//椭球五个基本几何参数定义，见《大地测量学基础.pdf》（37/269）页，（6-3）（6-4）公式
// 引入符号c,t,ita见（37/269）页，（6-5）公式；
//W,V定义见（38/269）页，（6-6）公式
constexpr double a = 6378137.0; //椭球长半轴a
constexpr double f = 0.003352810664; // = 1.0 / 298.257223563;
constexpr double b = 6356752.3142499477; //椭球短轴b _b = _a* (1 - _f);
constexpr double c = 6399593.6257536924;//c是为简化书写引入的符号，c = a*a/b;
constexpr double e2 = 0.006694379988651241; // 第一偏心率平方 e2 = e*e = (a*a -b*b)/a*a
constexpr double ep2 = 0.0067394967407662064; //第二偏心率平方 ep2 = ep * ep = (a*a - b*b)/b*b
//#region 一些提前计算的量
//m0~m8的定义，（71/269）页，（11-21）,以下常量值为带入wgs84椭球参数之后计算所得
constexpr double m0 = 6335439.3273023246;//m0 = _a* (1 - _e2);
constexpr double m2 = 63617.757378010137;//m2 = _e2* m0 * 1.5
constexpr double m4 = 532.35180239277622;//m4 = 1.25 * _e2* m2
constexpr double m6 = 4.1577261283373916;//m6 = _e2* m4 * 7.0 / 6.0
constexpr double m8 = 0.031312573415813519;//m8 = _e2* m6 * 9.0 / 8.0
//a0~a8的定义，（75/269）页，（11-45）
constexpr double a0 = 6367449.1457686741;//a0 = m0 + m2 / 2.0 + m4* 3.0 / 8.0 + m6* 5.0 / 16.0 + m8* 35.0 / 128.0;
constexpr double a2 = 32077.017223574985;//a2 = (m2 + m4) / 2.0 + m6* 15.0 / 32.0 + m8* 7.0 / 16.0;
constexpr double a4 = 67.330398573595;//a4 = m4 / 8.0 + m6* 3.0 / 16.0 + m8* 7.0 / 32.0;
constexpr double a6 = 0.13188597734903185;//a6 = m6 / 32.0 + m8 / 16.0;
constexpr double a8 = 0.00024462947981104312;//a8 = m8 / 128.0;
//namespace HDMapBase.Geometry
//{
// /// <summary>
// /// WGS84椭球定义
// /// </summary>
// public static class Wgs84
// {
// //椭球五个基本几何参数定义，见《大地测量学基础.pdf》（37/269）页，（6-3）（6-4）公式
// // 引入符号c,t,ita见（37/269）页，（6-5）公式；
// //W,V定义见（38/269）页，（6-6）公式
// public const double a = 6378137.0; //椭球长半轴a
// public const double f = 0.003352810664; // = 1.0 / 298.257223563;
// public const double b = 6356752.3142499477; //椭球短轴b _b = _a* (1 - _f);
// public const double c = 6399593.6257536924;//c是为简化书写引入的符号，c = a*a/b;
// public const double e2 = 0.006694379988651241; // 第一偏心率平方 e2 = e*e = (a*a -b*b)/a*a
// public const double ep2 = 0.0067394967407662064; //第二偏心率平方 ep2 = ep * ep = (a*a - b*b)/b*b
//
// #region 一些提前计算的量
// //m0~m8的定义，（71/269）页，（11-21）,以下常量值为带入wgs84椭球参数之后计算所得
// private const double m0 = 6335439.3273023246;//m0 = _a* (1 - _e2);
// private const double m2 = 63617.757378010137;//m2 = _e2* m0 * 1.5
// private const double m4 = 532.35180239277622;//m4 = 1.25 * _e2* m2
// private const double m6 = 4.1577261283373916;//m6 = _e2* m4 * 7.0 / 6.0
// private const double m8 = 0.031312573415813519;//m8 = _e2* m6 * 9.0 / 8.0
//
// //a0~a8的定义，（75/269）页，（11-45）
// private const double a0 = 6367449.1457686741;//a0 = m0 + m2 / 2.0 + m4* 3.0 / 8.0 + m6* 5.0 / 16.0 + m8* 35.0 / 128.0;
// private const double a2 = 32077.017223574985;//a2 = (m2 + m4) / 2.0 + m6* 15.0 / 32.0 + m8* 7.0 / 16.0;
// private const double a4 = 67.330398573595;//a4 = m4 / 8.0 + m6* 3.0 / 16.0 + m8* 7.0 / 32.0;
// private const double a6 = 0.13188597734903185;//a6 = m6 / 32.0 + m8 / 16.0;
// private const double a8 = 0.00024462947981104312;//a8 = m8 / 128.0;
// #endregion
//
// /// <summary>
// /// 获得某纬度处的卯酉圈曲率半径N （70/269）页 （11-13）公式
// /// </summary>
// /// <param name="B_radian">纬度值，以弧度位单位</param>
// /// <returns></returns>
// public static double GetN(double B_radian)
// {
// double cosB = Math.Cos(B_radian);
// double V = Math.Sqrt(1 + ep2 * cosB * cosB);
// double N = c / V;
// return N;
// }
//
// /// <summary>
// /// 获得某纬度处的子午线弧长，（76/269）页，（11-49）
// /// </summary>
// /// <param name="B_radian">纬度值，以弧度为单位</param>
// /// <returns></returns>
// public static double GetX(double B_radian)
// {
// double sinB = Math.Sin(B_radian);
// double cosB = Math.Cos(B_radian);
// double al = a0 * B_radian;
// double sc = sinB * cosB;
// double ss = sinB * sinB;
//
// double X = al - sc * ((a2 - a4 + a6) + (2 * a4 - a6 * 16 / 3) * ss + a6 * 16 * ss * ss / 3);
// return X;
// }
//
// /// <summary>
// /// 由子午圈弧长获得纬度，GetX()方法的反向计算
// /// （75/269）页，公式（11-46）
// /// </summary>
// /// <param name="X">子午圈弧长</param>
// /// <returns>返回的纬度值以弧度为单位</returns>
// public static double GetLatByX(double X)
// {
// double Bf, Bfi0, Bfi1, FBfi, sinB,sinB2, cosB;
//
// Bfi0 = X / a0;
// Bfi1 = 0;
// FBfi = 0;
// int num = 0;
// double minAngle = Math.PI * 1e-9 ;//最小角度差值，小于此值跳出循环
// double menus_minAngle = 0.0 - minAngle;
// double deltaB = 0;//纬度差值
// do
// {
// num = 0;
// //参照（11-49）
// sinB = Math.Sin(Bfi0);
// sinB2 = sinB * sinB;
// cosB = Math.Cos(Bfi0);
// //参照(11-46)公式的运算
// FBfi = 0.0 - sinB * cosB * ((a2 - a4 + a6) + sinB2 * (2 * a4 - 16 * a6 / 3) + sinB2 * sinB2 * a6 * 16 / 3);
// Bfi1 = (X - FBfi) / a0;
//
// deltaB = Bfi1 - Bfi0;
// if (deltaB < menus_minAngle || deltaB > minAngle)
// {
// num = 1;
// Bfi0 = Bfi1;
// }
// } while (num == 1);
// Bf = Bfi1;
// return Bf;
// }
//
// /// <summary>
// /// V为第二基本纬度函数,（38/269）页，（6-6）公式
// /// </summary>
// /// <param name="lat">纬度值，以弧度为单位</param>
// /// <returns></returns>
// public static double GetV(double lat)
// {
// double cosB = Math.Cos(lat);
// double V = Math.Sqrt(1 + ep2 * cosB * cosB);
// return V;
// }
//
// public static List<GeoPoint> ECEFList2BLH(IReadOnlyList<PointECEF> ecefs)
// {
// List<GeoPoint> res = new List<GeoPoint>();
// foreach(PointECEF pt in ecefs)
// {
// res.Add(ECEF2BLH(pt));
// }
// return res;
// }
// // 经纬高转空间直角坐标
// public static PointECEF BLH2ECEF(GeoPoint ptBLH)
// {
// double cosB = Math.Cos(ptBLH.radianB);
// double sinB = Math.Sin(ptBLH.radianB);
// double cosL = Math.Cos(ptBLH.radianL);
// double sinL = Math.Sin(ptBLH.radianL);
//
// double N = GetN(ptBLH.radianB);
//
// double X = (N + ptBLH.H) * cosB * cosL;
// double Y = (N + ptBLH.H) * cosB * sinL;
// double Z = (N * (1 - e2) + ptBLH.H) * sinB;
//
// PointECEF ptXYZ = new PointECEF(X, Y, Z);
// return ptXYZ;
// }
//
// public static PointECEF BLH2ECEF(double L, double B, double H) => BLH2ECEF(new GeoPoint(L, B, H));
// /// <summary>
// /// 空间直角坐标转经纬高
// /// </summary>
// /// <param name="ptXYZ"></param>
// /// <returns></returns>
// public static GeoPoint ECEF2BLH(PointECEF ptXYZ)
// {
// //大地测量学基础 42/269页，公式（6-33）
// double L_radian = Math.Atan(ptXYZ.y / ptXYZ.x);
// if(L_radian < 0)
// {
// L_radian += Math.PI;
// }
// double L = GisUtil.Radian2Degree(L_radian);
// double sqrtXY = Math.Sqrt(ptXYZ.x * ptXYZ.x + ptXYZ.y * ptXYZ.y);
// //大地测量学基础 42/269页，公式（6-42）
// double t0 = ptXYZ.z / sqrtXY;
// double p = c * e2 / sqrtXY;
// double k = 1 + ep2;
//
// //迭代计算纬度
// double ti = t0;
// double ti1 = 0;
// int loop = 0;
// while (loop < 10000)
// {
// loop++;
// ti1 = t0 + p * ti / Math.Sqrt(k + ti * ti);
// if (Math.Abs(ti1 - ti) < 1e-12)
// {
// break;
// }
// ti = ti1;
// }
// double B_radian = Math.Atan(ti1);
// double N = GetN(B_radian);
// double H = sqrtXY / Math.Cos(B_radian) - N;
//
// double B = GisUtil.Radian2Degree(B_radian);
//
// return new GeoPoint(L, B, H);
// }
//
// //获取给定距离对应的角度差，大略计算，距离百米以内
// public static double GetAngleDif(double distance)
// {
// double angleRadian = distance / a;
// return angleRadian * 180.0 / Math.PI;
// }
//
// #region 将来可能会用的方法
// //当前暂时不用，放这里备着
// ///// <summary>
// ///// 获得某纬度处的子午圈曲率半径M,(69/269)页，（11-9）公式
// ///// </summary>
// ///// <param name="lat">纬度值，以弧度为单位</param>
// ///// <returns></returns>
// //public static double GetM(double lat)
// //{
// // double W = GetW(lat);
// // double M = a * (1 - e2) / (W * W * W);
// // return M;
// //}
// ///// <summary>
// ///// W为第一基本纬度函数,（38/269）页，（6-6）公式
// ///// </summary>
// ///// <param name="lat">纬度值，以弧度为单位</param>
// ///// <returns></returns>
// //public static double GetW(double lat)
// //{
// // double sinB = Math.Sin(lat);
// // double W = Math.Sqrt(1 - e2 * sinB * sinB);
// // return W;
// //}
// #endregion
// }
//}
Wgs84& Wgs84::Instance()
{
static Wgs84 ins;
return ins;
}
Wgs84::Wgs84()
{
}
double Wgs84::GetN(double B_radian)
{
double cosB = std::cos(B_radian);
double V = std::sqrt(1.0l + ep2 * cosB * cosB);
double N = c / V;
return N;
}
double Wgs84::GetX(double B_radian)
{
double sinB = std::sin(B_radian);
double cosB = std::cos(B_radian);
double al = a0 * B_radian;
double sc = sinB * cosB;
double ss = sinB * sinB;
double X = al - sc * ((a2 - a4 + a6) + (2 * a4 - a6 * 16 / 3) * ss + a6 * 16 * ss * ss / 3);
return X;
}
double Wgs84::GetLatByX(double X)
{
double Bf, Bfi0, Bfi1, FBfi, sinB, sinB2, cosB;
Bfi0 = X / a0;
Bfi1 = 0;
FBfi = 0;
int num = 0;
double minAngle = pi * 1e-9;//最小角度差值，小于此值跳出循环
double menus_minAngle = 0.0 - minAngle;
double deltaB = 0;//纬度差值
do
{
num = 0;
//参照（11-49）
//sinB = Math.Sin(Bfi0);
sinB = std::sin(Bfi0);
sinB2 = sinB * sinB;
//cosB = Math.Cos(Bfi0);
cosB = std::cos(Bfi0);
//参照(11-46)公式的运算
FBfi = 0.0 - sinB * cosB * ((a2 - a4 + a6) + sinB2 * (2 * a4 - 16 * a6 / 3) + sinB2 * sinB2 * a6 * 16 / 3);
Bfi1 = (X - FBfi) / a0;
deltaB = Bfi1 - Bfi0;
if (deltaB < menus_minAngle || deltaB > minAngle)
{
num = 1;
Bfi0 = Bfi1;
}
} while (num == 1);
Bf = Bfi1;
return Bf;
}
double Wgs84::GetV(double lat)
{
double cosB = std::cos(lat);
double V = std::sqrt(1.0l + ep2 * cosB * cosB);
return V;
}
void Wgs84::BLH2ECEF(double* b2y, double* l2x, double* h2z)
{
double rad_b = (*b2y) * pi / 180.0l;
double rad_l = (*l2x) * pi / 180.0l;
double h = *h2z;
double cosB = std::cos(rad_b);
double sinB = std::sin(rad_b);
double cosL = std::cos(rad_l);
double sinL = std::sin(rad_l);
double N = GetN(rad_b);
//double X = (N + h) * cosB * cosL;
//double Y = (N + h) * cosB * sinL;
//double Z = (N * (1 - e2) + h) * sinB;
*l2x = (N + h) * cosB * cosL;
*b2y = (N + h) * cosB * sinL;
*h2z = (N * (1 - e2) + h) * sinB;
}
void Wgs84::ECEF2BLH(double* x2l, double* y2b, double* z2h)
{
//大地测量学基础 42/269页，公式（6-33）
//double L_radian = Math.Atan(ptXYZ.y / ptXYZ.x);
double L_radian = std::atan(*y2b / *x2l);
if (L_radian < 0)
{
L_radian += pi;
}
//double L = GisUtil.Radian2Degree(L_radian);
double L = L_radian;
Rad2Degree(&L);
//double sqrtXY = Math.Sqrt(ptXYZ.x * ptXYZ.x + ptXYZ.y * ptXYZ.y);
double sqrtXY = std::sqrt((*x2l) * (*x2l) + (*y2b) * (*y2b));
//大地测量学基础 42/269页，公式（6-42）
//double t0 = ptXYZ.z / sqrtXY;
double t0 = (*z2h) / sqrtXY;
double p = c * e2 / sqrtXY;
double k = 1 + ep2;
//迭代计算纬度
double ti = t0;
double ti1 = 0;
int loop = 0;
while (loop < 10000)
{
loop++;
//ti1 = t0 + p * ti / Math.Sqrt(k + ti * ti);
ti1 = t0 + p * ti / std::sqrt(k + ti * ti);
//if (Math.Abs(ti1 - ti) < 1e-12)
if (std::abs(ti1 - ti) < 1e-12)
{
break;
}
ti = ti1;
}
//double B_radian = Math.Atan(ti1);
double B_radian = std::atan(ti1);
double N = GetN(B_radian);
//double H = sqrtXY / Math.Cos(B_radian) - N;
double H = sqrtXY / std::cos(B_radian) - N;
double B = B_radian;
Rad2Degree(&B);
*x2l = L;
*y2b = B;
*z2h = H;
}
double Wgs84::GetAngleDif(double distance)
{
double angleRadian = distance / a;
return angleRadian * 180.0 / pi;
}
void Wgs84::Rad2Degree(double* rad2degree)
{
*rad2degree = (*rad2degree) * 180.0l / pi;
}
void Wgs84::Degree2Rad(double* degree2rad)
{
*degree2rad = (*degree2rad) * pi / 180.0l;
}
cloud_common/utility/wgs84.h 0 → 100644
View file @ 32041247
#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_
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment