Merge pull request #16 from KumarRobotics/feature/ned-frame

Feature-ned-frame

Author: jhughes50

README.md

@@ -4,10 +4,10 @@
 ![Jazzy CI](https://github.com/KumarRobotics/glider/actions/workflows/jazzy-ci.yml/badge.svg?branch=ros2)
 
 Glider is a G-INS system built on [GTSAM](https://github.com/borglab/gtsam). It currently takes in GPS and 9-DOF IMU and provides a full
-state estimate up to the rate of you IMU. Glider is highly configurable and more features are coming soon. 
+state estimate up to the rate of you IMU. Glider is designed to be configured to your system. 
 
 ## Hardware Setup
-You're setup needs a GPS and a 9-DOF IMU, that is an IMU that provides a full orientation. The IMU orientation should be provided in the `IMU` frame
+You're setup needs a GPS and a 9-DOF IMU, that is an IMU that provides a full orientation. The IMU orientation should be provided in the IMU's frame
 as this is standard for robotics, but we are working on supporting the NED frame. We use a VectorNav VN100T IMU. It is important make sure your IMU magnetometer is calibrated, if it is not aligned correctly the heading output of glider will be incorrect.
 
 ## ROS2 Setup
@@ -29,10 +29,10 @@ You can configure glider itself in `config/glider-params.yaml`, this is where yo
  - `covariances.heading`: covariance of the IMU's magnetometer heading in radians, 0.09 radians is about 5 degrees.
  - `covariances.roll_pitch`: covariance of the roll and pitch angles in radians.
  - `covariances.bias`: covariance of the bias estimate.
- - `frame`: What frame the IMU is in, currently only support ENU but NED support is coming .
-### GPS Parameters
+ - `frame`: What frame the IMU is in, either `enu` or `ned`.
+#### GPS Parameters
  - `gps.covariance`: covariance of the gps position estimate.
-### Other Parameters
+#### Other Parameters
  - `constants.gravity`: gravity in your IMU's frame.
  - `constants.bias_num_measurements`: number of IMU measurements to use to initially estimate the bias.
  - `constants.initial_num_measurements`: number of times to let the factor graph optimize before glider starts reporting odometry.
@@ -44,12 +44,14 @@ You can configure glider itself in `config/glider-params.yaml`, this is where yo
 ### Building and Running Unit Tests
 We use GTest to run unit tests. You can build the tests with 
 ``` 
-cmake -S . -B build
+cd glider
+cmake -S . -B build -DBUILD_TESTS=ON
 cmake --build build
 ```
 and run with:
 ```
 cd build 
 ctest
 ```
+Note these tests are run on PR's and pushes to the primary branch.
 

glider/config/graph-params.yaml glider/config/glider-params.yamlglider/config/graph-params.yaml renamed to glider/config/glider-params.yaml

@@ -14,6 +14,7 @@ frame:
   imu: "enu"
 logging:
   stdout: true
+  directory: "/home/jason/.ros/log/glider"
 optimizer:
   smooth: true
   lag_time: 5.0

glider/include/glider/core/glider.hpp

@@ -59,6 +59,12 @@ class Glider
          *  parameters
          *  @params: the laoded params from the yaml file */
         void initializeLogging(const Parameters& params) const;
+        /*! @brief rotate a quaternion by a rotation matrix
+         *  @params rot: the rotation matrix you want to rotate the quaterniuon by
+         *  @param quat: the orientation we want to rotate
+         *  @return the rotated orientation as quaternion as a Vector4d in
+         *  (w, x, y, z) format */
+        Eigen::Vector4d rotateQuaternion(const Eigen::Matrix3d& rot, const Eigen::Vector4d& quat) const;
 
         // @brief the factor manager, handles the factor graph
         FactorManager factor_manager_;
@@ -69,5 +75,7 @@ class Glider
         // @brief the relative translation from the gps to
         // the imu
         Eigen::Vector3d t_imu_gps_;
+        // @brief the rotation matrix from ned to enu frame
+        Eigen::Matrix3d r_enu_ned_;
 };
 }

glider/include/glider/utils/parameters.hpp

@@ -61,6 +61,9 @@ struct Parameters
     // @brief if true this logs to stdout and log file otherwise it logs
     // just to a file
     bool log;
+    // @brief the directory to save the log file, glog needs an 
+    // absolute path
+    std::string log_dir;
 
     // @brief if true optimize using a fixed lag smoother, otherwise
     // optimize with iSAM2

glider/launch/glider-node.launch.py

@@ -48,7 +48,7 @@ def generate_launch_description():
     graph_params_file = PathJoinSubstitution([
         glider_share,
         'config',
-        'graph-params.yaml'
+        'glider-params.yaml'
     ])
 
     # Create the glider node

glider/src/glider.cpp

@@ -18,6 +18,10 @@ Glider::Glider(const std::string& path)
 
     frame_ = params.frame;
     t_imu_gps_ = params.t_imu_gps;
+    r_enu_ned_ = Eigen::Matrix3d::Zero();
+    r_enu_ned_ << 0.0, 1.0, 0.0,
+                  1.0, 0.0, 0.0,
+                  0.0, 0.0, -1.0;
 
     LOG(INFO) << "[GLIDER] Using IMU frame: " << frame_;
     LOG(INFO) << "[GLIDER] Using Fixed Lag Smoother: " << std::boolalpha << params.smooth;
@@ -29,7 +33,7 @@ void Glider::initializeLogging(const Parameters& params) const
     // initialize GLog
     google::InitGoogleLogging("Glider");
     // TODO fix hard coding
-    FLAGS_log_dir = "/home/jason/.ros/log/glider";
+    FLAGS_log_dir = params.log_dir;
     if (params.log) FLAGS_alsologtostderr = 1;
 }
 
