test_utm.cpp
Go to the documentation of this file.
1 /* $Id: bbd8ace5d9989f7680620837c5eafd18ac5c51b2 $ */
2 
3 /*********************************************************************
4 * Software License Agreement (BSD License)
5 *
6 * Copyright (c) 2011 Jack O'Quin
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the author nor other contributors may be
20 * used to endorse or promote products derived from this software
21 * without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *********************************************************************/
36 
37 #include <sstream>
38 #include <gtest/gtest.h>
39 #include "geodesy/utm.h"
40 
41 
43 // Utility functions
45 
46 // check that two UTM points are near each other
48  const geodesy::UTMPoint &pt2,
49  double abs_err)
50 {
51  EXPECT_NEAR(pt1.easting, pt2.easting, abs_err);
52  EXPECT_NEAR(pt1.northing, pt2.northing, abs_err);
53  EXPECT_NEAR(pt1.altitude, pt2.altitude, abs_err);
54  EXPECT_EQ(pt1.zone, pt2.zone);
55  EXPECT_EQ(pt1.band, pt2.band);
56 }
57 
59 // Test cases
61 
62 // Test null constructor
63 TEST(UTMPoint, nullConstructor)
64 {
66 
67  EXPECT_EQ(pt.easting, 0.0);
68  EXPECT_EQ(pt.northing, 0.0);
69  EXPECT_TRUE(geodesy::is2D(pt));
70  EXPECT_EQ(pt.zone, 0);
71  EXPECT_EQ(pt.band, ' ');
72  EXPECT_FALSE(geodesy::isValid(pt));
73 }
74 
75 // Test 2D constructor
76 TEST(UTMPoint, flatConstructor)
77 {
78  double e = 1000.0;
79  double n = 2400.0;
80  uint8_t z = 14;
81  char b = 'R';
82  geodesy::UTMPoint pt(e, n, z, b);
83 
84  EXPECT_TRUE(geodesy::is2D(pt));
85  EXPECT_TRUE(geodesy::isValid(pt));
86 
87  EXPECT_EQ(pt.easting, e);
88  EXPECT_EQ(pt.northing, n);
89  EXPECT_EQ(pt.zone, z);
90  EXPECT_EQ(pt.band, b);
91 }
92 
93 // Test 3D constructor
94 TEST(UTMPoint, hasAltitude)
95 {
96  double e = 1000.0;
97  double n = 2400.0;
98  double a = 200.0;
99  uint8_t z = 14;
100  char b = 'R';
101  geodesy::UTMPoint pt(e, n, a, z, b);
102 
103  EXPECT_FALSE(geodesy::is2D(pt));
104  EXPECT_TRUE(geodesy::isValid(pt));
105 
106  EXPECT_EQ(pt.easting, e);
107  EXPECT_EQ(pt.northing, n);
108  EXPECT_EQ(pt.zone, z);
109  EXPECT_EQ(pt.band, b);
110 }
111 
112 // Test copy constructor
113 TEST(UTMPoint, copyConstructor)
114 {
115  double e = 1000.0;
116  double n = 2400.0;
117  double a = 200.0;
118  uint8_t z = 14;
119  char b = 'R';
120  geodesy::UTMPoint pt1(e, n, a, z, b);
121  geodesy::UTMPoint pt2(pt1);
122 
123  EXPECT_FALSE(geodesy::is2D(pt2));
124  EXPECT_TRUE(geodesy::isValid(pt2));
125 
126  EXPECT_EQ(pt2.easting, e);
127  EXPECT_EQ(pt2.northing, n);
128  EXPECT_EQ(pt2.zone, z);
129  EXPECT_EQ(pt2.band, b);
130 }
131 
132 // Test UTM point constructor from WGS 84
133 TEST(UTMPoint, fromLatLong)
134 {
135  // University of Texas, Austin, Pickle Research Campus
136  double lat = 30.385315;
137  double lon = -97.728524;
138  double alt = 209.0;
139 
140  geographic_msgs::GeoPoint ll;
141  ll.latitude = lat;
142  ll.longitude = lon;
143  ll.altitude = alt;
144 
145  // create UTM from point
146  geodesy::UTMPoint pt(ll);
147 
148  double e = 622159.34;
149  double n = 3362168.30;
150  uint8_t z = 14;
151  char b = 'R';
152  double abs_err = 0.01;
153 
154  EXPECT_FALSE(geodesy::is2D(pt));
155  EXPECT_TRUE(geodesy::isValid(pt));
156 
157  EXPECT_NEAR(pt.easting, e, abs_err);
158  EXPECT_NEAR(pt.northing, n, abs_err);
159  EXPECT_NEAR(pt.altitude, alt, abs_err);
160  EXPECT_EQ(pt.zone, z);
161  EXPECT_EQ(pt.band, b);
162 }
163 
164 // Test zone numbers
165 TEST(UTMPoint, testZones)
166 {
168  pt.band = 'X'; // supply a valid band letter
169 
170  pt.zone = 0;
171  EXPECT_FALSE(geodesy::isValid(pt));
172 
173  pt.zone = 61;
174  EXPECT_FALSE(geodesy::isValid(pt));
175 
176  pt.zone = 255;
177  EXPECT_FALSE(geodesy::isValid(pt));
178 
179  // these should all work
180  for (uint8_t b = 1; b <= 60; ++b)
181  {
182  pt.zone = b;
183  EXPECT_TRUE(geodesy::isValid(pt));
184  }
185 }
186 
187 // Test band letters
188 TEST(UTMPoint, testBands)
189 {
191  pt.zone = 14; // supply a valid zone number
192  EXPECT_FALSE(geodesy::isValid(pt));
193 
194  pt.band = '9';
195  EXPECT_FALSE(geodesy::isValid(pt));
196 
197  pt.band = ';';
198  EXPECT_FALSE(geodesy::isValid(pt));
199 
200  pt.band = 'I';
201  EXPECT_FALSE(geodesy::isValid(pt));
202 
203  pt.band = 'O';
204  EXPECT_FALSE(geodesy::isValid(pt));
205 
206  pt.band = 'Y';
207  EXPECT_FALSE(geodesy::isValid(pt));
208 
209  pt.band = 'r';
210  EXPECT_FALSE(geodesy::isValid(pt));
211 
212  // this should work
213  pt.band = 'X';
214  EXPECT_TRUE(geodesy::isValid(pt));
215 }
216 
217 // Test null pose constructor
218 TEST(UTMPose, nullConstructor)
219 {
220  geodesy::UTMPose pose;
221 
222  EXPECT_TRUE(geodesy::is2D(pose));
223  EXPECT_FALSE(geodesy::isValid(pose));
224 }
225 
226 // Test pose constructor from point and quaternion
227 TEST(UTMPose, pointQuaternionConstructor)
228 {
229  double e = 1000.0;
230  double n = 2400.0;
231  double a = 200.0;
232  uint8_t z = 14;
233  char b = 'R';
234  geodesy::UTMPoint pt(e, n, a, z, b);
235 
236  geometry_msgs::Quaternion q; // identity quaternion
237  q.x = 1.0;
238  q.y = 0.0;
239  q.z = 0.0;
240  q.w = 0.0;
241  geodesy::UTMPose pose(pt, q);
242 
243  EXPECT_FALSE(geodesy::is2D(pose));
244  EXPECT_TRUE(geodesy::isValid(pose));
245 
246  EXPECT_EQ(pose.position.easting, pt.easting);
247  EXPECT_EQ(pose.position.northing, pt.northing);
248  EXPECT_EQ(pose.position.altitude, pt.altitude);
249  EXPECT_EQ(pose.position.zone, pt.zone);
250  EXPECT_EQ(pose.position.band, pt.band);
251 
252  EXPECT_EQ(pose.orientation.w, q.w);
253  EXPECT_EQ(pose.orientation.x, q.x);
254  EXPECT_EQ(pose.orientation.y, q.y);
255  EXPECT_EQ(pose.orientation.z, q.z);
256 }
257 
258 // Test pose quaternion validation
259 TEST(UTMPose, quaternionValidation)
260 {
261  double e = 1000.0;
262  double n = 2400.0;
263  double a = 200.0;
264  uint8_t z = 14;
265  char b = 'R';
266  geodesy::UTMPoint pt(e, n, a, z, b);
267 
268  // identity quaternion
269  geometry_msgs::Quaternion q;
270  q.x = 1.0;
271  q.y = 0.0;
272  q.z = 0.0;
273  q.w = 0.0;
274  geodesy::UTMPose pose1(pt, q);
275  EXPECT_TRUE(geodesy::isValid(pose1));
276 
277  // valid quaternion
278  q.x = 0.7071;
279  q.y = 0.0;
280  q.z = 0.0;
281  q.w = 0.7071;
282  geodesy::UTMPose pose2(pt, q);
283  EXPECT_TRUE(geodesy::isValid(pose2));
284 
285  // quaternion not normalized
286  q.x = 0.8071;
287  q.y = 0.0;
288  q.z = 0.0;
289  q.w = 0.8071;
290  geodesy::UTMPose pose3(pt, q);
291  EXPECT_FALSE(geodesy::isValid(pose3));
292 
293  // zero quaternion (not normalized)
294  q.x = 0.0;
295  q.y = 0.0;
296  q.z = 0.0;
297  q.w = 0.0;
298  geodesy::UTMPose pose4(pt, q);
299  EXPECT_FALSE(geodesy::isValid(pose4));
300 }
301 
302 // Test conversion from UTM to WGS 84 and back
303 TEST(UTMConvert, fromUtmToLatLongAndBack)
304 {
305  double e = 500000.0; // central meridian of each zone
306  double n = 1000.0;
307  double alt = 100.0;
308  char b = 'N';
309 
310  // try every possible zone of longitude
311  for (uint8_t z = 1; z <= 60; ++z)
312  {
313  geodesy::UTMPoint pt1(e, n, alt, z, b);
314  geographic_msgs::GeoPoint ll;
315  convert(pt1, ll);
316  geodesy::UTMPoint pt2;
317  convert(ll, pt2);
318 
319  EXPECT_TRUE(geodesy::isValid(pt1));
320  EXPECT_TRUE(geodesy::isValid(pt2));
321  check_utm_near(pt1, pt2, 0.000001);
322  }
323 }
324 
325 // Test conversion from WGS 84 to UTM and back
326 TEST(UTMConvert, fromLatLongToUtmAndBack)
327 {
328  double alt = 100.0;
329 
330  // Try every possible degree of latitude and longitude. Avoid the
331  // international date line. Even though the converted longitude is
332  // close, it may end up less than -180 and hence inValid().
333  for (double lon = -179.5; lon < 180.0; lon += 1.0)
334  {
335  for (double lat = -80.0; lat <= 84.0; lat += 1.0)
336  {
337  geographic_msgs::GeoPoint pt1(geodesy::toMsg(lat, lon, alt));
338  EXPECT_TRUE(geodesy::isValid(pt1));
339 
340  geodesy::UTMPoint utm(pt1);
341  EXPECT_TRUE(geodesy::isValid(utm));
342 
343  geographic_msgs::GeoPoint pt2(geodesy::toMsg(utm));
344  EXPECT_TRUE(geodesy::isValid(pt2));
345 
346  EXPECT_NEAR(pt1.latitude, pt2.latitude, 0.0000001);
347  EXPECT_NEAR(pt1.longitude, pt2.longitude, 0.0000012);
348  EXPECT_NEAR(pt1.altitude, pt2.altitude, 0.000001);
349  }
350  }
351 }
352 
353 // Test conversion from WGS 84 to UTM and back at international date line
354 TEST(UTMConvert, internationalDateLine)
355 {
356  double alt = 100.0;
357  double lon = -180.0;
358 
359  for (double lat = -80.0; lat <= 84.0; lat += 1.0)
360  {
361  geographic_msgs::GeoPoint pt1(geodesy::toMsg(lat, lon, alt));
362  EXPECT_TRUE(geodesy::isValid(pt1));
363 
364  geodesy::UTMPoint utm;
365  geodesy::fromMsg(pt1, utm);
366  EXPECT_TRUE(geodesy::isValid(utm));
367 
368  geographic_msgs::GeoPoint pt2(geodesy::toMsg(utm));
369  EXPECT_TRUE(geodesy::isValid(pt2));
370  EXPECT_NEAR(pt1.latitude, pt2.latitude, 0.0000001);
371  EXPECT_NEAR(pt1.altitude, pt2.altitude, 0.000001);
372 
373  if (pt2.longitude - pt1.longitude > 359.0)
374  {
375  // pt2 seems to be slightly across the international date
376  // line from pt2, so de-normalize it
377  pt2.longitude -= 360.0;
378  }
379  EXPECT_NEAR(pt1.longitude, pt2.longitude, 0.0000012);
380  }
381 }
382 
383 // Test point output stream operator
384 TEST(OStream, point)
385 {
386  geodesy::UTMPoint pt1;
387  std::ostringstream out1;
388  out1 << pt1;
389  std::string expected("(0, 0, nan [0 ])");
390  EXPECT_EQ(out1.str(), expected);
391 
392  geodesy::UTMPoint pt2(622159.338, 3362168.303, 209, 14, 'R');
393  std::ostringstream out2;
394  out2 << pt2;
395  expected = "(622159.338, 3362168.303, 209 [14R])";
396  EXPECT_EQ(out2.str(), expected);
397 }
398 
399 // Test pose output stream operator
400 TEST(OStream, pose)
401 {
402  geodesy::UTMPoint pt(1000.0, 2400.0, 200.0, 14, 'R');
403  geometry_msgs::Quaternion q;
404  q.w = 1.0;
405  q.x = 0.0;
406  q.y = 0.0;
407  q.z = 0.0;
408  geodesy::UTMPose pose(pt, q);
409 
410  std::ostringstream out;
411  out << pose;
412  std::string expected("(1000, 2400, 200 [14R]), ([0, 0, 0], 1)");
413  EXPECT_EQ(out.str(), expected);
414 }
415 
416 // Run all the tests that were declared with TEST()
417 int main(int argc, char **argv)
418 {
419  testing::InitGoogleTest(&argc, argv);
420 
421  // run the tests in this thread
422  return RUN_ALL_TESTS();
423 }
char band
MGRS latitude band letter.
Definition: utm.h:130
Universal Transverse Mercator coordinates.
int main(int argc, char **argv)
Definition: test_utm.cpp:417
double northing
northing within grid zone [meters]
Definition: utm.h:127
double altitude
altitude above ellipsoid [meters] or NaN
Definition: utm.h:128
geometry_msgs::Quaternion orientation
Definition: utm.h:173
uint8_t zone
UTM longitude zone number.
Definition: utm.h:129
void fromMsg(const geographic_msgs::GeoPoint &from, UTMPoint &to)
TFSIMD_FORCE_INLINE const tfScalar & z() const
TEST(UTMPoint, nullConstructor)
Definition: test_utm.cpp:63
static bool is2D(const UTMPoint &pt)
Definition: utm.h:184
bool isValid(const UTMPoint &pt)
void check_utm_near(const geodesy::UTMPoint &pt1, const geodesy::UTMPoint &pt2, double abs_err)
Definition: test_utm.cpp:47
double easting
easting within grid zone [meters]
Definition: utm.h:126
UTMPoint position
Definition: utm.h:172
void convert(const From &from, To &to)
Definition: wgs84.h:227
geographic_msgs::GeoPoint toMsg(const UTMPoint &from)


geodesy
Author(s): Jack O'Quin
autogenerated on Thu Jun 6 2019 19:56:59