12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Convert to UTM

release/4.3a0
dellaert 2014-01-25 19:45:10 -05:00
parent 32455c3817
commit 94e045e864
1 changed files with 31 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
gtsam/navigation/tests/testGeographicLib.cpp
View File

@ -16,6 +16,7 @@
*/
#include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/UTMUPS.hpp>
#include <CppUnitLite/TestHarness.h>
#include <iostream>
@ -24,30 +25,47 @@ using namespace std;
//using namespace gtsam;
using namespace GeographicLib;
/* ************************************************************************* */
// Dekalb-Peachtree Airport runway 2L
double lat = 33.87071, lon = -84.30482000000001, h = 274;
//**************************************************************************
TEST( GeographicLib, Geocentric) {
Geocentric earth(Constants::WGS84_a(), Constants::WGS84_f());
// Dekalb-Peachtree Airport runway 2L
double lat = 33.87071, lon = -84.30482000000001, h = 274;
// From lat-lon to geocentric
double X, Y, Z;
earth.Forward(lat, lon, h, X, Y, Z);
Geocentric::WGS84.Forward(lat, lon, h, X, Y, Z);
EXPECT_DOUBLES_EQUAL(526, X/1000, 1);
EXPECT_DOUBLES_EQUAL(-5275, Y/1000, 1);
EXPECT_DOUBLES_EQUAL(3535, Z/1000, 1);
// UTM is 45N 250694.42 3751090.08
// Obtained by
// http://geographiclib.sourceforge.net/cgi-bin/GeoConvert?input=33.87071+84.30482000000001&zone=-3&prec=2&option=Submit
// From geocentric to lat-lon
double lat_, lon_, h_;
Geocentric::WGS84.Reverse(X, Y, Z, lat_, lon_, h_);
EXPECT_DOUBLES_EQUAL(lat, lat_, 1e-5);
EXPECT_DOUBLES_EQUAL(lon, lon_, 1e-5);
EXPECT_DOUBLES_EQUAL(h, h_, 1e-5);
}
/* ************************************************************************* */
//**************************************************************************
TEST( GeographicLib, UTM) {
// From lat-lon to UTM
int zone;
bool northp;
double x, y;
UTMUPS::Forward(lat, lon, zone, northp, x, y);
// UTM is 16N 749305.58 3751090.08
// Obtained by
// http://geographiclib.sourceforge.net/cgi-bin/GeoConvert?input=33.87071+-84.30482000000001&zone=-3&prec=2&option=Submit
EXPECT(UTMUPS::EncodeZone(zone, northp)=="16N");
EXPECT_DOUBLES_EQUAL(749305.58, x, 1e-2);
EXPECT_DOUBLES_EQUAL(3751090.08, y, 1e-2);
}
//**************************************************************************
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */
//**************************************************************************