GeospatialOperations.hpp

Go to the documentation of this file.

 
#ifndef GEOSPATIAL_OPERATIONS_HPP
#define GEOSPATIAL_OPERATIONS_HPP
 
#include "../AutonomyLogging.h"
 
#include <GeographicLib/Geodesic.hpp>
#include <GeographicLib/UTMUPS.hpp>
#include <chrono>
 
 
 
namespace geoops
{
    // Declare public enums.
 
    // This enum is used to store values for the waypoint type.
    enum class WaypointType
    {
        eNavigationWaypoint,
        eTagWaypoint,
        eMalletWaypoint,
        eWaterBottleWaypoint,
        eRockPickWaypoint,
        eObjectWaypoint,    // Used to represent either Mallet, WaterBottle, or RockPick waypoint.
        eObstacleWaypoint,
        eUNKNOWN
    };
 
    // This enum is used to store the location fix type for a UTM or GPS points, the enum indexes follow the UBLOX-F9P docs.
    enum class PositionFixType
    {
        eNoFix,
        eDeadReckoning,
        eFix2D,
        eFix3D,
        eGNSSDeadReckoningCombined,
        eTimeOnly,
        eUNKNOWN
    };
 
    // Declare public variables.
 
    // Declare public structs that are specific to and used within this class.
 
 
    struct GeoMeasurement
    {
        public:
            // Define public struct attributes.
            double dDistanceMeters;          // The great circle distance between the two points.
            double dArcLengthDegrees;        // The degree measurement difference between the two points from the center of the sphere. (0, 180)
            double dStartRelativeBearing;    // The relative bearing in degrees from the first point to the second point for the shortest great circle path. (0, 360)
            double dEndRelativeBearing;      // The relative bearing in degrees from the second point to the first point for the shortest great circle path. (0, 360)
    };
 
 
    struct GPSCoordinate
    {
        public:
            // Declare struct public attributes
            double dLatitude;                              // The geographic latitude in degrees, typically within the range [-90, 90].
            double dLongitude;                             // The geographic longitude in degrees, typically within the range [-180, 180].
            double dAltitude;                              // The elevation above sea level in meters.
            double d2DAccuracy;                            // The horizontal accuracy of the GPS coordinates in meters.
            double d3DAccuracy;                            // The three-dimensional accuracy, including altitude, in meters.
            double dMeridianConvergence;                   // The angle between true north and grid north at the given location in degrees. Positive in eastern direction.
            double dScale;                                 // The scale factor applied to the UTM coordinates for map projection.
            PositionFixType eCoordinateAccuracyFixType;    // The type of satellite location lock the coordinate was taken under.
            bool bIsDifferential;                          // Whether of not the coordinate was taken under a differential GPS lock.
            std::chrono::system_clock::time_point tmTimestamp;    // The chrono time point when the GPS coordinate was created/received.
 
            // Declare public methods.
 
