resolve merge conflicts

Author: jhughes50

docker/Dockerfile.humble

@@ -37,8 +37,6 @@ RUN sudo apt-get update \
 RUN sudo apt-get install -y python3-colcon-common-extensions
 
 # install visualizer deps
-RUN sudo apt-get install -y python3-pip
-RUN pip3 install flask folium flask_socketio utm
 RUN sudo apt install -y libgoogle-glog-dev
 # build ros env
 RUN mkdir ws
@@ -48,4 +46,3 @@ RUN sudo chown $USER:$USER ~/.bashrc \
  && /bin/sh -c 'echo ". /opt/ros/humble/setup.bash" >> ~/.bashrc' \
  && /bin/sh -c 'echo "source ~/ws/install/setup.bash" >> ~/.bashrc' \
  && echo 'export PS1="\[$(tput setaf 2; tput bold)\]\u\[$(tput setaf 7)\]@\[$(tput setaf 3)\]\h\[$(tput setaf 7)\]:\[$(tput setaf 4)\]\W\[$(tput setaf 7)\]$ \[$(tput sgr0)\]"' >> ~/.bashrc
-

docker/run-humble.bash

@@ -7,11 +7,11 @@ docker run -it --rm \
     -v "/dev:/dev" \
     -v "/tmp/.X11-unix:/tmp/.X11-unix" \
     -v "`pwd`/../glider:/home/`whoami`/ws/src/glider" \
-    -v "/home/`whoami`/Data/evmapper:/home/`whoami`/data" \
+    -v "/home/`whoami`/Data:/home/`whoami`/data" \
     -e DISPLAY=$DISPLAY \
     -e QT_X11_NO_MITSHM=1 \
     -e XAUTHORITY=$XAUTH \
-    --name glider-ros-humble \
-    glider-ros:humble \
+    --name glider-ros-jazzy \
+    glider-ros:jazzy \
     bash
 xhost -

glider/CMakeLists.txt

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

glider/config/glider-params.yaml

@@ -1,17 +1,22 @@
-covariances:
-  accelerometer: 0.00001
-  gyroscope: 0.00001
-  integration: 0.001
-  heading: 0.09 
-  roll_pitch: 0.001
-  bias: 0.001
-  gps: 2.0
+imu:
+  covariances:
+    accelerometer: 0.00001
+    gyroscope: 0.00001
+    integration: 0.001
+    heading: 0.09 
+    roll_pitch: 0.001
+    bias: 0.001
+  frame: "enu"
+gps: 
+  covariance: 2.0
+dgpsfm:
+  enable: false
+  integration_threshold: 1.0
+  covariance: 0.03
 constants:
   gravity: 9.81
   bias_num_measurements: 100
   initial_num_measurements: 4
-frame:
-  imu: "enu"
 logging:
   stdout: true
   directory: "/tmp/glider"

glider/include/glider/core/factor_manager.hpp

@@ -79,6 +79,13 @@ class FactorManager
          *  @param timestamp: time of the gps measurement
          *  @param gps: GPS measurement in the UTM frame */
         void addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps);
