added gps to imu extrinsics

Author: jhughes50

glider/config/example-params.yaml

@@ -4,14 +4,18 @@ integration_covariance: 0.001
 bias_covariance: 0.001
 use_second_order: 0.0
 gravity: 9.81
-bias_num_measurements: 100
+bias_num_measurements: 100 # number for imu measurements to use for imu bias calc
 gps_noise: 2.0
-odom_noise: 0.5
-heading_noise: 0.1
+odom_noise: 0.1 ## dep??
+heading_noise: 0.1 # dep??
 odom_orientation_noise: 0.1
 odom_translation_noise: 1.0
-odom_scale_noise: 50.0
-lag_time: 10.0
-imu_frame: "enu"
-scale_odom: true
-correct_imu: true
+odom_scale_noise: 1.0
+lag_time: 10.0 # amount of time for fixed lag smoother to look at
+imu_frame: "enu" # frame the imu outputs
+scale_odom: false # if you adding odom, does it need to be scaled?
+correct_imu: true # use mag to correct imu orienation
+gps_to_imu: # gps to imu extrinsics
+  X: 0.0
+  y: 0.0
+  z: 0.0

glider/config/vectornav-vn100t.yaml

@@ -15,3 +15,7 @@ lag_time: 10.0
 imu_frame: "enu"
 scale_odom: true
 correct_imu: true
+gps_to_imu:
+  X: 0.0
+  y: 0.0
+  z: 0.0

glider/include/glider/core/glider.hpp

@@ -42,6 +42,7 @@ class Glider
         bool set_initial_heading_;
         bool correct_imu_;
         std::string frame_;
+        Eigen::Vector3d t_imu_gps_;
 
         Eigen::Matrix3d ned_to_enu_rot_;
         Eigen::Quaterniond ned_to_enu_quat_;

glider/include/glider/utils/parameters.hpp

@@ -5,6 +5,7 @@
 * glider-mono parameters loaded from a yaml file.
 */
 
+#include <Eigen/Dense>
 #include <yaml-cpp/yaml.h>
 #include <string>
 
@@ -37,5 +38,6 @@ struct Parameters
 
     bool scale_odom;
     bool correct_imu;
+    Eigen::Vector3d t_imu_gps;
 };
 }

glider/src/glider.cpp

@@ -18,6 +18,7 @@ Glider::Glider(const std::string& path)
 
     frame_ = params.frame;
     correct_imu_ = params.correct_imu;
+    t_imu_gps_ = params.t_imu_gps;
 
     // IMU transformations
     ned_to_enu_rot_ << 0.0, 1.0, 0.0, 
@@ -75,6 +76,8 @@ void Glider::addGPS(int64_t timestamp, Eigen::Vector3d& gps)
     meas.head(2) << easting, northing;
     meas(2) = gps(2);
 
+    meas = meas + t_imu_gps_;
+
     factor_manager_.addGpsFactor(timestamp, meas);
 }
 

glider/src/parameters.cpp

@@ -33,6 +33,9 @@ Glider::Parameters::Parameters(const std::string& path)
 
         frame = config["imu_frame"].as<std::string>();
         scale_odom = config["scale_odom"].as<bool>();
+        t_imu_gps(0) = config["gps_to_imu"]["x"].as<double>();
+        t_imu_gps(1) = config["gps_to_imu"]["y"].as<double>();
+        t_imu_gps(2) = config["gps_to_imu"]["z"].as<double>();
     }
     catch (const YAML::Exception& e)
     {