Commit cb134b4f authored by Cédric Traizet's avatar Cédric Traizet
Browse files

Merge branch 'world_to_ecef_free_function' into 'develop'

WorldToEcef and EcefToWorld free functions

See merge request !873
parents 2f730be6 44bd316e
Pipeline #9277 failed with stages
in 74 minutes and 5 seconds
......@@ -66,21 +66,61 @@ public:
OutputPointType TransformPoint(const InputPointType& point) const override;
protected:
GeocentricTransform();
GeocentricTransform() : Superclass(ParametersDimension) {};
~GeocentricTransform() override = default;
private:
GeocentricTransform(const Self&) = delete;
void operator=(const Self&) = delete;
};
std::unique_ptr<CoordinateTransformation> m_MapProjection;
double m_a;
double m_b;
double m_f;
double m_es;
namespace Projection
{
/** \struct WGS84Ellipsoid
*
* \brief a structure holding the ellipsoid parameters for WGS84
*
* \ingroup OTBTransform
*
*/
template <class TScalarType = double>
struct WGS84Ellipsoid
{
/** Semi-major axis a */
static constexpr TScalarType a = 6378137.;
/** Semi-major axis b */
static constexpr TScalarType b = 6356752.314245;
/** flattening */
static constexpr TScalarType f = (a - b) / a;
/** first eccentricity squared */
static constexpr TScalarType es = 1 - (b * b) / (a * a);
};
/** \fn WorldToEcef
*
* \brief convert from geographic (lon, lat, height) to ECEF coordinates (x, y, z)
*
* \param[in] ecefPoint : the input geographic point
*
* \ingroup OTBTransform
*
*/
template <class TScalarType = double, class TEllipsoid = WGS84Ellipsoid<TScalarType>>
itk::Point<TScalarType, 3> WorldToEcef(const itk::Point<TScalarType, 3> & worldPoint);
/** \fn EcefToWorld
*
* \brief convert from ECEF (x, y, z) to geographic coordinates (lon, lat, height)
*
* \param[in] ecefPoint : the input ECEF point
*
* \ingroup OTBTransform
*/
template <class TScalarType = double, class TEllipsoid = WGS84Ellipsoid<TScalarType>>
itk::Point<TScalarType, 3> EcefToWorld(const itk::Point<TScalarType, 3> & ecefPoint);
} // namespace Projection
} // namespace otb
#ifndef OTB_MANUAL_INSTANTIATION
......
......@@ -28,110 +28,109 @@ namespace otb
{
template <TransformDirection TDirectionOfMapping, class TScalarType>
GeocentricTransform<TDirectionOfMapping, TScalarType>::GeocentricTransform() : Superclass(ParametersDimension)
typename GeocentricTransform<TDirectionOfMapping, TScalarType>::OutputPointType
GeocentricTransform<TDirectionOfMapping, TScalarType>::TransformPoint(const InputPointType& point) const
{
SpatialReference wgs84 = SpatialReference::FromWGS84();
SpatialReference ecefSpatialReference = SpatialReference::FromDescription("EPSG:4978");
#if GDAL_VERSION_NUM >= 3000000
wgs84.SetAxisMappingStrategy(OAMS_TRADITIONAL_GIS_ORDER);
ecefSpatialReference.SetAxisMappingStrategy(OAMS_TRADITIONAL_GIS_ORDER);
#endif
if (DirectionOfMapping == TransformDirection::INVERSE)
// Geographic to geocentric
if (DirectionOfMapping == TransformDirection::FORWARD)
{
std::unique_ptr<CoordinateTransformation> newMapProjection(new CoordinateTransformation(ecefSpatialReference, wgs84));
if (newMapProjection)
m_MapProjection.swap(newMapProjection);
return Projection::WorldToEcef(point);
}
// Geocentric to geographic
else
{
std::unique_ptr<CoordinateTransformation> newMapProjection(new CoordinateTransformation(wgs84, ecefSpatialReference));
if (newMapProjection)
m_MapProjection.swap(newMapProjection);
return Projection::EcefToWorld(point);
}
}
namespace Projection
{
template <class TScalarType, class TEllipsoid>
itk::Point<TScalarType, 3> WorldToEcef(const itk::Point<TScalarType, 3> & worldPoint)
{
itk::Point<TScalarType, 3> ecefPoint;
const auto lon = worldPoint[0]* CONST_PI_180;
const auto lat = worldPoint[1]* CONST_PI_180;
const auto height = worldPoint[2];
auto sin_latitude = std::sin(lat);
auto cos_latitude = std::cos(lat);
auto N = TEllipsoid::a / sqrt( 1.0 - TEllipsoid::es * sin_latitude * sin_latitude);
ecefPoint[0] = (N + height) * cos_latitude * cos(lon);
ecefPoint[1] = (N + height) * cos_latitude * sin(lon);
ecefPoint[2] = (N * (1 - TEllipsoid::es) + height) * sin_latitude;
m_a = 6378137.;
m_b = 6356752.314245;
m_f = (m_a - m_b)/m_a;
const double e = std::sqrt(1 - (m_b * m_b) / (m_a * m_a));
m_es = e * e;
return ecefPoint;
}
template <TransformDirection TDirectionOfMapping, class TScalarType>
typename GeocentricTransform<TDirectionOfMapping, TScalarType>::OutputPointType
GeocentricTransform<TDirectionOfMapping, TScalarType>::TransformPoint(const InputPointType& point) const
template <class TScalarType, class TEllipsoid>
itk::Point<TScalarType, 3> EcefToWorld(const itk::Point<TScalarType, 3> & ecefPoint)
{
OutputPointType outputPoint;
itk::Point<TScalarType, 3> worldPoint;
// Geographic to geocentric
if (DirectionOfMapping == TransformDirection::FORWARD)
{
const auto lon = point[0]* CONST_PI_180;
const auto lat = point[1]* CONST_PI_180;
const auto height = point[2];
auto sin_latitude = std::sin(lat);
auto cos_latitude = std::cos(lat);
auto N = m_a / sqrt( 1.0 - m_es * sin_latitude * sin_latitude);
outputPoint[0] = (N + height) * cos_latitude * cos(lon);
outputPoint[1] = (N + height) * cos_latitude * sin(lon);
outputPoint[2] = (N * (1 - m_es) + height) * sin_latitude;
}
// Geocentric to geographic
else
{
const auto & x = point[0];
const auto & y = point[1];
const auto & z = point[2];
const auto x = ecefPoint[0];
const auto y = ecefPoint[1];
const auto z = ecefPoint[2];
const TScalarType tol = 1e-15;
const TScalarType d2 = x*x + y*y;
const TScalarType d = sqrt(d2);
const int MAX_ITER = 10;
const double tol = 1e-15;
const double d = sqrt(x*x + y*y);
const int MAX_ITER = 10;
constexpr TScalarType a2 = TEllipsoid::a * TEllipsoid::a;
constexpr TScalarType b2 = TEllipsoid::b * TEllipsoid::b;
const TScalarType pa2 = d2 * a2;
const TScalarType qb2 = z * z * b2;
constexpr TScalarType ae2 = a2 * TEllipsoid::es;
constexpr TScalarType ae4 = ae2 * ae2;
const double a2 = m_a * m_a;
const double b2 = m_b * m_b;
const double pa2 = d * d * a2;
const double qb2 = z * z * b2;
const double ae2 = a2 * m_es;
const double ae4 = ae2 * ae2;
const TScalarType c3 = -( ae4/2 + pa2 + qb2 ); // s^2
const TScalarType c4 = ae2*( pa2 - qb2 ); // s^1
const TScalarType c5 = ae4/4 * ( ae4/4 - pa2 - qb2 ); // s^0
const double c3 = -( ae4/2 + pa2 + qb2 ); // s^2
const double c4 = ae2*( pa2 - qb2 ); // s^1
const double c5 = ae4/4 * ( ae4/4 - pa2 - qb2 ); // s^0
TScalarType s0 = 0.5 * (a2 + b2) * std::sqrt(d/TEllipsoid::a * d/TEllipsoid::a + z/TEllipsoid::b * z/TEllipsoid::b);
for (int iterIdx = 0; iterIdx < MAX_ITER; ++iterIdx)
{
const TScalarType pol = c5 + s0 * ( c4 + s0 * ( c3 + s0 * s0 ) );
const TScalarType der = c4 + s0 * ( 2 * c3 + 4 * s0 * s0 );
double s0 = 0.5 * (a2 + b2) * std::hypot( d/m_a, z/m_b );
const TScalarType ds = - pol / der;
s0 += ds;
for( int iterIdx = 0; iterIdx < MAX_ITER; ++iterIdx )
if (std::abs(ds) < tol * s0)
{
const double pol = c5 + s0 * ( c4 + s0 * ( c3 + s0 * s0 ) );
const double der = c4 + s0 * ( 2 * c3 + 4 * s0 * s0 );
constexpr TScalarType half = 0.5;
constexpr TScalarType one = 1.0;
const double ds = - pol / der;
s0 += ds;
const auto t = s0 - half * (a2 + b2);
const auto x_ell = d / (one + t/a2);
const auto y_ell = z / (one + t/b2);
if( fabs( ds ) < tol * s0 )
{
const double t = s0 - 0.5 * (a2 + b2);
const double x_ell = d / ( 1.0 + t/a2 );
const double y_ell = z / ( 1.0 + t/b2 );
auto const xea2 = x_ell / a2;
auto const yeb2 = y_ell / b2;
double height = ( d - x_ell ) * x_ell/a2 + ( z - y_ell ) * y_ell/b2;
height /= std::hypot( x_ell/a2 , y_ell/b2 );
auto height = (d - x_ell) * xea2 + (z - y_ell) * yeb2;
height /= std::sqrt(xea2 * xea2 + yeb2 * yeb2);
//lat
outputPoint[1] = std::atan2( y_ell/b2, x_ell/a2 ) * CONST_180_PI;
//lat
worldPoint[1] = std::atan2(yeb2, xea2) * CONST_180_PI;
//lon
outputPoint[0] = std::atan2( y, x ) * CONST_180_PI;
outputPoint[2] = height;
}
//lon
worldPoint[0] = std::atan2(y, x) * CONST_180_PI;
worldPoint[2] = height;
}
}
return outputPoint;
return worldPoint;
}
} // namespace Projection
} // namespace otb
#endif
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