+        /*! @brief adds the gps measurement and a heading from dgps
+         *  @param timestamp: time of the gps measurement 
+         *  @param gps: GPS measurement in the UTM frame
+         *  @param heading: heading from dgpsfm in the ENU frame 
+         *  @param fuse: whether or not to add the heading measurement
+         *  to the factor graph */
+        void addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps, const double& heading, const bool fuse);
         /*! @brief adds the imu measurements to the pim and saves the orientation
          *  @param timestamp: time of the imu measurement 
          *  @param accel: the accelerometer reading
@@ -165,6 +172,8 @@ class FactorManager
         gtsam::noiseModel::Isotropic::shared_ptr gps_noise_;
         // @brief noise in the orientation estimate from the imu
         gtsam::noiseModel::Base::shared_ptr orient_noise_;
+        // @brief noise in the heading estimate of differential gps
+        gtsam::noiseModel::Base::shared_ptr dgpsfm_noise_;
 
         // factor graph
         // @brief tracks the number of times the optimizer has been called

glider/include/glider/core/glider.hpp

@@ -13,6 +13,7 @@
 #include <glog/logging.h>
 
 #include "glider/core/factor_manager.hpp"
+#include "glider/utils/gps_heading.hpp"
 #include "glider/utils/geodetics.hpp"
 
 namespace Glider
@@ -77,5 +78,15 @@ class Glider
         Eigen::Vector3d t_imu_gps_;
         // @brief the rotation matrix from ned to enu frame
         Eigen::Matrix3d r_enu_ned_;
+        // @brief whether or not to use differential gps
+        // from motion for heading
+        bool use_dgpsfm_;
+        // @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/include/glider/utils/gps_heading.hpp

@@ -23,45 +23,17 @@ namespace geodetics
  *  @param lon2: the TO longitude in degrees decimal
  *  @return heading_rad: the heading in radians in the NED frame 
 */
-double gpsHeading(double lat1, double lon1, double lat2, double lon2)
-{
-    double lat1_rad = lat1 * M_PI / 180.0;
-    double lon1_rad = lon1 * M_PI / 180.0;
-    double lat2_rad = lat2 * M_PI / 180.0;
-    double lon2_rad = lon2 * 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);
-
-    return heading_rad;
-}
-
+double gpsHeading(double lat1, double lon1, double lat2, double lon2);
 /*! @brief convert a heading in radians to degrees and normalize to [0,360)
  *  @param heading: the heading in radians, can be ENU or NED frame
  *  @return heading_deg: normalized heading in degrees in the same frame
  *  that was input
 */
-double headingRadiansToDegrees(double heading)
-{
-    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;
-}
-
+double headingRadiansToDegrees(double heading);
 /*! @brief convert a heading from the geodetic frame (NED) to the ENU frame
  *  @param geodetic_heading: heading in radians in the geodetic (NED) frame
  *  @return enu_heading: heading in radians in the ENU frame
 */
-double geodeticToENU(double geodetic_heading)
-{
-    double enu_heading = std::fmod((M_PI/2 - geodetic_heading + (2*M_PI)), (2*M_PI));
-    
-    return enu_heading;
-}
+double geodeticToENU(double geodetic_heading);
 } // namespace geodetics
 } // namespace glider

glider/include/glider/utils/parameters.hpp

@@ -72,6 +72,15 @@ struct Parameters
     // smooth over.
     double lag_time; 
 
+    // @brief wheather or not to integrate differential gps from motion heading,
+    // if false orientation from the IMU will be integrated
+    bool use_dgpsfm;
+    // @brief velocity in m/s that the robot should be moving at to integrate
+    // dgpsfm
+    double dgpsfm_threshold;
+    // @brief heading noise for differential gps from motion 
+    double dgpsfm_cov;
+
     // @brief translation from the GPS to the IMU
     Eigen::Vector3d t_imu_gps;
 };

glider/launch/glider-node.launch.py

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

glider/src/factor_manager.cpp

@@ -34,7 +34,8 @@ FactorManager::FactorManager(const Parameters& params)
     // set noise model
     gps_noise_ = gtsam::noiseModel::Isotropic::Sigma(3, params.gps_noise);
     orient_noise_ = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(params.roll_pitch_cov, params.roll_pitch_cov, params.heading_cov));
-
+    dgpsfm_noise_ = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(M_PI/2, M_PI/2, params.dgpsfm_cov));
+    
     // set key index
     key_index_ = 0;
 
@@ -153,14 +154,78 @@ void FactorManager::addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps)
     gtsam::Point3 meas(gps(0), gps(1), gps(2));
     gtsam::Rot3 rot = gtsam::Rot3::Quaternion(orient_(0), orient_(1), orient_(2), orient_(3));
 
