47 #define M_PI 3.14159265358979323846
66 longitude_(longitude),
82 static const double a = 6378137;
83 static const double b = 6356752.3142;
85 static const double ae =
acos(
b/
a);
86 static const double cos2_ae_earth =
square(
cos(ae));
87 static const double sin2_ae_earth =
square(
sin(ae));
114 cv::Point3d P_geocentric_ref =
origin.toGeocentric_WGS84();
120 const cv::Point3d & geocentric_WGS84,
121 const cv::Point3d & origin_geocentric_WGS84,
139 cv::Point3d geocentric_WGS84_rel = geocentric_WGS84-origin_geocentric_WGS84;
144 out.x = -slon*geocentric_WGS84_rel.x + clon*geocentric_WGS84_rel.y;
145 out.y = -clon*slat*geocentric_WGS84_rel.x -slon*slat*geocentric_WGS84_rel.y + clat*geocentric_WGS84_rel.z;
146 out.z = clon*clat*geocentric_WGS84_rel.x + slon*clat*geocentric_WGS84_rel.y +slat*geocentric_WGS84_rel.z;
153 static const double a = 6378137;
154 static const double b = 6356752.3142;
156 const double sa2 =
a*
a;
157 const double sb2 =
b*
b;
159 const double e2 = (sa2 - sb2) / sa2;
160 const double ep2 = (sa2 - sb2) / sb2;
161 const double p =
std::sqrt(geocentric.x * geocentric.x + geocentric.y * geocentric.y);
162 const double theta =
atan2(geocentric.z *
a,
p *
b);
166 geocentric.z + ep2 *
b *
sin(theta) *
sin(theta) *
sin(theta),
167 p - e2 *
a *
cos(theta) *
cos(theta) *
cos(theta));
171 const double N = sa2 /
std::sqrt(sa2 * clat * clat + sb2 * slat * slat);
186 cv::Point3f originGeocentric;
187 originGeocentric =
origin.toGeocentric_WGS84();
189 cv::Vec3d P_ref(originGeocentric.x, originGeocentric.y, originGeocentric.z);
192 cv::Vec3d REF_X, REF_Y, REF_Z;
202 cv::Vec3d AUX_X(-REF_Z[1], REF_Z[0], 0);
206 REF_Y = REF_Z.cross(REF_X);
208 cv::Point3d out_coords;
210 REF_X[0] * enu.x + REF_Y[0] * enu.y + REF_Z[0] * enu.z + originGeocentric.x;
212 REF_X[1] * enu.x + REF_Y[1] * enu.y + REF_Z[1] * enu.z + originGeocentric.y;
214 REF_X[2] * enu.x + REF_Y[2] * enu.y + REF_Z[2] * enu.z + originGeocentric.z;