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

Check GeographicLib version

release/4.3a0
dellaert 2018-11-06 17:32:31 -05:00
parent 2ee72806ab
commit 8e7892875a
2 changed files with 20 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -22,19 +22,26 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <GeographicLib/Config.h>
#include <GeographicLib/LocalCartesian.hpp> #include <GeographicLib/LocalCartesian.hpp>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using namespace GeographicLib; using namespace GeographicLib;
#if GEOGRAPHICLIB_VERSION_MINOR<37
static const auto& kWGS84 = Geocentric::WGS84;
#else
static const auto& kWGS84 = Geocentric::WGS84();
#endif
// ************************************************************************* // *************************************************************************
namespace example { namespace example {
// ENU Origin is where the plane was in hold next to runway // ENU Origin is where the plane was in hold next to runway
const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274; const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
// Convert from GPS to ENU // Convert from GPS to ENU
LocalCartesian origin_ENU(lat0, lon0, h0, Geocentric::WGS84); LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84);
// Dekalb-Peachtree Airport runway 2L // Dekalb-Peachtree Airport runway 2L
const double lat = 33.87071, lon = -84.30482, h = 274; const double lat = 33.87071, lon = -84.30482, h = 274;
@ -107,8 +114,7 @@ TEST(GPSData, init) {
// GPS Reading 1 will be ENU origin // GPS Reading 1 will be ENU origin
double t1 = 84831; double t1 = 84831;
Point3 NED1(0, 0, 0); Point3 NED1(0, 0, 0);
LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54, LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54, kWGS84);
Geocentric::WGS84);
// GPS Readin 2 // GPS Readin 2
double t2 = 84831.5; double t2 = 84831.5;

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

@ -15,6 +15,7 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <GeographicLib/Config.h>
#include <GeographicLib/Geocentric.hpp> #include <GeographicLib/Geocentric.hpp>
#include <GeographicLib/UTMUPS.hpp> #include <GeographicLib/UTMUPS.hpp>
#include <GeographicLib/LocalCartesian.hpp> #include <GeographicLib/LocalCartesian.hpp>
@ -29,21 +30,27 @@ using namespace std;
using namespace GeographicLib; using namespace GeographicLib;
// Dekalb-Peachtree Airport runway 2L // Dekalb-Peachtree Airport runway 2L
const double lat = 33.87071, lon = -84.30482, h = 274; static const double lat = 33.87071, lon = -84.30482, h = 274;
#if GEOGRAPHICLIB_VERSION_MINOR<37
static const auto& kWGS84 = Geocentric::WGS84;
#else
static const auto& kWGS84 = Geocentric::WGS84();
#endif
//************************************************************************** //**************************************************************************
TEST( GeographicLib, Geocentric) { TEST( GeographicLib, Geocentric) {
// From lat-lon to geocentric // From lat-lon to geocentric
double X, Y, Z; double X, Y, Z;
Geocentric::WGS84.Forward(lat, lon, h, X, Y, Z); kWGS84.Forward(lat, lon, h, X, Y, Z);
EXPECT_DOUBLES_EQUAL(526, X/1000, 1); EXPECT_DOUBLES_EQUAL(526, X/1000, 1);
EXPECT_DOUBLES_EQUAL(-5275, Y/1000, 1); EXPECT_DOUBLES_EQUAL(-5275, Y/1000, 1);
EXPECT_DOUBLES_EQUAL(3535, Z/1000, 1); EXPECT_DOUBLES_EQUAL(3535, Z/1000, 1);
// From geocentric to lat-lon // From geocentric to lat-lon
double lat_, lon_, h_; double lat_, lon_, h_;
Geocentric::WGS84.Reverse(X, Y, Z, lat_, lon_, h_); kWGS84.Reverse(X, Y, Z, lat_, lon_, h_);
EXPECT_DOUBLES_EQUAL(lat, lat_, 1e-5); EXPECT_DOUBLES_EQUAL(lat, lat_, 1e-5);
EXPECT_DOUBLES_EQUAL(lon, lon_, 1e-5); EXPECT_DOUBLES_EQUAL(lon, lon_, 1e-5);
EXPECT_DOUBLES_EQUAL(h, h_, 1e-5); EXPECT_DOUBLES_EQUAL(h, h_, 1e-5);
@ -69,11 +76,9 @@ TEST( GeographicLib, UTM) {
//************************************************************************** //**************************************************************************
TEST( GeographicLib, ENU) { TEST( GeographicLib, ENU) {
const Geocentric& earth = Geocentric::WGS84;
// ENU Origin is where the plane was in hold next to runway // ENU Origin is where the plane was in hold next to runway
const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274; const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
LocalCartesian enu(lat0, lon0, h0, earth); LocalCartesian enu(lat0, lon0, h0, kWGS84);
// From lat-lon to geocentric // From lat-lon to geocentric
double E, N, U; double E, N, U;