geodesy: geodesy: test_utm.cpp Source File

00001 /* $Id: 28df191ac9d0060f7f6db0f987546cc9334e2379 $ */
00002 
00003 /*********************************************************************
00004 * Software License Agreement (BSD License)
00005 *
00006 *  Copyright (c) 2011 Jack O'Quin
00007 *  All rights reserved.
00008 *
00009 *  Redistribution and use in source and binary forms, with or without
00010 *  modification, are permitted provided that the following conditions
00011 *  are met:
00012 *
00013 *   * Redistributions of source code must retain the above copyright
00014 *     notice, this list of conditions and the following disclaimer.
00015 *   * Redistributions in binary form must reproduce the above
00016 *     copyright notice, this list of conditions and the following
00017 *     disclaimer in the documentation and/or other materials provided
00018 *     with the distribution.
00019 *   * Neither the name of the author nor other contributors may be
00020 *     used to endorse or promote products derived from this software
00021 *     without specific prior written permission.
00022 *
00023 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 *  POSSIBILITY OF SUCH DAMAGE.
00035 *********************************************************************/
00036 
00037 #include <sstream>
00038 #include <gtest/gtest.h>
00039 #include "geodesy/utm.h"
00040 
00041 
00043 // Utility functions
00045 
00046 // check that two UTM points are near each other
00047 void check_utm_near(const geodesy::UTMPoint &pt1,
00048                     const geodesy::UTMPoint &pt2,
00049                     double abs_err)
00050 {
00051   EXPECT_NEAR(pt1.easting, pt2.easting, abs_err);
00052   EXPECT_NEAR(pt1.northing, pt2.northing, abs_err);
00053   EXPECT_NEAR(pt1.altitude, pt2.altitude, abs_err);
00054   EXPECT_EQ(pt1.zone, pt2.zone);
00055   EXPECT_EQ(pt1.band, pt2.band);
00056 }
00057 
00059 // Test cases
00061 
00062 // Test null constructor
00063 TEST(UTMPoint, nullConstructor)
00064 {
00065   geodesy::UTMPoint pt;
00066 
00067   EXPECT_EQ(pt.easting, 0.0);
00068   EXPECT_EQ(pt.northing, 0.0);
00069   EXPECT_TRUE(geodesy::is2D(pt));
00070   EXPECT_EQ(pt.zone, 0);
00071   EXPECT_EQ(pt.band, ' ');
00072   EXPECT_FALSE(geodesy::isValid(pt));
00073 }
00074 
00075 // Test 2D constructor
00076 TEST(UTMPoint, flatConstructor)
00077 {
00078   double e = 1000.0;
00079   double n = 2400.0;
00080   uint8_t z = 14;
00081   char b = 'R';
00082   geodesy::UTMPoint pt(e, n, z, b);
00083 
00084   EXPECT_TRUE(geodesy::is2D(pt));
00085   EXPECT_TRUE(geodesy::isValid(pt));
00086 
00087   EXPECT_EQ(pt.easting, e);
00088   EXPECT_EQ(pt.northing, n);
00089   EXPECT_EQ(pt.zone, z);
00090   EXPECT_EQ(pt.band, b);
00091 }
00092 
00093 // Test 3D constructor
00094 TEST(UTMPoint, hasAltitude)
00095 {
00096   double e = 1000.0;
00097   double n = 2400.0;
00098   double a = 200.0;
00099   uint8_t z = 14;
00100   char b = 'R';
00101   geodesy::UTMPoint pt(e, n, a, z, b);
00102 
00103   EXPECT_FALSE(geodesy::is2D(pt));
00104   EXPECT_TRUE(geodesy::isValid(pt));
00105 
00106   EXPECT_EQ(pt.easting, e);
00107   EXPECT_EQ(pt.northing, n);
00108   EXPECT_EQ(pt.zone, z);
00109   EXPECT_EQ(pt.band, b);
00110 }
00111 
00112 // Test copy constructor
00113 TEST(UTMPoint, copyConstructor)
00114 {
00115   double e = 1000.0;
00116   double n = 2400.0;
00117   double a = 200.0;
00118   uint8_t z = 14;
00119   char b = 'R';
00120   geodesy::UTMPoint pt1(e, n, a, z, b);
00121   geodesy::UTMPoint pt2(pt1);
00122 
00123   EXPECT_FALSE(geodesy::is2D(pt2));
00124   EXPECT_TRUE(geodesy::isValid(pt2));
00125 
00126   EXPECT_EQ(pt2.easting, e);
00127   EXPECT_EQ(pt2.northing, n);
00128   EXPECT_EQ(pt2.zone, z);
00129   EXPECT_EQ(pt2.band, b);
00130 }
00131 
00132 // Test UTM point constructor from WGS 84
00133 TEST(UTMPoint, fromLatLong)
00134 {
00135   // University of Texas, Austin, Pickle Research Campus
00136   double lat = 30.385315;
00137   double lon = -97.728524;
00138   double alt = 209.0;
00139 
00140   geographic_msgs::GeoPoint ll;
00141   ll.latitude = lat;
00142   ll.longitude = lon;
00143   ll.altitude = alt;
00144 
00145   // create UTM from point
00146   geodesy::UTMPoint pt(ll);
00147 
00148   double e = 622159.34;
00149   double n = 3362168.30;
00150   uint8_t z = 14;
00151   char b = 'R';
00152   double abs_err = 0.01;
00153 
00154   EXPECT_FALSE(geodesy::is2D(pt));
00155   EXPECT_TRUE(geodesy::isValid(pt));
00156 
00157   EXPECT_NEAR(pt.easting, e, abs_err);
00158   EXPECT_NEAR(pt.northing, n, abs_err);
00159   EXPECT_NEAR(pt.altitude, alt, abs_err);
00160   EXPECT_EQ(pt.zone, z);
00161   EXPECT_EQ(pt.band, b);
00162 }
00163 
00164 // Test zone numbers
00165 TEST(UTMPoint, testZones)
00166 {
00167   geodesy::UTMPoint pt;
00168   pt.band = 'X';                        // supply a valid band letter
00169 
00170   pt.zone = 0;
00171   EXPECT_FALSE(geodesy::isValid(pt));
00172 
00173   pt.zone = 61;
00174   EXPECT_FALSE(geodesy::isValid(pt));
00175 
00176   pt.zone = 255;
00177   EXPECT_FALSE(geodesy::isValid(pt));
00178 
00179   // these should all work
00180   for (uint8_t b = 1; b <= 60; ++b)
00181     {
00182       pt.zone = b;
00183       EXPECT_TRUE(geodesy::isValid(pt));
00184     }
00185 }
00186 
00187 // Test band letters
00188 TEST(UTMPoint, testBands)
00189 {
00190   geodesy::UTMPoint pt;
00191   pt.zone = 14;                         // supply a valid zone number
00192   EXPECT_FALSE(geodesy::isValid(pt));
00193 
00194   pt.band = '9';
00195   EXPECT_FALSE(geodesy::isValid(pt));
00196 
00197   pt.band = ';';
00198   EXPECT_FALSE(geodesy::isValid(pt));
00199 
00200   pt.band = 'I';
00201   EXPECT_FALSE(geodesy::isValid(pt));
00202 
00203   pt.band = 'O';
00204   EXPECT_FALSE(geodesy::isValid(pt));
00205 
00206   pt.band = 'Y';
00207   EXPECT_FALSE(geodesy::isValid(pt));
00208 
00209   pt.band = 'r';
00210   EXPECT_FALSE(geodesy::isValid(pt));
00211 
00212   // this should work
00213   pt.band = 'X';
00214   EXPECT_TRUE(geodesy::isValid(pt));
00215 }
00216 
00217 // Test null pose constructor
00218 TEST(UTMPose, nullConstructor)
00219 {
00220   geodesy::UTMPose pose;
00221 
00222   EXPECT_TRUE(geodesy::is2D(pose));
00223   EXPECT_FALSE(geodesy::isValid(pose));
00224 }
00225 
00226 // Test pose constructor from point and quaternion
00227 TEST(UTMPose, pointQuaternionConstructor)
00228 {
00229   double e = 1000.0;
00230   double n = 2400.0;
00231   double a = 200.0;
00232   uint8_t z = 14;
00233   char b = 'R';
00234   geodesy::UTMPoint pt(e, n, a, z, b);
00235 
00236   geometry_msgs::Quaternion q;          // identity quaternion
00237   q.x = 1.0;
00238   q.y = 0.0;
00239   q.z = 0.0;
00240   q.w = 0.0;
00241   geodesy::UTMPose pose(pt, q);
00242 
00243   EXPECT_FALSE(geodesy::is2D(pose));
00244   EXPECT_TRUE(geodesy::isValid(pose));
00245 
00246   EXPECT_EQ(pose.position.easting, pt.easting);
00247   EXPECT_EQ(pose.position.northing, pt.northing);
00248   EXPECT_EQ(pose.position.altitude, pt.altitude);
00249   EXPECT_EQ(pose.position.zone, pt.zone);
00250   EXPECT_EQ(pose.position.band, pt.band);
00251 
00252   EXPECT_EQ(pose.orientation.w, q.w);
00253   EXPECT_EQ(pose.orientation.x, q.x);
00254   EXPECT_EQ(pose.orientation.y, q.y);
00255   EXPECT_EQ(pose.orientation.z, q.z);
00256 }
00257 
00258 // Test pose quaternion validation
00259 TEST(UTMPose, quaternionValidation)
00260 {
00261   double e = 1000.0;
00262   double n = 2400.0;
00263   double a = 200.0;
00264   uint8_t z = 14;
00265   char b = 'R';
00266   geodesy::UTMPoint pt(e, n, a, z, b);
00267 
00268   // identity quaternion
00269   geometry_msgs::Quaternion q;
00270   q.x = 1.0;
00271   q.y = 0.0;
00272   q.z = 0.0;
00273   q.w = 0.0;
00274   geodesy::UTMPose pose1(pt, q);
00275   EXPECT_TRUE(geodesy::isValid(pose1));
00276 
00277   // valid quaternion
00278   q.x = 0.7071;
00279   q.y = 0.0;
00280   q.z = 0.0;
00281   q.w = 0.7071;
00282   geodesy::UTMPose pose2(pt, q);
00283   EXPECT_TRUE(geodesy::isValid(pose2));
00284 
00285   // quaternion not normalized
00286   q.x = 0.8071;
00287   q.y = 0.0;
00288   q.z = 0.0;
00289   q.w = 0.8071;
00290   geodesy::UTMPose pose3(pt, q);
00291   EXPECT_FALSE(geodesy::isValid(pose3));
00292 
00293   // zero quaternion (not normalized)
00294   q.x = 0.0;
00295   q.y = 0.0;
00296   q.z = 0.0;
00297   q.w = 0.0;
00298   geodesy::UTMPose pose4(pt, q);
00299   EXPECT_FALSE(geodesy::isValid(pose4));
00300 }
00301 
00302 // Test conversion from UTM to WGS 84 and back
00303 TEST(UTMConvert, fromUtmToLatLongAndBack)
00304 {
00305   double e = 500000.0;                  // central meridian of each zone
00306   double n = 1000.0;
00307   double alt = 100.0;
00308   char b = 'N';
00309 
00310   // try every possible zone of longitude
00311   for (uint8_t z = 1; z <= 60; ++z)
00312     {
00313       geodesy::UTMPoint pt1(e, n, alt, z, b);
00314       geographic_msgs::GeoPoint ll;
00315       convert(pt1, ll);
00316       geodesy::UTMPoint pt2;
00317       convert(ll, pt2);
00318 
00319       EXPECT_TRUE(geodesy::isValid(pt1));
00320       EXPECT_TRUE(geodesy::isValid(pt2));
00321       check_utm_near(pt1, pt2, 0.000001);
00322     }
00323 }
00324 
00325 // Test conversion from WGS 84 to UTM and back
00326 TEST(UTMConvert, fromLatLongToUtmAndBack)
00327 {
00328   double alt = 100.0;
00329 
00330   // Try every possible degree of latitude and longitude. Avoid the
00331   // international date line. Even though the converted longitude is
00332   // close, it may end up less than -180 and hence inValid().
00333   for (double lon = -179.5; lon < 180.0; lon += 1.0)
00334     {
00335       for (double lat = -80.0; lat <= 84.0; lat += 1.0)
00336         {
00337           geographic_msgs::GeoPoint pt1(geodesy::toMsg(lat, lon, alt));
00338           EXPECT_TRUE(geodesy::isValid(pt1));
00339 
00340           geodesy::UTMPoint utm(pt1);
00341           EXPECT_TRUE(geodesy::isValid(utm));
00342 
00343           geographic_msgs::GeoPoint pt2(geodesy::toMsg(utm));
00344           EXPECT_TRUE(geodesy::isValid(pt2));
00345 
00346           EXPECT_NEAR(pt1.latitude,  pt2.latitude,  0.0000001);
00347           EXPECT_NEAR(pt1.longitude, pt2.longitude, 0.0000012);
00348           EXPECT_NEAR(pt1.altitude,  pt2.altitude,  0.000001);
00349         }
00350     }
00351 }
00352 
00353 // Test conversion from WGS 84 to UTM and back at international date line
00354 TEST(UTMConvert, internationalDateLine)
00355 {
00356   double alt = 100.0;
00357   double lon = -180.0;
00358 
00359   for (double lat = -80.0; lat <= 84.0; lat += 1.0)
00360     {
00361       geographic_msgs::GeoPoint pt1(geodesy::toMsg(lat, lon, alt));
00362       EXPECT_TRUE(geodesy::isValid(pt1));
00363 
00364       geodesy::UTMPoint utm;
00365       geodesy::fromMsg(pt1, utm);
00366       EXPECT_TRUE(geodesy::isValid(utm));
00367 
00368       geographic_msgs::GeoPoint pt2(geodesy::toMsg(utm));
00369       EXPECT_TRUE(geodesy::isValid(pt2));
00370       EXPECT_NEAR(pt1.latitude,  pt2.latitude,  0.0000001);
00371       EXPECT_NEAR(pt1.altitude,  pt2.altitude,  0.000001);
00372 
00373       if (pt2.longitude - pt1.longitude > 359.0)
00374         {
00375           // pt2 seems to be slightly across the international date
00376           // line from pt2, so de-normalize it
00377           pt2.longitude -= 360.0;
00378         }
00379       EXPECT_NEAR(pt1.longitude, pt2.longitude, 0.0000012);
00380     }
00381 }
00382 
00383 // Test point output stream operator
00384 TEST(OStream, point)
00385 {
00386   geodesy::UTMPoint pt1;
00387   std::ostringstream out1;
00388   out1 << pt1;
00389   std::string expected("(0, 0, nan [0 ])");
00390   EXPECT_EQ(out1.str(), expected);
00391 
00392   geodesy::UTMPoint pt2(622159.338, 3362168.303, 209, 14, 'R');
00393   std::ostringstream out2;
00394   out2 << pt2;
00395   expected = "(622159.338, 3362168.303, 209 [14R])";
00396   EXPECT_EQ(out2.str(), expected);
00397 }
00398 
00399 // Test pose output stream operator
00400 TEST(OStream, pose)
00401 {
00402   geodesy::UTMPoint pt(1000.0, 2400.0, 200.0, 14, 'R');
00403   geometry_msgs::Quaternion q;
00404   q.w = 1.0;
00405   q.x = 0.0;
00406   q.y = 0.0;
00407   q.z = 0.0;
00408   geodesy::UTMPose pose(pt, q);
00409 
00410   std::ostringstream out;
00411   out << pose;
00412   std::string expected("(1000, 2400, 200 [14R]), ([0, 0, 0], 1)");
00413   EXPECT_EQ(out.str(), expected);
00414 }
00415 
00416 // Run all the tests that were declared with TEST()
00417 int main(int argc, char **argv)
00418 {
00419   ros::init(argc, argv, "test_utm_cpp");
00420   testing::InitGoogleTest(&argc, argv);
00421 
00422   // run the tests in this thread
00423   return RUN_ALL_TESTS();
00424 }