            GPSCoordinate(double dLatitude                                  = 0.0,
                          double dLongitude                                 = 0.0,
                          double dAltitude                                  = 0.0,
                          double d2DAccuracy                                = -1.0,
                          double d3DAccuracy                                = -1.0,
                          double dMeridianConvergence                       = -1.0,
                          double dScale                                     = 0.0,
                          PositionFixType eCoordinateAccuracyFixType        = PositionFixType::eUNKNOWN,
                          bool bIsDifferential                              = false,
                          std::chrono::system_clock::time_point tmTimestamp = std::chrono::system_clock::time_point().min())
            {
                // Initialize member variables with given values.
                this->dLatitude                  = dLatitude;
                this->dLongitude                 = dLongitude;
                this->dAltitude                  = dAltitude;
                this->d2DAccuracy                = d2DAccuracy;
                this->d3DAccuracy                = d3DAccuracy;
                this->dMeridianConvergence       = dMeridianConvergence;
                this->dScale                     = dScale;
                this->eCoordinateAccuracyFixType = eCoordinateAccuracyFixType;
                this->bIsDifferential            = bIsDifferential;
                this->tmTimestamp                = tmTimestamp;
            }
 
 
            bool operator==(const GPSCoordinate& stOtherCoordinate) const
            {
                // Check if location, altitude, and accuracy are the same. Not going to worry about other values for now.
                return (dLatitude == stOtherCoordinate.dLatitude && dLongitude == stOtherCoordinate.dLongitude && dAltitude == stOtherCoordinate.dAltitude &&
                        d2DAccuracy == stOtherCoordinate.d2DAccuracy && d3DAccuracy == stOtherCoordinate.d3DAccuracy &&
                        eCoordinateAccuracyFixType == stOtherCoordinate.eCoordinateAccuracyFixType && bIsDifferential == stOtherCoordinate.bIsDifferential);
            }
 
 
            bool operator!=(const GPSCoordinate& stOtherCoordinate) const { return !this->operator==(stOtherCoordinate); }
 
 
            std::string ToString() const
            {
                std::ostringstream oss;
                oss << "Latitude: " << dLatitude << ", Longitude: " << dLongitude << ", Altitude: " << dAltitude;
                return oss.str();
            }
    };
 
 
    struct UTMCoordinate
    {
        public:
            // Declare struct public attributes.
            double dEasting;                               // The eastward displacement from the UTM zone's central meridian in meters.
            double dNorthing;                              // The northward displacement from the equator in meters.
            int nZone;                                     // The UTM zone number identifying the region on the Earth's surface.
            bool bWithinNorthernHemisphere;                // Indicates whether the coordinate is located in the northern hemisphere.
            double dAltitude;                              // The elevation above sea level in meters.
            double d2DAccuracy;                            // The horizontal accuracy of the UTM coordinates in meters.
            double d3DAccuracy;                            // The three-dimensional accuracy, including altitude, in meters.
            double dMeridianConvergence;                   // The angle between true north and grid north at the given location in degrees. Positive in eastern direction.
            double dScale;                                 // The scale factor applied to the UTM coordinates for map projection.
            PositionFixType eCoordinateAccuracyFixType;    // The type of satellite location lock the coordinate was taken under.
            bool bIsDifferential;                          // Whether of not the coordinate was taken under a differential GPS lock.
            std::chrono::system_clock::time_point tmTimestamp;    // The chrono time point when the GPS coordinate was created/received.
 
            // Declare public methods.
 
 
            UTMCoordinate(double dEasting                                   = 0.0,
                          double dNorthing                                  = 0.0,
                          int nZone                                         = 0,
                          bool bWithinNorthernHemisphere                    = true,
                          double dAltitude                                  = 0.0,
                          double d2DAccuracy                                = -1.0,
                          double d3DAccuracy                                = -1.0,
                          double dMeridianConvergence                       = -1.0,
                          double dScale                                     = 0.0,
                          PositionFixType eCoordinateAccuracyFixType        = PositionFixType::eUNKNOWN,
                          bool bIsDifferential                              = false,
                          std::chrono::system_clock::time_point tmTimestamp = std::chrono::system_clock::time_point().min())
            {
                // Initialize member variables with given values.
                this->dEasting                   = dEasting;
                this->dNorthing                  = dNorthing;
                this->nZone                      = nZone;
                this->bWithinNorthernHemisphere  = bWithinNorthernHemisphere;
                this->dAltitude                  = dAltitude;
                this->d2DAccuracy                = d2DAccuracy;
                this->d3DAccuracy                = d3DAccuracy;
                this->dMeridianConvergence       = dMeridianConvergence;
                this->dScale                     = dScale;
                this->eCoordinateAccuracyFixType = eCoordinateAccuracyFixType;
                this->bIsDifferential            = bIsDifferential;
                this->tmTimestamp                = tmTimestamp;
            }
 
 
            bool operator==(const UTMCoordinate& stOtherCoordinate) const
            {
                // Check if location, altitude, and accuracy are the same. Not going to worry about other values for now.
                return (dEasting == stOtherCoordinate.dEasting && dNorthing == stOtherCoordinate.dNorthing && nZone == stOtherCoordinate.nZone &&
                        bWithinNorthernHemisphere == stOtherCoordinate.bWithinNorthernHemisphere && dAltitude == stOtherCoordinate.dAltitude &&
                        d2DAccuracy == stOtherCoordinate.d2DAccuracy && d3DAccuracy == stOtherCoordinate.d3DAccuracy &&
                        eCoordinateAccuracyFixType == stOtherCoordinate.eCoordinateAccuracyFixType && bIsDifferential == stOtherCoordinate.bIsDifferential);
            }
 
 
            bool operator!=(const UTMCoordinate& stOtherCoordinate) const { return !this->operator==(stOtherCoordinate); }
 
 
            std::string ToString() const
            {
                std::ostringstream oss;
                oss << "Easting: " << dEasting << ", Northing: " << dNorthing << ", Altitude: " << dAltitude;
                return oss.str();
            }
    };
 
 
    inline UTMCoordinate ConvertGPSToUTM(const GPSCoordinate& stGPSCoord)
    {
        // Create instance variables.
        UTMCoordinate stConvertCoord;
 
        // Get data out of the GPS coord and repackage it into the UTM struct.
        stConvertCoord.d2DAccuracy                = stGPSCoord.d2DAccuracy;
        stConvertCoord.d3DAccuracy                = stGPSCoord.d3DAccuracy;
        stConvertCoord.dAltitude                  = stGPSCoord.dAltitude;
        stConvertCoord.eCoordinateAccuracyFixType = stGPSCoord.eCoordinateAccuracyFixType;
        stConvertCoord.bIsDifferential            = stGPSCoord.bIsDifferential;
        stConvertCoord.tmTimestamp                = stGPSCoord.tmTimestamp;
 
        // Catch errors from GeographicLib.
        try
        {
            // Forward solve for the UTM coord.
            GeographicLib::UTMUPS::Forward(stGPSCoord.dLatitude,
                                           stGPSCoord.dLongitude,
                                           stConvertCoord.nZone,
                                           stConvertCoord.bWithinNorthernHemisphere,
                                           stConvertCoord.dEasting,
                                           stConvertCoord.dNorthing,
                                           stConvertCoord.dMeridianConvergence,
                                           stConvertCoord.dScale);
        }
        catch (const GeographicLib::GeographicErr::exception& geError)
        {
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "Unable to forward solve a GPSCoordinate to UTMCoordinate. GeographicLib error is: {}", geError.what());
        }
 
