Merge pull request #20 from KumarRobotics/feature/differential-gps

dgps starter, the regular version with IMU heading still works but this needs more testing. But I broke main so I need to merge this.

Author: jhughes50

glider/CMakeLists.txt

@@ -59,6 +59,7 @@ add_library(${PROJECT_NAME} SHARED
   src/parameters.cpp
   src/time.cpp
   src/gps_heading.cpp
+  src/differential_gps.cpp
   src/odometry.cpp
   src/odometry_with_covariance.cpp
   src/factor_manager.cpp

glider/include/glider/core/differential_gps.hpp

@@ -0,0 +1,43 @@
+/*!
+*  Jason Hughes
+*  October 2025
+*
+*  Object that handles everything for differenctial gps
+*  from motion
+*/
+
+#pragma once 
+
+#include <cmath>
+#include <string>
+#include <Eigen/Dense>
+
+namespace Glider
+{
+namespace Geodetics
+{
+class DifferentialGpsFromMotion
+{
+    public:
+        DifferentialGpsFromMotion() = default;
+        DifferentialGpsFromMotion(const std::string& frame, const double& threshold);
+
+        double getHeading(const Eigen::Vector3d& gps);
+
+        double headingRadiansToDegrees(const double heading) const;
+
+        void setLastGps(const Eigen::Vector3d& gps);
+        bool isInitialized() const;
+        double getVelocityThreshold() const;
+
+    private:
+
+        double nedToEnu(const double heading) const;
+
+        Eigen::Vector3d last_gps_;
+        std::string frame_;
+        double vel_threshold_;
+        bool initialized_{false};
+};
+} // namespace Geodetics
+} // namespace Glider

glider/include/glider/core/glider.hpp

@@ -13,7 +13,7 @@
 #include <glog/logging.h>
 
 #include "glider/core/factor_manager.hpp"
-#include "glider/utils/gps_heading.hpp"
+#include "glider/core/differential_gps.hpp"
 #include "glider/utils/geodetics.hpp"
 
 namespace Glider
@@ -35,6 +35,7 @@ class Glider
          *  should be in degree decimal and altitude in meters. Altitude
          *  frame does not matter */
         void addGps(int64_t timestamp, Eigen::Vector3d& gps);
+        void addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps);
         /*! @brief converts the imu measurements into the ENU frame if
          *  they are not in that frame already.
          *  @param timestamp: time the imu measurement was taken
@@ -81,12 +82,11 @@ class Glider
         // @brief whether or not to use differential gps
         // from motion for heading
         bool use_dgpsfm_;
+        // @brief object to handle differential gps 
+        // from motion
+        Geodetics::DifferentialGpsFromMotion dgps_;
         // @brief save the state estimate from 
         // the optimizer
         OdometryWithCovariance current_odom_;
-        // @brief save the previous gps measurement for 
-        // dgpsfm 
-        Eigen::Vector3d last_gps_;
-        double del_threshold_;
 };
 }

glider/launch/glider-node.launch.py

@@ -62,8 +62,8 @@ def generate_launch_description():
              'use_odom': False}
         ],
         remappings=[
-            ('/gps', '/ublox_gps_node/fix'),
-            ('/imu', '/vectornav/imu'),
+            ('/gps', '/ublox/fix'),
+            ('/imu', '/VN100T/imu'),
             ('/odom', '/Odometry'),
         ]
     )

glider/src/differential_gps.cpp

@@ -0,0 +1,71 @@
+/*!
+* Jason Hughes
+* October 2025
+*
+*/
+
+#include "glider/core/differential_gps.hpp"
+
+using namespace Glider::Geodetics;
+
+DifferentialGpsFromMotion::DifferentialGpsFromMotion(const std::string& frame, const double& threshold)
+{
+    frame_ = frame;
+    vel_threshold_ = threshold;
+}
+
+double DifferentialGpsFromMotion::getHeading(const Eigen::Vector3d& gps)
+{
+    if (!initialized_)
+    {
+        last_gps_ = gps;
+        initialized_ = true;
+    }
+
+    double lat1_rad = last_gps_(0) * M_PI / 180.0;
+    double lon1_rad = last_gps_(1) * M_PI / 180.0;
+    double lat2_rad = gps(0) * M_PI / 180.0;
+    double lon2_rad = gps(1) * M_PI / 180.0;
+
+    double lon_diff = lon2_rad - lon1_rad;
+
+    double y = std::sin(lon_diff) * std::cos(lat2_rad);
+    double x = std::cos(lat1_rad) * std::sin(lat2_rad) - std::sin(lat1_rad) * std::cos(lat2_rad) * std::cos(lon_diff);
+
+    double heading_rad = std::atan2(y,x);
+    last_gps_ = gps;
+
+    if (frame_ == "enu") heading_rad = nedToEnu(heading_rad);
+
+    return heading_rad;
+}
+
+bool DifferentialGpsFromMotion::isInitialized() const
+{
+    return initialized_;
+}
+
+double DifferentialGpsFromMotion::getVelocityThreshold() const
+{
+    return vel_threshold_;
+}
+
+void DifferentialGpsFromMotion::setLastGps(const Eigen::Vector3d& gps)
+{
+    last_gps_ = gps;
+    initialized_ = true;
+}
+
+double DifferentialGpsFromMotion::nedToEnu(const double heading) const
+{
+    double enu_heading = std::fmod((M_PI/2 - heading + (2*M_PI)), (2*M_PI));
+    return enu_heading;
+}
+
+double DifferentialGpsFromMotion::headingRadiansToDegrees(const double heading) const
+{
+    double heading_deg = heading * (180.0 / M_PI);
+
+    heading_deg = std::fmod(std::fmod(heading_deg, 360.0) + 360.0, 360.0);
+    return heading_deg;
+}

