Added GPS and GeodeticCoords tests

corelib/include/rtabmap/core/GPS.h

@@ -32,9 +32,21 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class GPS
+ * @brief WGS84 GPS fix attached to a sensor sample or graph node.
+ *
+ * Stores a timestamped geographic position with horizontal accuracy and bearing.
+ * Values are persisted in the database as six doubles in the order
+ * @c stamp, @c longitude, @c latitude, @c altitude, @c error, @c bearing.
+ *
+ * @see SensorData::gps()
+ * @see GeodeticCoords
+ */
 class GPS
 {
 public:
+	/** @brief Default-constructs a fix at the origin with zero stamp and error. */
 	GPS():
 		stamp_(0.0),
 		longitude_(0.0),
@@ -43,6 +55,15 @@ public:
 		error_(0.0),
 		bearing_(0.0)
 	{}
+	/**
+	 * @brief Constructs a GPS fix.
+	 * @param stamp Timestamp in seconds.
+	 * @param longitude Longitude in decimal degrees (DD, east positive).
+	 * @param latitude Latitude in decimal degrees (DD, north positive).
+	 * @param altitude Altitude in meters above the WGS84 ellipsoid.
+	 * @param error Horizontal position error radius in meters.
+	 * @param bearing Heading in degrees, 0 = north, increasing clockwise.
+	 */
 	GPS(const double & stamp,
 		const double & longitude,
 		const double & latitude,
@@ -56,13 +77,22 @@ public:
 			error_(error),
 			bearing_(bearing)
 	{}
+	/** @return Timestamp in seconds. */
 	const double & stamp() const {return stamp_;}
+	/** @return Longitude in decimal degrees (DD). */
 	const double & longitude() const {return longitude_;}
+	/** @return Latitude in decimal degrees (DD). */
 	const double & latitude() const {return latitude_;}
+	/** @return Altitude in meters. */
 	const double & altitude() const {return altitude_;}
+	/** @return Horizontal position error in meters. */
 	const double & error() const {return error_;}
+	/** @return Bearing in degrees (north = 0, clockwise). */
 	const double & bearing() const {return bearing_;}
 
+	/**
+	 * @return @ref GeodeticCoords built from @ref latitude(), @ref longitude() and @ref altitude().
+	 */
 	GeodeticCoords toGeodeticCoords() const {return GeodeticCoords(latitude_, longitude_, altitude_);}
 private:
 	double stamp_;     // in sec

corelib/include/rtabmap/core/GeodeticCoords.h

@@ -49,27 +49,61 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class GeodeticCoords
+ * @brief WGS84 geodetic latitude, longitude and altitude with coordinate conversions.
+ *
+ * Conversions follow the WGS84 reference ellipsoid (MRPT-derived implementation).
+ * ENU frames use east = X, north = Y, up = Z relative to a local origin.
+ *
+ * @see GPS::toGeodeticCoords()
+ */
 class RTABMAP_CORE_EXPORT GeodeticCoords
 {
 public:
+	/** @brief Default-constructs coordinates at (0°, 0°, 0 m). */
 	GeodeticCoords();
+	/**
+	 * @brief Constructs geodetic coordinates.
+	 * @param latitude Latitude in decimal degrees (DD, north positive).
+	 * @param longitude Longitude in decimal degrees (DD, east positive).
+	 * @param altitude Altitude in meters above the WGS84 ellipsoid.
+	 */
 	GeodeticCoords(double latitude, double longitude, double altitude);
 
+	/** @return Latitude in decimal degrees. */
 	const double & latitude() const {return latitude_;}
+	/** @return Longitude in decimal degrees. */
 	const double & longitude() const {return longitude_;}
+	/** @return Altitude in meters. */
 	const double & altitude() const {return altitude_;}
 
+	/** @brief Sets latitude in decimal degrees. */
 	void setLatitude(const double & value) {latitude_ = value;}
+	/** @brief Sets longitude in decimal degrees. */
 	void setLongitude(const double & value) {longitude_ = value;}
+	/** @brief Sets altitude in meters. */
 	void setAltitude(const double & value) {altitude_ = value;}
 
+	/** @return ECEF geocentric coordinates (meters) in the WGS84 frame. */
 	cv::Point3d toGeocentric_WGS84() const;
+	/**
+	 * @return ENU offset (meters) from @p origin to this point.
+	 * East = X, north = Y, up = Z.
+	 */
 	cv::Point3d toENU_WGS84(const GeodeticCoords & origin) const; // East=X, North=Y
 
+	/** @brief Sets this point from ECEF geocentric @p geocentric coordinates. */
 	void fromGeocentric_WGS84(const cv::Point3d& geocentric);
+	/** @brief Sets this point from an ENU offset relative to @p origin. */
 	void fromENU_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
 
+	/** @return ECEF geocentric coordinates of ENU point @p enu relative to @p origin. */
 	static cv::Point3d ENU_WGS84ToGeocentric_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
+	/**
+	 * @return ENU offset from geocentric @p origin_geocentric_WGS84 to @p geocentric_WGS84.
+	 * @param origin Geodetic origin used to define the local ENU basis.
+	 */
 	static cv::Point3d Geocentric_WGS84ToENU_WGS84(
 			const cv::Point3d & geocentric_WGS84,
 			const cv::Point3d & origin_geocentric_WGS84,

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);
+}