        // Return the converted UTM coordinate.
        return stConvertCoord;
    }
 
 
    inline GPSCoordinate ConvertUTMToGPS(const UTMCoordinate& stUTMCoord)
    {
        // Create instance variables.
        GPSCoordinate stConvertCoord;
 
        // Get data out of the UTM coord and repackage it into the GPS struct.
        stConvertCoord.d2DAccuracy                = stUTMCoord.d2DAccuracy;
        stConvertCoord.d3DAccuracy                = stUTMCoord.d3DAccuracy;
        stConvertCoord.dAltitude                  = stUTMCoord.dAltitude;
        stConvertCoord.eCoordinateAccuracyFixType = stUTMCoord.eCoordinateAccuracyFixType;
        stConvertCoord.bIsDifferential            = stUTMCoord.bIsDifferential;
        stConvertCoord.tmTimestamp                = stUTMCoord.tmTimestamp;
 
        // Catch errors from GeographicLib.
        try
        {
            // Reverse solve for the UTM coord.
            GeographicLib::UTMUPS::Reverse(stUTMCoord.nZone,
                                           stUTMCoord.bWithinNorthernHemisphere,
                                           std::fabs(stUTMCoord.dEasting),
                                           stUTMCoord.dNorthing,
                                           stConvertCoord.dLatitude,
                                           stConvertCoord.dLongitude,
                                           stConvertCoord.dMeridianConvergence,
                                           stConvertCoord.dScale);
        }
        catch (const GeographicLib::GeographicErr::exception& geError)
        {
            // Submit logger message.
            LOG_ERROR(logging::g_qSharedLogger, "Unable to reverse solve a UTMCoordinate to GPSCoordinate. GeographicLib error is: {}", geError.what());
        }
 
        // Return the converted UTM coordinate.
        return stConvertCoord;
    }
 
 
    struct Waypoint
    {
        private:
            // Declare struct private member variables.
            geoops::GPSCoordinate stGPSLocation;    // This is not meant to be changed once this waypoint is created.
            geoops::UTMCoordinate stUTMLocation;    // This is not meant to be changed once this waypoint is created.
 
        public:
            // Declare struct public member variables.
            WaypointType eType;
            double dRadius;
            int nID;
 
 
            Waypoint(const geoops::GPSCoordinate& stGPSLocation = geoops::GPSCoordinate(),
                     const WaypointType& eType                  = WaypointType::eUNKNOWN,
                     const double dRadius                       = 0.0,
                     const int nID                              = -1)
            {
                // Initialize member variables.
                this->stGPSLocation = stGPSLocation;
                this->stUTMLocation = geoops::ConvertGPSToUTM(stGPSLocation);
                this->eType         = eType;
                this->dRadius       = dRadius;
                this->nID           = nID;
            }
 
 
            Waypoint(const geoops::UTMCoordinate& stUTMLocation, const WaypointType& eType = WaypointType::eUNKNOWN, const double dRadius = 0.0, const int nID = -1)
            {
                // Initialize member variables.
                this->stUTMLocation = stUTMLocation;
                this->stGPSLocation = geoops::ConvertUTMToGPS(stUTMLocation);
                this->eType         = eType;
                this->dRadius       = dRadius;
                this->nID           = nID;
            }
 
 
            const geoops::GPSCoordinate& GetGPSCoordinate() const { return stGPSLocation; }
 
 
            const geoops::UTMCoordinate& GetUTMCoordinate() const { return stUTMLocation; }
 
 
            bool operator==(const Waypoint& stOtherWaypoint) const
            {
                // Check if location, altitude, and accuracy are the same. Not going to worry about other values for now.
                return (stGPSLocation == stOtherWaypoint.stGPSLocation && stUTMLocation == stOtherWaypoint.stUTMLocation && eType == stOtherWaypoint.eType &&
                        dRadius == stOtherWaypoint.dRadius && nID == stOtherWaypoint.nID);
            }
 
 
            bool operator!=(const Waypoint& stOtherWaypoint) const { return !this->operator==(stOtherWaypoint); }
    };
 
 
    inline GeoMeasurement CalculateGeoMeasurement(const GPSCoordinate& stCoord1, const GPSCoordinate& stCoord2)
    {
        // Create instance variables.
        GeoMeasurement stMeasurements;
 
        // Construct a geodesic with earth characteristics. (Radius and flattening)
        // The WGS84 standard is widely used and aligns with Google Maps.
        GeographicLib::Geodesic geGeodesic = GeographicLib::Geodesic::WGS84();
 
        // Solve the inverse geodesic for distance and arc length degrees at the center of the globe, and relative bearings.
        stMeasurements.dArcLengthDegrees = geGeodesic.Inverse(stCoord1.dLatitude,
                                                              stCoord1.dLongitude,
                                                              stCoord2.dLatitude,
                                                              stCoord2.dLongitude,
                                                              stMeasurements.dDistanceMeters,
                                                              stMeasurements.dStartRelativeBearing,
                                                              stMeasurements.dEndRelativeBearing);
 
        // NOTE: Regarding azi1 vs azi2, azi1 is the direction measured at point 1 (your navigation aid) to point 2. azi2 is the direction measured at point 2 (your
        // location) away from point 1. (If you want the direction to point 1, add ±180° to azi2.)
        // Map the -180, 180 range of the azimuths to 0, 360, with both points zeroed at North.
        stMeasurements.dStartRelativeBearing = std::fmod((stMeasurements.dStartRelativeBearing + 360), 360);
        stMeasurements.dEndRelativeBearing   = std::fmod((stMeasurements.dEndRelativeBearing + 180), 360);
        // Ensure the result angle is positive.
        if (stMeasurements.dStartRelativeBearing < 0)
        {
            // Add 360 degrees.
            stMeasurements.dStartRelativeBearing += 360;
        }
        // Ensure the result angle is positive.
        if (stMeasurements.dEndRelativeBearing < 0)
        {
            // Add 360 degrees.
            stMeasurements.dEndRelativeBearing += 360;
        }
 
        // Return result distance.
        return stMeasurements;
    }
 
 
    inline GeoMeasurement CalculateGeoMeasurement(const UTMCoordinate& stCoord1, const UTMCoordinate& stCoord2)
    {
        // Create instance variables.
        GeoMeasurement stMeasurements;
 
        // Construct a geodesic with earth characteristics. (Radius and flattening)
        // The WGS84 standard is widely used and aligns with Google Maps.
        GeographicLib::Geodesic geGeodesic = GeographicLib::Geodesic::WGS84();
 
        // Convert the given UTM coords into GPS coords for temporary use.
        GPSCoordinate stGPSCoord1 = ConvertUTMToGPS(stCoord1);
        GPSCoordinate stGPSCoord2 = ConvertUTMToGPS(stCoord2);
 
        // Solve the inverse geodesic.
        stMeasurements.dArcLengthDegrees = geGeodesic.Inverse(stGPSCoord1.dLatitude,
                                                              stGPSCoord1.dLongitude,
                                                              stGPSCoord2.dLatitude,
                                                              stGPSCoord2.dLongitude,
                                                              stMeasurements.dDistanceMeters,
                                                              stMeasurements.dStartRelativeBearing,
                                                              stMeasurements.dEndRelativeBearing);
 
        // NOTE: Regarding azi1 vs azi2, azi1 is the direction measured at point 1 (your navigation aid) to point 2. azi2 is the direction measured at point 2 (your
        // location) away from point 1. (If you want the direction to point 1, add ±180° to azi2.)
        // Map the -180, 180 range of the azimuths to 0, 360, with both points zeroed at North.
        stMeasurements.dStartRelativeBearing = std::fmod((stMeasurements.dStartRelativeBearing + 360), 360);
        stMeasurements.dEndRelativeBearing   = std::fmod((stMeasurements.dEndRelativeBearing + 180), 360);
        // Ensure the result angle is positive.
        if (stMeasurements.dStartRelativeBearing < 0)
        {
            // Add 360 degrees.
            stMeasurements.dStartRelativeBearing += 360;
        }
        // Ensure the result angle is positive.
        if (stMeasurements.dEndRelativeBearing < 0)
        {
            // Add 360 degrees.
            stMeasurements.dEndRelativeBearing += 360;
        }
 
        // Return result distance.
        return stMeasurements;
    }
 
 
    inline GeoMeasurement CalculateGeoMeasurement(const geoops::Waypoint& stWaypoint1, const geoops::Waypoint& stWaypoint2)
    {
        // Create instance variables.
        GeoMeasurement stMeasurements;
 
        // Construct a geodesic with earth characteristics. (Radius and flattening)
        // The WGS84 standard is widely used and aligns with Google Maps.
        GeographicLib::Geodesic geGeodesic = GeographicLib::Geodesic::WGS84();
 
        // Solve the inverse geodesic.
        stMeasurements.dArcLengthDegrees = geGeodesic.Inverse(stWaypoint1.GetGPSCoordinate().dLatitude,
                                                              stWaypoint1.GetGPSCoordinate().dLongitude,
                                                              stWaypoint2.GetGPSCoordinate().dLatitude,
                                                              stWaypoint2.GetGPSCoordinate().dLongitude,
                                                              stMeasurements.dDistanceMeters,
                                                              stMeasurements.dStartRelativeBearing,
                                                              stMeasurements.dEndRelativeBearing);
 
        // NOTE: Regarding azi1 vs azi2, azi1 is the direction measured at point 1 (your navigation aid) to point 2. azi2 is the direction measured at point 2 (your
        // location) away from point 1. (If you want the direction to point 1, add ±180° to azi2.)
        // Map the -180, 180 range of the azimuths to 0, 360, with both points zeroed at North.
        stMeasurements.dStartRelativeBearing = std::fmod((stMeasurements.dStartRelativeBearing + 360), 360);
        stMeasurements.dEndRelativeBearing   = std::fmod((stMeasurements.dEndRelativeBearing + 180), 360);
        // Ensure the result angle is positive.
        if (stMeasurements.dStartRelativeBearing < 0)
        {
            // Add 360 degrees.
            stMeasurements.dStartRelativeBearing += 360;
        }
        // Ensure the result angle is positive.
        if (stMeasurements.dEndRelativeBearing < 0)
        {
            // Add 360 degrees.
            stMeasurements.dEndRelativeBearing += 360;
        }
 
        // Return result distance.
        return stMeasurements;
    }
 
 
    struct RoverPose
    {
        private:
            // Declare struct private member variables.
            Waypoint stRoverPosition;    // Stores the rover's 3D location in UTM and GPS form.
            double dRoverHeading;        // Stores the rover's compass heading. North = 0, CW=pos
 
        public:
 
            RoverPose(const geoops::GPSCoordinate& stRoverPosition = geoops::GPSCoordinate(), const double dRoverHeading = 0.0)
            {
                // Update member variables.
                this->stRoverPosition = Waypoint(stRoverPosition);
                this->dRoverHeading   = dRoverHeading;
            }
 
 
            RoverPose(const geoops::UTMCoordinate& stRoverPosition, const double dRoverHeading = 0.0)
            {
                // Update member variables.
                this->stRoverPosition = Waypoint(stRoverPosition);
                this->dRoverHeading   = dRoverHeading;
            }
 
 
            const geoops::GPSCoordinate& GetGPSCoordinate() const { return stRoverPosition.GetGPSCoordinate(); }
 
 
            const geoops::UTMCoordinate& GetUTMCoordinate() const { return stRoverPosition.GetUTMCoordinate(); }
 
 
            const geoops::Waypoint& GetWaypoint() const { return stRoverPosition; }
 
 
            double GetCompassHeading() const { return dRoverHeading; }
 
 
            bool operator==(const RoverPose& stOtherRoverPose) const
            {
                // Check if location, altitude, and accuracy are the same. Not going to worry about other values for now.
                return (stRoverPosition == stOtherRoverPose.GetGPSCoordinate() && stRoverPosition == stOtherRoverPose.GetUTMCoordinate() &&
                        dRoverHeading == stOtherRoverPose.dRoverHeading);
            }
 
 
            bool operator!=(const RoverPose& stOtherRoverPose) const { return !this->operator==(stOtherRoverPose); }
    };
}    // namespace geoops
#endif