@@ -57,7 +61,11 @@ void Glider::addImu(int64_t timestamp, Eigen::Vector3d& accel, Eigen::Vector3d&
     if (frame_ == "ned")
     {
         //TODO what transforms need to happen here
-        factor_manager_.addImuFactor(timestamp, accel, gyro, quat);
+        Eigen::Vector3d accel_enu = r_enu_ned_ * accel;
+        Eigen::Vector3d gyro_enu = r_enu_ned_ * gyro;
+        Eigen::Vector4d quat_enu = rotateQuaternion(r_enu_ned_, quat);
+
+        factor_manager_.addImuFactor(timestamp, accel_enu, gyro_enu, quat_enu);
     }
     else if (frame_ == "enu")
     {
@@ -96,4 +104,14 @@ OdometryWithCovariance Glider::optimize(int64_t timestamp)
         return OdometryWithCovariance::Uninitialized();
     }
 }
+
+Eigen::Vector4d Glider::rotateQuaternion(const Eigen::Matrix3d& rot, const Eigen::Vector4d& quat) const
+{
+    Eigen::Quaterniond q_ned(quat(0), quat(1), quat(2), quat(3));
+    Eigen::Quaterniond q_ned_enu(rot);
+
+    Eigen::Quaterniond q_enu = q_ned_enu * q_ned;
+
+    return Eigen::Vector4d(q_enu.w(), q_enu.x(), q_enu.y(), q_enu.z());
+}
 }

glider/src/parameters.cpp

@@ -31,6 +31,7 @@ Glider::Parameters::Parameters(const std::string& path)
         frame = config["frame"]["imu"].as<std::string>();
 
         log = config["logging"]["stdout"].as<bool>(); 
+        log_dir = config["logging"]["directory"].as<std::string>();
 
         smooth = config["optimizer"]["smooth"].as<bool>();
         lag_time = config["optimizer"]["lag_time"].as<double>();

glider/test/test_factor_manager.cpp

@@ -10,7 +10,7 @@
 TEST(FactorManagerTestSuite, ImuInitialization)
 {
     // initialized glider factor manager and params
-    Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml");
+    Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml");
     Glider::FactorManager manager(params);
 
     // provide measurements for initialization
@@ -43,7 +43,7 @@ TEST(FactorManagerTestSuite, ImuInitialization)
 TEST(FactorManagerTestSuite, PimParameters)
 { 
     // initialized glider factor manager and params
-    Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml");
+    Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml");
     Glider::FactorManager manager(params);
 
     // provide measurements for initialization
@@ -87,7 +87,7 @@ TEST(FactorManagerTestSuite, KeyIndex)
     double lon = -75.199197;
 
     // initialized glider factor manager and params
-    Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml");
+    Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml");
     Glider::FactorManager manager(params);
 
     ASSERT_EQ(manager.getKeyIndex(), 0);
@@ -115,7 +115,7 @@ TEST(FactorManagerTestSuite, GPSInitialization)
     double lon = -75.199197;
 
     // initialized glider factor manager and params
-    Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml");
+    Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml");
     Glider::FactorManager manager(params);
 
     // provide imu measurements for initialization
@@ -143,7 +143,7 @@ TEST(FactorManagerTestSuite, SystemInitialization)
     double lon = -75.199197;
 
     // initialized glider factor manager and params
-    Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml");
+    Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml");
     Glider::FactorManager manager(params);
 
     // assert system is NOT initialized

glider/test/test_glider.cpp

@@ -11,7 +11,7 @@
 
 TEST(GliderTestSuite, AddTo)
 {
-    Glider::Glider glider("../config/graph-params.yaml");
+    Glider::Glider glider("../config/glider-params.yaml");
 
     Eigen::Vector3d accel(0.0, 0.0, 9.81);
     Eigen::Vector3d gyro(0.0, 0.0, 0.0);

glider/test/test_odometry_w_cov.cpp

@@ -40,7 +40,7 @@ static const double GZ = 0.0;
 TEST(OdometryWithCovarianceTestSuite, TestInitialization)
 {
     // initialized glider factor manager and params
-    Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml");
+    Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml");
     Glider::FactorManager manager(params);
 
     Glider::OdometryWithCovariance odom;
@@ -71,7 +71,7 @@ TEST(OdometryWithCovarianceTestSuite, TestInitialization)
 TEST(OdometryWithCovarainceTestSuite, TestCovariances)
 {
     // initialized glider factor manager and params
-    Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml");
+    Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml");
     Glider::FactorManager manager(params);
 
     Glider::OdometryWithCovariance odom;
@@ -125,7 +125,7 @@ TEST(OdometryWithCovarainceTestSuite, TestCovariances)
 TEST(OdometryWithCovarianceTestSuite, TestBiases)
 {
     // initialized glider factor manager and params
-    Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml");
+    Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml");
     Glider::FactorManager manager(params);
 
     Glider::OdometryWithCovariance odom;
@@ -193,7 +193,7 @@ TEST(OdometryWithCovarianceTestSuite, TestBiases)
 TEST(OdometryWithCovarianceTestSuite, TestKeyIndex)
 { 
     // initialized glider factor manager and params
-    Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml");
+    Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml");
     Glider::FactorManager manager(params);
 
     Glider::OdometryWithCovariance odom;
@@ -230,7 +230,7 @@ TEST(OdometryWithCovarianceTestSuite, TestKeyIndex)
 TEST(OdometryWithCovarianceTestSuite, TestMovementStatus)
 { 
     // initialized glider factor manager and params
-    Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml");
+    Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml");
     Glider::FactorManager manager(params);
 
     Glider::OdometryWithCovariance odom;