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

Tested local ENU coordinates

release/4.3a0
dellaert 2014-01-25 20:32:22 -05:00
parent 94e045e864
commit 60f0c4e51f
1 changed files with 33 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -17,6 +17,8 @@
#include <GeographicLib/Geocentric.hpp> #include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/UTMUPS.hpp> #include <GeographicLib/UTMUPS.hpp>
#include <GeographicLib/LocalCartesian.hpp>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <iostream> #include <iostream>
@ -26,7 +28,7 @@ using namespace std;
using namespace GeographicLib; using namespace GeographicLib;
// Dekalb-Peachtree Airport runway 2L // Dekalb-Peachtree Airport runway 2L
double lat = 33.87071, lon = -84.30482000000001, h = 274; const double lat = 33.87071, lon = -84.30482, h = 274;
//************************************************************************** //**************************************************************************
TEST( GeographicLib, Geocentric) { TEST( GeographicLib, Geocentric) {
@ -63,6 +65,36 @@ TEST( GeographicLib, UTM) {
EXPECT_DOUBLES_EQUAL(3751090.08, y, 1e-2); EXPECT_DOUBLES_EQUAL(3751090.08, y, 1e-2);
} }
//**************************************************************************
TEST( GeographicLib, ENU) {
const Geocentric& earth = Geocentric::WGS84;
// ENU Origin is where the plane was in hold next to runway
const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
LocalCartesian enu(lat0, lon0, h0, earth);
// From lat-lon to geocentric
double E, N, U;
enu.Forward(lat0, lon0, h0, E, N, U);
EXPECT_DOUBLES_EQUAL(0, E, 1e-2);
EXPECT_DOUBLES_EQUAL(0, N, 1e-2);
EXPECT_DOUBLES_EQUAL(0, U, 1e-2);
// From lat-lon to geocentric
enu.Forward(lat, lon, h, E, N, U);
EXPECT_DOUBLES_EQUAL(133.24, E, 1e-2);
EXPECT_DOUBLES_EQUAL(80.98, N, 1e-2);
EXPECT_DOUBLES_EQUAL(0, U, 1e-2);
// From geocentric to lat-lon
double lat_, lon_, h_;
enu.Reverse(E, N, U, lat_, lon_, h_);
EXPECT_DOUBLES_EQUAL(lat, lat_, 1e-5);
EXPECT_DOUBLES_EQUAL(lon, lon_, 1e-5);
EXPECT_DOUBLES_EQUAL(h, h_, 1e-5);
}
//************************************************************************** //**************************************************************************
int main() { int main() {
TestResult tr; TestResult tr;