-    // add measurements to factor graph
+    // add gps measurement to factor graph as gtsam object
     graph_.add(gtsam::GPSFactor(X(key_index_), gps, gps_noise_));
     graph_.addExpressionFactor(gtsam::rotation(X(key_index_)), rot, orient_noise_);
+ 
+    // increment key index
+    key_index_++;
+}
+
+void FactorManager::addGpsFactor(int64_t timestamp, const Eigen::Vector3d& gps, const double& heading, const bool fuse) 
+{
+    // wait until the imu is initialized
+    if (!imu_initialized_) return;
+    
+    double time = nanosecIntToDouble(timestamp);
+
+    if (key_index_ == 0)
+    {
+        // set the initial NavState 
+        // The initial orientation is the the initial orientation from the imu initialization
+        // The initial position is from the gps 
+        // Initial velocity is set to zero
+        gtsam::Pose3 initial_pose = gtsam::Pose3(initial_orientation_, gtsam::Point3(gps(0), gps(1), gps(2)));
+        gtsam::NavState initial_navstate(initial_pose, gtsam::Point3(0.0, 0.0, 0.0)); // TODO why do I need this??
+        
+        // save the initial values
+        initials_.insert(X(key_index_), initial_pose);
+        initials_.insert(V(key_index_), gtsam::Point3(0.0, 0.0, 0.0));
+        initials_.insert(B(key_index_), bias_);
+
+        // save the timestamps for the smoother
+        smoother_timestamps_[X(key_index_)] = time;
+        smoother_timestamps_[V(key_index_)] = time;
+        smoother_timestamps_[B(key_index_)] = time;
+
+        // add prior factors to the graph
+        graph_.add(gtsam::PriorFactor<gtsam::Pose3>(X(key_index_), initial_pose, gtsam::noiseModel::Isotropic::Sigma(6, 0.001)));
+        graph_.add(gtsam::PriorFactor<gtsam::Point3>(V(key_index_), gtsam::Point3(0.0, 0.0, 0.0), gtsam::noiseModel::Isotropic::Sigma(3, 0.001)));
+        graph_.add(gtsam::PriorFactor<gtsam::imuBias::ConstantBias>(B(key_index_), bias_, gtsam::noiseModel::Isotropic::Sigma(6, 0.001)));
+
+        key_index_++;
+        gps_initialized_ = true;
+        LOG(INFO) << "[GLIDER] GPS Initialized";
+        return;
+    }
+   
+    // add the pim to the graph under a mutex
+    std::unique_lock<std::mutex> lock(mutex_);
+    graph_.add(gtsam::CombinedImuFactor(X(key_index_-1), V(key_index_-1), X(key_index_), V(key_index_), B(key_index_-1), B(key_index_), *pim_));
+    lock.unlock();
+
+    // insert new initial values
+    initials_.insert(X(key_index_), current_state_.getPose<gtsam::Pose3>());
+    initials_.insert(V(key_index_), current_state_.getVelocity<gtsam::Vector3>());
+    initials_.insert(B(key_index_), bias_);
+
+    // save the time for the smoother
+    smoother_timestamps_[X(key_index_)] = time;
+    smoother_timestamps_[V(key_index_)] = time;
+    smoother_timestamps_[B(key_index_)] = time;
+    
+    // add gps measurement to factor graph as gtsam object
+    gtsam::Point3 meas(gps(0), gps(1), gps(2));
+    gtsam::Rot3 rot = gtsam::Rot3::Ypr(heading, 0.0, 0.0);
+
+    graph_.add(gtsam::GPSFactor(X(key_index_), gps, gps_noise_));
+    if (fuse) graph_.addExpressionFactor(gtsam::rotation(X(key_index_)), rot, dgpsfm_noise_);
 
     // increment key index
     key_index_++;
 }
 
+
 void FactorManager::addImuFactor(int64_t timestamp, const Eigen::Vector3d& accel, const Eigen::Vector3d& gyro, const Eigen::Vector4d& orient) 
 {
     // if the imu is not initialized, pass the meaurements to initialize it