Added GPS and GeodeticCoords tests

corelib/test/CMakeLists.txt

@@ -101,6 +101,16 @@ add_executable(test_link test_link.cpp)
 target_link_libraries(test_link gtest_main rtabmap_core)
 gtest_discover_tests(test_link)
 
+#GPS.h
+add_executable(test_gps test_gps.cpp)
+target_link_libraries(test_gps gtest_main rtabmap_core)
+gtest_discover_tests(test_gps)
+
+#GeodeticCoords.h
+add_executable(test_geodeticcoords test_geodeticcoords.cpp)
+target_link_libraries(test_geodeticcoords gtest_main rtabmap_core)
+gtest_discover_tests(test_geodeticcoords)
+
 #Signature.h
 add_executable(test_signature test_signature.cpp)
 target_link_libraries(test_signature gtest_main rtabmap_core)

corelib/test/test_geodeticcoords.cpp

@@ -0,0 +1,133 @@
+#include <gtest/gtest.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <cmath>
+
+using namespace rtabmap;
+
+namespace {
+constexpr double kTestLatitude = 45.37855;
+constexpr double kTestLongitude = -71.94304;
+} // namespace
+
+static GeodeticCoords makeReferenceOrigin()
+{
+	return GeodeticCoords(kTestLatitude, kTestLongitude, 0.0);
+}
+
+static GeodeticCoords makeReferenceOffset()
+{
+	return GeodeticCoords(kTestLatitude + 0.0003, kTestLongitude + 0.0003, 10.0);
+}
+
+TEST(GeodeticCoordsTest, DefaultConstructorZeros)
+{
+	const GeodeticCoords coords;
+	EXPECT_DOUBLE_EQ(coords.latitude(), 0.0);
+	EXPECT_DOUBLE_EQ(coords.longitude(), 0.0);
+	EXPECT_DOUBLE_EQ(coords.altitude(), 0.0);
+}
+
+TEST(GeodeticCoordsTest, ParameterizedConstructorStoresFields)
+{
+	const double altitude = 36.5;
+
+	const GeodeticCoords coords(kTestLatitude, kTestLongitude, altitude);
+
+	EXPECT_DOUBLE_EQ(coords.latitude(), kTestLatitude);
+	EXPECT_DOUBLE_EQ(coords.longitude(), kTestLongitude);
+	EXPECT_DOUBLE_EQ(coords.altitude(), altitude);
+}
+
+TEST(GeodeticCoordsTest, SettersUpdateFields)
+{
+	GeodeticCoords coords;
+	coords.setLatitude(1.0);
+	coords.setLongitude(2.0);
+	coords.setAltitude(3.0);
+
+	EXPECT_DOUBLE_EQ(coords.latitude(), 1.0);
+	EXPECT_DOUBLE_EQ(coords.longitude(), 2.0);
+	EXPECT_DOUBLE_EQ(coords.altitude(), 3.0);
+}
+
+TEST(GeodeticCoordsTest, ToGeocentricWgs84RoundTrip)
+{
+	const GeodeticCoords coords = makeReferenceOffset();
+	const cv::Point3d geocentric = coords.toGeocentric_WGS84();
+
+	GeodeticCoords restored;
+	restored.fromGeocentric_WGS84(geocentric);
+
+	EXPECT_NEAR(restored.latitude(), coords.latitude(), 1e-9);
+	EXPECT_NEAR(restored.longitude(), coords.longitude(), 1e-9);
+	EXPECT_NEAR(restored.altitude(), coords.altitude(), 1e-3);
+}
+
+TEST(GeodeticCoordsTest, ToEnuWgs84RelativeToOrigin)
+{
+	const GeodeticCoords origin = makeReferenceOrigin();
+	const GeodeticCoords offset = makeReferenceOffset();
+	const cv::Point3d enu = offset.toENU_WGS84(origin);
+
+	EXPECT_GT(enu.x, 0.0); // east
+	EXPECT_GT(enu.y, 0.0); // north
+	EXPECT_NEAR(enu.z, 10.0, 0.5);
+}
+
+TEST(GeodeticCoordsTest, OriginHasZeroEnu)
+{
+	const GeodeticCoords origin = makeReferenceOrigin();
+	const cv::Point3d enu = origin.toENU_WGS84(origin);
+
+	EXPECT_NEAR(enu.x, 0.0, 1e-6);
+	EXPECT_NEAR(enu.y, 0.0, 1e-6);
+	EXPECT_NEAR(enu.z, 0.0, 1e-6);
+}
+
+TEST(GeodeticCoordsTest, FromEnuWgs84RoundTrip)
+{
+	const GeodeticCoords origin = makeReferenceOrigin();
+	const GeodeticCoords expected = makeReferenceOffset();
+	const cv::Point3d enu = expected.toENU_WGS84(origin);
+
+	GeodeticCoords restored;
+	restored.fromENU_WGS84(enu, origin);
+
+	EXPECT_NEAR(restored.latitude(), expected.latitude(), 1e-5);
+	EXPECT_NEAR(restored.longitude(), expected.longitude(), 1e-5);
+	EXPECT_NEAR(restored.altitude(), expected.altitude(), 0.1);
+}
+
+TEST(GeodeticCoordsTest, StaticGeocentricToEnuMatchesMemberMethod)
+{
+	const GeodeticCoords origin = makeReferenceOrigin();
+	const GeodeticCoords point = makeReferenceOffset();
+	const cv::Point3d geocentric = point.toGeocentric_WGS84();
+	const cv::Point3d originGeocentric = origin.toGeocentric_WGS84();
+
+	const cv::Point3d enuMember = point.toENU_WGS84(origin);
+	const cv::Point3d enuStatic = GeodeticCoords::Geocentric_WGS84ToENU_WGS84(
+			geocentric,
+			originGeocentric,
+			origin);
+
+	EXPECT_NEAR(enuStatic.x, enuMember.x, 1e-6);
+	EXPECT_NEAR(enuStatic.y, enuMember.y, 1e-6);
+	EXPECT_NEAR(enuStatic.z, enuMember.z, 1e-6);
+}
+
+TEST(GeodeticCoordsTest, StaticEnuToGeocentricMatchesMemberPath)
+{
+	const GeodeticCoords origin = makeReferenceOrigin();
+	const GeodeticCoords expected = makeReferenceOffset();
+	const cv::Point3d enu = expected.toENU_WGS84(origin);
+
+	const cv::Point3d geocentricStatic = GeodeticCoords::ENU_WGS84ToGeocentric_WGS84(enu, origin);
+
+	GeodeticCoords restored;
+	restored.fromGeocentric_WGS84(geocentricStatic);
+
+	EXPECT_NEAR(restored.latitude(), expected.latitude(), 1e-5);
+	EXPECT_NEAR(restored.longitude(), expected.longitude(), 1e-5);
+	EXPECT_NEAR(restored.altitude(), expected.altitude(), 0.1);
+}

