made imu heading correction with mag a param

Author: jhughes50

glider/config/graph_params.yaml

@@ -16,3 +16,4 @@ odom_scale_noise: 50.0
 lag_time: 10.0
 imu_frame: "enu"
 scale_odom: true
+correct_imu: false

glider/include/glider/core/glider.hpp

@@ -40,6 +40,7 @@ class Glider
         double initial_heading_;
         double current_heading_;
         bool set_initial_heading_;
+        bool correct_imu_;
         std::string frame_;
 
         Eigen::Matrix3d ned_to_enu_rot_;

glider/include/glider/utils/parameters.hpp

@@ -38,5 +38,6 @@ struct Parameters
     std::string frame;
 
     bool scale_odom;
+    bool correct_imu;
 };
 }

glider/launch/glider-node.launch.py

@@ -16,7 +16,7 @@ def generate_launch_description():
 
     use_odom_arg = DeclareLaunchArgument(
         'use_odom',
-        default_value='true',
+        default_value='false',
         description='Use odometry'
     )
 
@@ -53,9 +53,9 @@ def generate_launch_description():
              'use_odom': use_odom}
         ],
         remappings=[
-            ('/gps', '/ublox/fix'),
-            ('/imu', '/VN100T/imu'),
-            ('/mag', '/VN100T/mag'),
+            ('/gps', '/ublox_gps_node/fix'),
+            ('/imu', '/zed/zed_node/imu/data'),
+            ('/mag', '/zed/zed_node/imu/mag'),
             ('/odom', '/Odometry'),
         ]
     )

glider/src/glider.cpp

@@ -19,6 +19,7 @@ Glider::Glider(const std::string& path)
     origin_x_ = params.origin_x;
     origin_y_ = params.origin_y;
     frame_ = params.frame;
+    correct_imu_ = params.correct_imu;
 
     // IMU transformations
     ned_to_enu_rot_ << 0.0, 1.0, 0.0, 
@@ -33,6 +34,7 @@ Glider::Glider(const std::string& path)
     set_initial_heading_ = true;
 
     std::cout << "[GLIDER] Using IMU frame: " << frame_ << std::endl;
+    std::cout << "[GLIDER] Correcting IMU heading with mag: " << std::boolalpha << correct_imu_ << std::endl;
     std::cout << "[GLIDER] Glider initialized" << std::endl;
 }
 
@@ -91,17 +93,31 @@ void Glider::addIMU(int64_t timestamp, Eigen::Vector3d& accel, Eigen::Vector3d&
         Eigen::Vector4d vec_enu;
         vec_enu << quat_enu.w(), quat_enu.x(), quat_enu.y(), quat_enu.z();
 
-        Eigen::Vector4d quat_corr = correctImuOrientation(vec_enu);
-
         Eigen::Vector3d accel_enu = ned_to_enu_rot_ * accel;
         Eigen::Vector3d gyro_enu = ned_to_enu_rot_ * gyro;
 
-        factor_manager_.addImuFactor(timestamp, accel_enu, gyro_enu, quat_corr);
+        if (correct_imu_)
+        {
+            Eigen::Vector4d quat_corr = correctImuOrientation(vec_enu);
+            factor_manager_.addImuFactor(timestamp, accel_enu, gyro_enu, quat_corr);
+        }
+        else
+        {
+            factor_manager_.addImuFactor(timestamp, accel_enu, gyro_enu, vec_enu);
+        }
+
     }
     else if (frame_ == "enu")
     {
-        Eigen::Vector4d quat_corr = correctImuOrientation(quat);
-        factor_manager_.addImuFactor(timestamp, accel, gyro, quat_corr);
+        if (correct_imu_)
+        {
+            Eigen::Vector4d quat_corr = correctImuOrientation(quat);
+            factor_manager_.addImuFactor(timestamp, accel, gyro, quat_corr);
+        }
+        else
+        {
+            factor_manager_.addImuFactor(timestamp, accel, gyro, quat);
+        }
     }
     else
     {

glider/src/parameters.cpp

@@ -29,6 +29,7 @@ Glider::Parameters::Parameters(const std::string& path)
         odom_orientation_noise = config["odom_orientation_noise"].as<double>();
         odom_translation_noise = config["odom_translation_noise"].as<double>();
         odom_scale_noise = config["odom_scale_noise"].as<double>();
+        correct_imu = config["correct_imu"].as<bool>();
 
         bias_num_measurements = config["bias_num_measurements"].as<int>();