Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2018-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: MIT
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include "gtest/gtest.h"
10 : : #include <ad/map/point/Operation.hpp>
11 : : #include <ad/map/test_support/NoLogTestMacros.hpp>
12 : : #include "RandomGeometry.hpp"
13 : :
14 : : using namespace ::ad::physics;
15 : : using namespace ::ad::map;
16 : : using namespace ::ad::map::point;
17 : :
18 : : struct CoordianteTransformTest : ::testing::Test
19 : : {
20 : 8 : CoordianteTransformTest()
21 [ + - ]: 8 : {
22 : 8 : }
23 : :
24 : 8 : virtual void SetUp()
25 : : {
26 [ + - ]: 8 : access::cleanup();
27 : 8 : GeoPoint enu_origin = createGeoPoint(Longitude(-78.12), Latitude(43.21), Altitude(120));
28 [ + - ]: 8 : mCoordinateTransform.setENUReferencePoint(enu_origin);
29 : 8 : }
30 : :
31 : 8 : virtual void TearDown()
32 : : {
33 : 8 : access::cleanup();
34 : 8 : }
35 : :
36 : 8 : template <typename TF1, typename TF2> void TestCT(const TF1 &tf1, const TF2 &tf2)
37 : : {
38 : 8 : TF1 tf1_from_tf1(tf1);
39 : 8 : TF2 tf2_from_tf2;
40 : 8 : TF1 tf1_from_tf2;
41 : 8 : TF2 tf2_from_tf1;
42 : 8 : TF1 tf1_from_tf2_from_tf1;
43 : 8 : TF2 tf2_from_tf1_from_tf2;
44 [ + - - + : 8 : ASSERT_TRUE(isValid(tf1_from_tf1));
- - - - -
- - - ]
45 [ + - - + : 8 : ASSERT_FALSE(isValid(tf2_from_tf2, false));
- - - - -
- - - ]
46 [ + - - + : 8 : ASSERT_FALSE(isValid(tf1_from_tf2, false));
- - - - -
- - - ]
47 [ + - - + : 8 : ASSERT_FALSE(isValid(tf2_from_tf1, false));
- - - - -
- - - ]
48 [ + - - + : 8 : ASSERT_FALSE(isValid(tf1_from_tf2_from_tf1, false));
- - - - -
- - - ]
49 [ + - - + : 8 : ASSERT_FALSE(isValid(tf2_from_tf1_from_tf2, false));
- - - - -
- - - ]
50 : :
51 : 8 : tf2_from_tf2 = tf2;
52 [ + - - + : 8 : ASSERT_TRUE(isValid(tf1_from_tf1));
- - - - -
- - - ]
53 [ + - - + : 8 : ASSERT_TRUE(isValid(tf2_from_tf2));
- - - - -
- - - ]
54 : :
55 [ + - ]: 8 : mCoordinateTransform.convert(tf2, tf1_from_tf2);
56 [ + - ]: 8 : mCoordinateTransform.convert(tf1, tf2_from_tf1);
57 [ + - - + : 8 : ASSERT_TRUE(isValid(tf1_from_tf2));
- - - - -
- - - ]
58 [ + - - + : 8 : ASSERT_TRUE(isValid(tf2_from_tf1));
- - - - -
- - - ]
59 : :
60 [ + - ]: 8 : mCoordinateTransform.convert(tf2_from_tf1, tf1_from_tf2_from_tf1);
61 [ + - ]: 8 : mCoordinateTransform.convert(tf1_from_tf2, tf2_from_tf1_from_tf2);
62 [ + - - + : 8 : ASSERT_TRUE(isValid(tf1_from_tf2_from_tf1));
- - - - -
- - - ]
63 [ + - - + : 8 : ASSERT_TRUE(isValid(tf2_from_tf1_from_tf2));
- - - - -
- - - ]
64 : :
65 : : // consider the actual distances of the points to be 1cm accurate
66 : 8 : const Distance epsilon(0.01);
67 [ + - + - : 8 : EXPECT_LE(distance(tf1, tf1_from_tf1), epsilon);
- + - - -
- - - ]
68 [ + - + - : 8 : EXPECT_LE(distance(tf1, tf1_from_tf2), epsilon);
- + - - -
- - - ]
69 [ + - + - : 8 : EXPECT_LE(distance(tf1, tf1_from_tf2_from_tf1), epsilon);
- + - - -
- - - ]
70 : :
71 [ + - + - : 8 : EXPECT_LE(distance(tf2, tf2_from_tf2), epsilon);
- + - - -
- - - ]
72 [ + - + - : 8 : EXPECT_LE(distance(tf2, tf2_from_tf1), epsilon);
- + - - -
- - - ]
73 [ + - + - : 8 : EXPECT_LE(distance(tf2, tf2_from_tf1_from_tf2), epsilon);
- + - - -
- - - ]
74 : : }
75 : :
76 : 4 : template <typename TF1, typename TF2> void TestCT2(const TF1 &tf1, const TF2 &tf2)
77 : : {
78 : 4 : TestCT(tf1, tf2);
79 : 4 : TestCT(tf2, tf1);
80 : 4 : }
81 : :
82 : : CoordinateTransform mCoordinateTransform;
83 : : };
84 : :
85 : 2 : TEST_F(CoordianteTransformTest, Geo2ENU)
86 : : {
87 : : // Position of tree from http://www.dirsig.org/docs/new/coordinates.html
88 : 1 : GeoPoint geo = createGeoPoint(Longitude(-78.120123), Latitude(43.21009), Altitude(124));
89 : 1 : ENUPoint enu = createENUPoint(-10, 10, 4);
90 [ + - ]: 1 : TestCT2(geo, enu);
91 : 1 : }
92 : :
93 : 2 : TEST_F(CoordianteTransformTest, ECEF2ENU)
94 : : {
95 : : // Position of tree from http://www.dirsig.org/docs/new/coordinates.html
96 : 1 : ENUPoint enu = createENUPoint(-10, 10, 4);
97 : : // ECEFPoint ecef(958506.011, -4556367.375, 4344627.164); // Original values
98 : 1 : ECEFPoint ecef = createECEFPoint(958506.00937351841, -4556367.3723087218, 4344627.1621201029); // Exact
99 [ + - ]: 1 : TestCT2(ecef, enu);
100 : 1 : }
101 : :
102 : 2 : TEST_F(CoordianteTransformTest, ECEFGeo)
103 : : {
104 : : {
105 : : // Position of Intel Office
106 : 1 : GeoPoint geo = createGeoPoint(Longitude(-121.935549), Latitude(37.401336), Altitude(123.456));
107 : 1 : ECEFPoint ecef = createECEFPoint(-2683524.7374057802, -4305306.3074802049, 3852945.8669066750);
108 [ + - ]: 1 : TestCT2(geo, ecef);
109 : : }
110 : :
111 : : {
112 : : // Position of tree from http://www.dirsig.org/docs/new/coordinates.html
113 : 1 : GeoPoint geo = createGeoPoint(Longitude(-78.120123), Latitude(43.21009), Altitude(124));
114 : : // ECEFPoint ecef = createECEFPoint(958506.011, -4556367.375, 4344627.164); // Original values
115 : 1 : ECEFPoint ecef = createECEFPoint(958506.00937351841, -4556367.3723087218, 4344627.1621201029); // Exact
116 [ + - ]: 1 : TestCT2(geo, ecef);
117 : : }
118 : 1 : }
119 : :
120 : 2 : TEST_F(CoordianteTransformTest, Distances)
121 : : {
122 : : {
123 : 1 : const Altitude h(123.456); // Invented altitude.
124 : 1 : const Altitude dh(100.123); // Invented altitude difference.
125 : :
126 : 1 : GeoPoint geo0;
127 [ + - - + : 1 : ASSERT_FALSE(isValid(geo0, false));
- - - - -
- - - ]
128 : : // Position of Intel Office
129 : 1 : GeoPoint geo1 = createGeoPoint(Longitude(-121.935549), Latitude(37.401336), h);
130 : : // Position of San Jose Fire Department Station 29
131 : 1 : GeoPoint geo2 = createGeoPoint(Longitude(-121.933886), Latitude(37.401112), h);
132 : : // Position of San Jose Fire Department Station 29 + altitude difference
133 [ + - ]: 1 : GeoPoint geo3 = createGeoPoint(Longitude(-121.933886), Latitude(37.401112), h + dh);
134 : :
135 [ + - - + : 1 : ASSERT_TRUE(isValid(geo1));
- - - - -
- - - ]
136 [ + - - + : 1 : ASSERT_TRUE(isValid(geo2));
- - - - -
- - - ]
137 [ + - - + : 1 : ASSERT_TRUE(isValid(geo3));
- - - - -
- - - ]
138 : :
139 : 1 : ECEFPoint ecef0;
140 [ + - - + : 1 : ASSERT_FALSE(isValid(ecef0, false));
- - - - -
- - - ]
141 : :
142 [ + - ]: 1 : ECEFPoint ecef1 = mCoordinateTransform.Geo2ECEF(geo1);
143 [ + - ]: 1 : ECEFPoint ecef2 = mCoordinateTransform.Geo2ECEF(geo2);
144 [ + - ]: 1 : ECEFPoint ecef3 = mCoordinateTransform.Geo2ECEF(geo3);
145 : :
146 [ + - - + : 1 : ASSERT_TRUE(isValid(ecef1));
- - - - -
- - - ]
147 [ + - - + : 1 : ASSERT_TRUE(isValid(ecef2));
- - - - -
- - - ]
148 [ + - - + : 1 : ASSERT_TRUE(isValid(ecef3));
- - - - -
- - - ]
149 : :
150 [ + - ]: 1 : ENUPoint enu0 = mCoordinateTransform.ECEF2ENU(ecef1);
151 [ + - - + : 1 : ASSERT_TRUE(isValid(enu0, false));
- - - - -
- - - ]
152 : :
153 [ + - ]: 1 : mCoordinateTransform.setENUReferencePoint(geo1);
154 [ + - ]: 1 : ENUPoint enu1 = mCoordinateTransform.ECEF2ENU(ecef1);
155 [ + - ]: 1 : ENUPoint enu2 = mCoordinateTransform.ECEF2ENU(ecef2);
156 [ + - ]: 1 : ENUPoint enu3 = mCoordinateTransform.ECEF2ENU(ecef3);
157 [ + - - + : 1 : ASSERT_TRUE(isValid(enu1));
- - - - -
- - - ]
158 [ + - - + : 1 : ASSERT_TRUE(isValid(enu2));
- - - - -
- - - ]
159 [ + - - + : 1 : ASSERT_TRUE(isValid(enu3));
- - - - -
- - - ]
160 : :
161 [ + - ]: 1 : Distance d_geo_12 = distance(geo1, geo2);
162 [ + - ]: 1 : Distance d_enu_12 = distance(enu1, enu2);
163 [ + - ]: 1 : Distance d_ecef_12 = distance(ecef1, ecef2);
164 [ + - ]: 1 : Distance d_geo_13 = distance(geo1, geo3);
165 [ + - ]: 1 : Distance d_enu_13 = distance(enu1, enu3);
166 [ + - ]: 1 : Distance d_ecef_13 = distance(ecef1, ecef3);
167 [ + - ]: 1 : Distance d_geo_23 = distance(geo2, geo3);
168 [ + - ]: 1 : Distance d_enu_23 = distance(enu2, enu3);
169 [ + - ]: 1 : Distance d_ecef_23 = distance(ecef2, ecef3);
170 : :
171 [ + - - + : 1 : ASSERT_EQ(d_geo_12, d_ecef_12);
- - - - -
- ]
172 [ + - - + : 1 : ASSERT_EQ(d_geo_13, d_ecef_13);
- - - - -
- ]
173 [ + - - + : 1 : ASSERT_EQ(d_geo_23, d_ecef_23);
- - - - -
- ]
174 [ + - - + : 1 : ASSERT_EQ(d_geo_12, d_enu_12);
- - - - -
- ]
175 [ + - - + : 1 : ASSERT_EQ(d_geo_13, d_enu_13);
- - - - -
- ]
176 [ + - - + : 1 : ASSERT_EQ(d_geo_23, d_enu_23);
- - - - -
- ]
177 [ + - - + : 1 : ASSERT_EQ(d_enu_12, d_ecef_12);
- - - - -
- ]
178 [ + - - + : 1 : ASSERT_EQ(d_enu_13, d_ecef_13);
- - - - -
- ]
179 [ + - - + : 1 : ASSERT_EQ(d_enu_23, d_ecef_23);
- - - - -
- ]
180 [ + - - + : 1 : ASSERT_EQ(d_geo_23, Distance(static_cast<double>(dh)));
- - - - -
- ]
181 [ + - - + : 1 : ASSERT_EQ(d_enu_23, Distance(static_cast<double>(dh)));
- - - - -
- ]
182 [ + - - + : 1 : ASSERT_EQ(d_ecef_23, Distance(static_cast<double>(dh)));
- - - - -
- ]
183 : : }
184 : : }
185 : :
186 : 2 : TEST_F(CoordianteTransformTest, GetRef)
187 : : {
188 [ + - ]: 1 : CoordinateTransform coordinateTransform;
189 : : // Position of Intel Office
190 : 1 : GeoPoint pt1_geo = createGeoPoint(Longitude(-121.935549), Latitude(37.401336), Altitude(123.456));
191 : : // Position of San Jose Fire Department Station 29
192 : 1 : GeoPoint pt2_geo = createGeoPoint(Longitude(-121.933886), Latitude(37.401112), Altitude(123.456));
193 : :
194 : 1 : size_t enu_ref = 0;
195 : 1 : size_t result = 0;
196 [ + - ]: 1 : coordinateTransform.setENUReferencePoint(pt1_geo);
197 [ + - ]: 1 : enu_ref = coordinateTransform.getENURef();
198 [ + - ]: 1 : coordinateTransform.setENUReferencePoint(pt2_geo);
199 [ + - ]: 1 : result = coordinateTransform.getENURef();
200 [ + - - + : 1 : ASSERT_EQ(enu_ref + 1, result);
- - - - -
- ]
201 : : }
202 : :
203 : 2 : TEST_F(CoordianteTransformTest, Branch)
204 : : {
205 [ + - ]: 2 : CoordinateTransform coordinateTransform;
206 : :
207 : : // Position of Intel Office
208 : 1 : GeoPoint pt1_geo = createGeoPoint(Longitude(-121.935549), Latitude(37.401336), Altitude(123.456));
209 : :
210 : 1 : GeoPoint geo_pt_invalid;
211 : 1 : ECEFPoint ecef_pt_invalid;
212 : 1 : ENUPoint enu_pt_invalid;
213 : :
214 [ + - + - : 2 : EXPECT_THROW_NO_LOG(coordinateTransform.Geo2ENU(geo_pt_invalid), std::invalid_argument);
+ - + - +
- + - - +
- + - - -
- - - + -
- - ]
215 [ + - + - : 2 : EXPECT_THROW_NO_LOG(coordinateTransform.ENU2Geo(enu_pt_invalid), std::invalid_argument);
+ - + - +
- + - - +
- + - - -
- - - + -
- - ]
216 [ + - + - : 2 : EXPECT_THROW_NO_LOG(coordinateTransform.Geo2ECEF(geo_pt_invalid), std::invalid_argument);
+ - + - +
- + - - +
- + - - -
- - - + -
- - ]
217 [ + - + - : 2 : EXPECT_THROW_NO_LOG(coordinateTransform.ECEF2Geo(ecef_pt_invalid), std::invalid_argument);
+ - + - +
- + - - +
- + - - -
- - - + -
- - ]
218 [ + - + - : 2 : EXPECT_THROW_NO_LOG(coordinateTransform.ENU2ECEF(enu_pt_invalid), std::invalid_argument);
+ - + - +
- + - - +
- + - - -
- - - + -
- - ]
219 [ + - + - : 2 : EXPECT_THROW_NO_LOG(coordinateTransform.ECEF2ENU(ecef_pt_invalid), std::invalid_argument);
+ - + - +
- + - - +
- + - - -
- - - + -
- - ]
220 : :
221 [ + - + - : 2 : EXPECT_THROW_NO_LOG(coordinateTransform.setENUReferencePoint(geo_pt_invalid), std::invalid_argument);
+ - + - +
- + - - +
- + - - -
- - - + -
- - ]
222 : :
223 [ + - + - : 1 : EXPECT_NO_THROW(coordinateTransform.setENUReferencePoint(pt1_geo));
+ - + - +
- - - - -
- - - - ]
224 : : // ensure the transformation is working
225 [ + - ]: 1 : auto enu_origin_transformed = coordinateTransform.Geo2ENU(pt1_geo);
226 [ + - - + : 1 : EXPECT_EQ(enu_origin_transformed, point::createENUPoint(0., 0., 0.));
- - - - -
- ]
227 : :
228 [ + - + - : 2 : EXPECT_THROW_NO_LOG(coordinateTransform.Geo2ENU(geo_pt_invalid), std::invalid_argument);
+ - + - +
- + - - +
- + - - -
- - - + -
- - ]
229 [ + - + - : 2 : EXPECT_THROW_NO_LOG(coordinateTransform.ENU2Geo(enu_pt_invalid), std::invalid_argument);
+ - + - +
- + - - +
- + - - -
- - - + -
- - ]
230 [ + - + - : 2 : EXPECT_THROW_NO_LOG(coordinateTransform.ENU2ECEF(enu_pt_invalid), std::invalid_argument);
+ - + - +
- + - - +
- + - - -
- - - + -
- - ]
231 [ + - + - : 2 : EXPECT_THROW_NO_LOG(coordinateTransform.ECEF2ENU(ecef_pt_invalid), std::invalid_argument);
+ - + - +
- + - - +
- + - - -
- - - + -
- - ]
232 : 1 : }
233 : :
234 : 2 : TEST_F(CoordianteTransformTest, ENUGeometry)
235 : : {
236 : : // Position of Intel Office
237 : 1 : GeoPoint pt1_geo = createGeoPoint(Longitude(-121.935549), Latitude(37.401336), Altitude(123.456));
238 : : // Position of San Jose Fire Department Station 29
239 : 1 : GeoPoint pt2_geo = createGeoPoint(Longitude(-121.933886), Latitude(37.401112), Altitude(123.456));
240 : :
241 [ + - ]: 1 : ECEFPoint pt1_ecef = toECEF(pt1_geo);
242 [ + - - + : 1 : EXPECT_TRUE(isValid(pt1_ecef));
- - - - -
- - - ]
243 : :
244 [ + - ]: 1 : ECEFPoint pt2_ecef = toECEF(pt2_geo);
245 [ + - - + : 1 : EXPECT_TRUE(isValid(pt2_ecef));
- - - - -
- - - ]
246 : :
247 : : static constexpr size_t geo1_n = 200;
248 : : static constexpr size_t geo2_n = 250;
249 : : static constexpr size_t geo1_seed = 123;
250 : : static constexpr size_t geo2_seed = 456;
251 : :
252 [ + - ]: 2 : Geometry geo1 = randGeometry(pt1_ecef, geo1_n, geo1_seed);
253 [ - + - - : 1 : EXPECT_TRUE(isValid(geo1));
- - - - -
- ]
254 : :
255 [ + - ]: 2 : Geometry geo2 = randGeometry(pt2_ecef, geo2_n, geo2_seed);
256 [ - + - - : 1 : EXPECT_TRUE(isValid(geo2));
- - - - -
- ]
257 : :
258 : : // no ENU reference set
259 [ + - + - : 2 : EXPECT_THROW_NO_LOG(toENU(geo1.ecefEdge), std::invalid_argument);
+ - + - +
- + - - +
- + - - -
- - - + -
- - ]
260 [ + - + - : 2 : EXPECT_THROW_NO_LOG(toENU(geo2.ecefEdge), std::invalid_argument);
+ - + - +
- + - - +
- + - - -
- - - + -
- - ]
261 : :
262 [ + - ]: 2 : const ENUEdge geo1_cf1_enu1 = toENU(geo1.ecefEdge, pt1_geo);
263 [ + - - + : 1 : EXPECT_TRUE(isValid(geo1_cf1_enu1));
- - - - -
- - - ]
264 [ + - ]: 2 : const ENUEdge geo2_cf1_enu1 = toENU(geo2.ecefEdge, pt1_geo);
265 [ + - - + : 1 : EXPECT_TRUE(isValid(geo2_cf1_enu1));
- - - - -
- - - ]
266 : :
267 [ + - ]: 2 : const ENUEdge geo1_cf1_enu2 = toENU(geo1.ecefEdge, pt2_geo);
268 [ + - - + : 1 : EXPECT_TRUE(isValid(geo1_cf1_enu2));
- - - - -
- - - ]
269 [ + - ]: 2 : const ENUEdge geo2_cf1_enu2 = toENU(geo2.ecefEdge, pt2_geo);
270 [ + - - + : 1 : EXPECT_TRUE(isValid(geo2_cf1_enu2));
- - - - -
- - - ]
271 : :
272 [ + - ]: 2 : const ENUEdge geo1_cf1_enu1a = toENU(geo1.ecefEdge, pt1_geo);
273 [ + - - + : 1 : EXPECT_TRUE(isValid(geo1_cf1_enu1a));
- - - - -
- - - ]
274 [ + - ]: 2 : const ENUEdge geo2_cf1_enu1a = toENU(geo2.ecefEdge, pt1_geo);
275 [ + - - + : 1 : EXPECT_TRUE(isValid(geo2_cf1_enu1a));
- - - - -
- - - ]
276 : :
277 [ + - - + : 1 : EXPECT_NE(geo1_cf1_enu1, geo1_cf1_enu2);
- - - - -
- ]
278 [ + - - + : 1 : EXPECT_EQ(geo1_cf1_enu1, geo1_cf1_enu1a);
- - - - -
- ]
279 [ + - - + : 1 : EXPECT_NE(geo2_cf1_enu1, geo2_cf1_enu2);
- - - - -
- ]
280 [ + - - + : 1 : EXPECT_EQ(geo2_cf1_enu1, geo2_cf1_enu1a);
- - - - -
- ]
281 : :
282 [ + - ]: 2 : const ENUEdge geo1_cf2_enu2 = toENU(geo1.ecefEdge, pt2_geo);
283 [ + - - + : 1 : EXPECT_TRUE(isValid(geo1_cf2_enu2));
- - - - -
- - - ]
284 [ + - ]: 2 : const ENUEdge geo2_cf2_enu2 = toENU(geo2.ecefEdge, pt2_geo);
285 [ + - - + : 1 : EXPECT_TRUE(isValid(geo2_cf2_enu2));
- - - - -
- - - ]
286 : :
287 [ + - ]: 2 : const ENUEdge geo1_cf2_enu1 = toENU(geo1.ecefEdge, pt1_geo);
288 [ + - - + : 1 : EXPECT_TRUE(isValid(geo1_cf2_enu1));
- - - - -
- - - ]
289 [ + - ]: 2 : const ENUEdge geo2_cf2_enu1 = toENU(geo2.ecefEdge, pt1_geo);
290 [ + - - + : 1 : EXPECT_TRUE(isValid(geo2_cf2_enu1));
- - - - -
- - - ]
291 : :
292 [ + - ]: 2 : const ENUEdge geo1_cf2_enu2a = toENU(geo1.ecefEdge, pt2_geo);
293 [ + - - + : 1 : EXPECT_TRUE(isValid(geo1_cf2_enu2a));
- - - - -
- - - ]
294 [ + - ]: 2 : const ENUEdge geo2_cf2_enu2a = toENU(geo2.ecefEdge, pt2_geo);
295 [ + - - + : 1 : EXPECT_TRUE(isValid(geo2_cf2_enu2a));
- - - - -
- - - ]
296 : :
297 [ + - - + : 1 : EXPECT_NE(geo1_cf2_enu1, geo1_cf2_enu2);
- - - - -
- ]
298 [ + - - + : 1 : EXPECT_EQ(geo1_cf2_enu2, geo1_cf2_enu2a);
- - - - -
- ]
299 [ + - - + : 1 : EXPECT_NE(geo2_cf2_enu1, geo2_cf2_enu2);
- - - - -
- ]
300 [ + - - + : 1 : EXPECT_EQ(geo2_cf2_enu2, geo2_cf2_enu2a);
- - - - -
- ]
301 : :
302 [ + - - + : 1 : EXPECT_EQ(geo1_cf1_enu1, geo1_cf2_enu1);
- - - - -
- ]
303 [ + - - + : 1 : EXPECT_EQ(geo1_cf1_enu2, geo1_cf2_enu2);
- - - - -
- ]
304 [ + - - + : 1 : EXPECT_EQ(geo2_cf1_enu1, geo2_cf2_enu1);
- - - - -
- ]
305 [ + - - + : 1 : EXPECT_EQ(geo2_cf1_enu2, geo2_cf2_enu2);
- - - - -
- ]
306 : 1 : }
307 : :
308 : 2 : TEST_F(CoordianteTransformTest, TOENU)
309 : : {
310 : : // Position of Intel Office
311 : 1 : GeoPoint pt1_geo = createGeoPoint(Longitude(-121.935549), Latitude(37.401336), Altitude(123.456));
312 : : // Position of San Jose Fire Department Station 29
313 : 1 : GeoPoint pt2_geo = createGeoPoint(Longitude(-121.933886), Latitude(37.401112), Altitude(123.456));
314 [ + - ]: 1 : Distance geo_abs_length = distance(pt1_geo, pt2_geo);
315 : 2 : GeoEdge geo_edge;
316 [ + - ]: 1 : geo_edge.push_back(pt1_geo);
317 [ + - ]: 1 : geo_edge.push_back(pt2_geo);
318 [ + - ]: 1 : Distance geo_edge_length = calcLength(geo_edge);
319 [ + - - + : 1 : EXPECT_EQ(geo_abs_length, geo_edge_length);
- - - - -
- ]
320 : :
321 [ + - ]: 1 : ECEFPoint pt1_ecef = toECEF(pt1_geo);
322 [ + - - + : 1 : EXPECT_TRUE(isValid(pt1_ecef));
- - - - -
- - - ]
323 [ + - ]: 1 : ECEFPoint pt2_ecef = toECEF(pt2_geo);
324 [ + - - + : 1 : EXPECT_TRUE(isValid(pt2_ecef));
- - - - -
- - - ]
325 [ + - ]: 1 : Distance ecef_abs_length = distance(pt1_ecef, pt2_ecef);
326 : 2 : ECEFEdge ecef_edge;
327 [ + - ]: 1 : ecef_edge.push_back(pt1_ecef);
328 [ + - ]: 1 : ecef_edge.push_back(pt2_ecef);
329 [ + - ]: 1 : Distance ecef_edge_length = calcLength(ecef_edge);
330 [ + - - + : 1 : EXPECT_EQ(ecef_abs_length, ecef_edge_length);
- - - - -
- ]
331 : :
332 [ + - - + : 1 : EXPECT_EQ(geo_abs_length, ecef_abs_length);
- - - - -
- ]
333 : :
334 : 1 : const Altitude h(123.456); // Invented altitude.
335 : 1 : const Altitude dh(100.123); // Invented altitude difference.
336 [ + - ]: 1 : GeoPoint enu_origin = createGeoPoint(Longitude(-121.933886), Latitude(37.401112), h + dh);
337 : 1 : ENUPoint pt1_enu;
338 : 1 : ENUPoint pt2_enu;
339 : 1 : Distance enu_length;
340 [ + - ]: 1 : pt1_enu = toENU(pt1_ecef, enu_origin);
341 [ + - ]: 1 : pt2_enu = toENU(pt2_ecef, enu_origin);
342 [ + - ]: 1 : enu_length = distance(pt1_enu, pt2_enu);
343 [ + - - + : 1 : EXPECT_EQ(enu_length, ecef_abs_length);
- - - - -
- ]
344 : :
345 [ + - ]: 1 : pt1_enu = toENU(pt1_geo, enu_origin);
346 [ + - ]: 1 : pt2_enu = toENU(pt2_geo, enu_origin);
347 [ + - ]: 1 : enu_length = distance(pt1_enu, pt2_enu);
348 [ + - - + : 1 : EXPECT_EQ(enu_length, ecef_abs_length);
- - - - -
- ]
349 : :
350 : 2 : ENUEdge enu_edge;
351 [ + - ]: 1 : enu_edge.push_back(pt1_enu);
352 [ + - ]: 1 : enu_edge.push_back(pt2_enu);
353 : 2 : GeoEdge geo_edge_ret;
354 [ + - ]: 1 : geo_edge_ret = toGeo(enu_edge, enu_origin);
355 [ + - ]: 1 : Distance geo_from_enu_length = calcLength(geo_edge_ret);
356 [ + - - + : 1 : EXPECT_EQ(geo_from_enu_length, geo_edge_length);
- - - - -
- ]
357 : :
358 : 2 : ECEFEdge ecef_edge_ret;
359 [ + - ]: 1 : ecef_edge_ret = toECEF(enu_edge, enu_origin);
360 [ + - ]: 1 : Distance ecef_from_enu_length = calcLength(ecef_edge_ret);
361 [ + - - + : 1 : EXPECT_EQ(ecef_from_enu_length, ecef_edge_length);
- - - - -
- ]
362 : :
363 : 2 : ENUEdge enu_edge_ret;
364 [ + - ]: 1 : enu_edge_ret = toENU(geo_edge_ret, enu_origin);
365 [ + - ]: 1 : enu_length = calcLength(enu_edge_ret);
366 [ + - - + : 1 : EXPECT_EQ(enu_length, geo_edge_length);
- - - - -
- ]
367 : 1 : }
|