corelib/test/test_gps.cpp

@@ -0,0 +1,77 @@
+#include <gtest/gtest.h>
+#include <rtabmap/core/GPS.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <cmath>
+
+using namespace rtabmap;
+
+namespace {
+constexpr double kTestLatitude = 45.37855;
+constexpr double kTestLongitude = -71.94304;
+} // namespace
+
+TEST(GPSTest, DefaultConstructorZeros)
+{
+	const GPS gps;
+	EXPECT_DOUBLE_EQ(gps.stamp(), 0.0);
+	EXPECT_DOUBLE_EQ(gps.longitude(), 0.0);
+	EXPECT_DOUBLE_EQ(gps.latitude(), 0.0);
+	EXPECT_DOUBLE_EQ(gps.altitude(), 0.0);
+	EXPECT_DOUBLE_EQ(gps.error(), 0.0);
+	EXPECT_DOUBLE_EQ(gps.bearing(), 0.0);
+}
+
+TEST(GPSTest, ParameterizedConstructorStoresFields)
+{
+	const double stamp = 12345.678;
+	const double altitude = 36.5;
+	const double error = 2.5;
+	const double bearing = 127.25;
+
+	const GPS gps(stamp, kTestLongitude, kTestLatitude, altitude, error, bearing);
+
+	EXPECT_DOUBLE_EQ(gps.stamp(), stamp);
+	EXPECT_DOUBLE_EQ(gps.longitude(), kTestLongitude);
+	EXPECT_DOUBLE_EQ(gps.latitude(), kTestLatitude);
+	EXPECT_DOUBLE_EQ(gps.altitude(), altitude);
+	EXPECT_DOUBLE_EQ(gps.error(), error);
+	EXPECT_DOUBLE_EQ(gps.bearing(), bearing);
+}
+
+TEST(GPSTest, ToGeodeticCoordsMapsLatLonAlt)
+{
+	const GPS gps(1.0, kTestLongitude, kTestLatitude, 250.0, 1.0, 90.0);
+	const GeodeticCoords coords = gps.toGeodeticCoords();
+
+	EXPECT_DOUBLE_EQ(coords.latitude(), gps.latitude());
+	EXPECT_DOUBLE_EQ(coords.longitude(), gps.longitude());
+	EXPECT_DOUBLE_EQ(coords.altitude(), gps.altitude());
+}
+
+TEST(GPSTest, ToGeodeticCoordsGeocentricRoundTrip)
+{
+	const GPS gps(0.0, kTestLongitude, kTestLatitude, 36.5, 0.0, 0.0);
+	const GeodeticCoords coords = gps.toGeodeticCoords();
+	const cv::Point3d geocentric = coords.toGeocentric_WGS84();
+
+	GeodeticCoords restored;
+	restored.fromGeocentric_WGS84(geocentric);
+
+	EXPECT_NEAR(restored.latitude(), gps.latitude(), 1e-9);
+	EXPECT_NEAR(restored.longitude(), gps.longitude(), 1e-9);
+	EXPECT_NEAR(restored.altitude(), gps.altitude(), 1e-3);
+}
+
+TEST(GPSTest, ToGeodeticCoordsEnuRelativeToOrigin)
+{
+	const GPS origin(0.0, kTestLongitude, kTestLatitude, 0.0, 0.0, 0.0);
+	const GPS offset(0.0, kTestLongitude + 0.0003, kTestLatitude + 0.0003, 10.0, 0.0, 0.0);
+
+	const GeodeticCoords originCoords = origin.toGeodeticCoords();
+	const GeodeticCoords offsetCoords = offset.toGeodeticCoords();
+	const cv::Point3d enu = offsetCoords.toENU_WGS84(originCoords);
+
+	EXPECT_GT(enu.x, 0.0); // east
+	EXPECT_GT(enu.y, 0.0); // north
+	EXPECT_NEAR(enu.z, 10.0, 0.5);
+}