1 // Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are met:
6 //
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 //
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 //
14 // * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
15 // its contributors may be used to endorse or promote products derived
16 // from this software without specific prior written permission.
17 //
18 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
22 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
23 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
24 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
25 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
26 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
28 // POSSIBILITY OF SUCH DAMAGE.
29 //
30 // Author: Johannes L. Schoenberger (jsch-at-demuc-dot-de)
31
32 #include "base/gps.h"
33
34 #include "util/math.h"
35
36 namespace colmap {
37
GPSTransform(const int ellipsoid)38 GPSTransform::GPSTransform(const int ellipsoid) {
39 switch (ellipsoid) {
40 case GRS80:
41 a_ = 6378137;
42 b_ = 6.356752314140356e+06;
43 f_ = 0.003352810681182;
44 break;
45 case WGS84:
46 a_ = 6378137;
47 b_ = 6.356752314245179e+06;
48 f_ = 0.003352810664747;
49 break;
50 default:
51 a_ = std::numeric_limits<double>::quiet_NaN();
52 b_ = std::numeric_limits<double>::quiet_NaN();
53 f_ = std::numeric_limits<double>::quiet_NaN();
54 throw std::invalid_argument("Ellipsoid not defined");
55 }
56
57 e2_ = (a_ * a_ - b_ * b_) / (a_ * a_);
58 }
59
EllToXYZ(const std::vector<Eigen::Vector3d> & ell) const60 std::vector<Eigen::Vector3d> GPSTransform::EllToXYZ(
61 const std::vector<Eigen::Vector3d>& ell) const {
62 std::vector<Eigen::Vector3d> xyz(ell.size());
63
64 for (size_t i = 0; i < ell.size(); ++i) {
65 const double lat = DegToRad(ell[i](0));
66 const double lon = DegToRad(ell[i](1));
67 const double alt = ell[i](2);
68
69 const double sin_lat = sin(lat);
70 const double sin_lon = sin(lon);
71 const double cos_lat = cos(lat);
72 const double cos_lon = cos(lon);
73
74 // Normalized radius
75 const double N = a_ / sqrt(1 - e2_ * sin_lat * sin_lat);
76
77 xyz[i](0) = (N + alt) * cos_lat * cos_lon;
78 xyz[i](1) = (N + alt) * cos_lat * sin_lon;
79 xyz[i](2) = (N * (1 - e2_) + alt) * sin_lat;
80 }
81
82 return xyz;
83 }
84
XYZToEll(const std::vector<Eigen::Vector3d> & xyz) const85 std::vector<Eigen::Vector3d> GPSTransform::XYZToEll(
86 const std::vector<Eigen::Vector3d>& xyz) const {
87 std::vector<Eigen::Vector3d> ell(xyz.size());
88
89 for (size_t i = 0; i < ell.size(); ++i) {
90 const double x = xyz[i](0);
91 const double y = xyz[i](1);
92 const double z = xyz[i](2);
93
94 const double xx = x * x;
95 const double yy = y * y;
96
97 const double kEps = 1e-12;
98
99 // Latitude
100 double lat = atan2(z, sqrt(xx + yy));
101 double alt;
102
103 for (size_t j = 0; j < 100; ++j) {
104 const double sin_lat0 = sin(lat);
105 const double N = a_ / sqrt(1 - e2_ * sin_lat0 * sin_lat0);
106 alt = sqrt(xx + yy) / cos(lat) - N;
107 const double prev_lat = lat;
108 lat = atan((z / sqrt(xx + yy)) * 1 / (1 - e2_ * N / (N + alt)));
109
110 if (std::abs(prev_lat - lat) < kEps) {
111 break;
112 }
113 }
114
115 ell[i](0) = RadToDeg(lat);
116
117 // Longitude
118 ell[i](1) = RadToDeg(atan2(y, x));
119 // Alt
120 ell[i](2) = alt;
121 }
122
123 return ell;
124 }
125
126 } // namespace colmap
127