glider/src/glider.cpp

@@ -15,7 +15,7 @@ Glider::Glider(const std::string& path)
     Parameters params = Parameters::Load(path);
     initializeLogging(params);
     factor_manager_ = FactorManager(params);
-
+    
     frame_ = params.frame;
     t_imu_gps_ = params.t_imu_gps;
     r_enu_ned_ = Eigen::Matrix3d::Zero();
@@ -27,7 +27,7 @@ Glider::Glider(const std::string& path)
     LOG(INFO) << "[GLIDER] Using Fixed Lag Smoother: " << std::boolalpha << params.smooth;
     LOG(INFO) << "[GLIDER] Logging to: " << params.log_dir;    
     use_dgpsfm_ = params.use_dgpsfm;
-    vel_threshold_ = params.dgpsfm_threshold;
+    dgps_ = Geodetics::DifferentialGpsFromMotion(params.frame, params.dgpsfm_threshold);
 
     current_odom_ = OdometryWithCovariance::Uninitialized();
 
@@ -46,7 +46,34 @@ void Glider::initializeLogging(const Parameters& params) const
     if (params.log) FLAGS_alsologtostderr = 1;
 }
 
+
 void Glider::addGps(int64_t timestamp, Eigen::Vector3d& gps)
+{
+    // route the
+    if (use_dgpsfm_)
+    {
+        addGpsWithHeading(timestamp, gps);
+        return;
+    }
+
+    // transform from lat lon To UTM
+    Eigen::Vector3d meas = Eigen::Vector3d::Zero();
+    
+    double easting, northing;
+    char zone[4];
+    geodetics::LLtoUTM(gps(0), gps(1), northing, easting, zone);
+    
+    // keep everything in the enu frame
+    meas.head(2) << easting, northing;
+    meas(2) = gps(2);
+
+    // TODO t_imu_gps_ needs to be rotated!!
+    meas = meas + t_imu_gps_;
+
+    factor_manager_.addGpsFactor(timestamp, meas);
+}
+
+void Glider::addGpsWithHeading(int64_t timestamp, Eigen::Vector3d& gps)
 {
     // transform from lat lon To UTM
     Eigen::Vector3d meas = Eigen::Vector3d::Zero();
@@ -62,25 +89,17 @@ void Glider::addGps(int64_t timestamp, Eigen::Vector3d& gps)
     // TODO t_imu_gps_ needs to be rotated!!
     meas = meas + t_imu_gps_;
 
-    if (use_dgpsfm_ && current_odom_.isInitialized())
+    if(factor_manager_.isSystemInitialized() && current_odom_.isMovingFasterThan(dgps_.getVelocityThreshold()))
     {
-        if(factor_manager_.isSystemInitialized() && current_odom_.isMovingFasterThan(vel_threshold_))
-        {
-            LOG(INFO) << "[GLIDER] Adding DGPS heading";
-            double heading_ne = geodetics::gpsHeading(last_gps_(0), last_gps_(1), gps(0), gps(1));
-            double heading_en = geodetics::geodeticToENU(heading_ne);
-            factor_manager_.addGpsFactor(timestamp, meas, heading_en, true);
-        }
-        else
-        {
-            factor_manager_.addGpsFactor(timestamp, meas, 0.0, false);
-        }
+        LOG(INFO) << "[GLIDER] Adding DGPS heading";
+        double heading = dgps_.getHeading(gps);
+        factor_manager_.addGpsFactor(timestamp, meas, heading, true);
     }
     else
     {
-        factor_manager_.addGpsFactor(timestamp, meas);
+        dgps_.setLastGps(gps);
+        factor_manager_.addGpsFactor(timestamp, meas, 0.0, false);
     }
-    last_gps_ = gps;
 }
 
 void Glider::addImu(int64_t timestamp, Eigen::Vector3d& accel, Eigen::Vector3d& gyro, Eigen::Vector4d& quat)
@@ -92,8 +111,6 @@ void Glider::addImu(int64_t timestamp, Eigen::Vector3d& accel, Eigen::Vector3d&
         Eigen::Vector4d quat_enu = rotateQuaternion(r_enu_ned_, quat);
 
         factor_manager_.addImuFactor(timestamp, accel_enu, gyro_enu, quat_enu);
-        //factor_manager_.addImuFactor(timestamp, accel, gyro, quat);'
-        LOG(FATAL) << "[GLIDER] NED frame not supported yet";
     }
     else if (frame_ == "enu")
     {