diff --git a/README.md b/README.md index 19dd9fb..4a515a5 100644 --- a/README.md +++ b/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,7 +44,8 @@ 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: @@ -52,4 +53,5 @@ and run with: cd build ctest ``` +Note these tests are run on PR's and pushes to the primary branch. diff --git a/glider/config/graph-params.yaml b/glider/config/glider-params.yaml similarity index 89% rename from glider/config/graph-params.yaml rename to glider/config/glider-params.yaml index 15cc119..b762568 100644 --- a/glider/config/graph-params.yaml +++ b/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 diff --git a/glider/include/glider/core/glider.hpp b/glider/include/glider/core/glider.hpp index 999edb5..b6b2ebf 100644 --- a/glider/include/glider/core/glider.hpp +++ b/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_; }; } diff --git a/glider/include/glider/utils/parameters.hpp b/glider/include/glider/utils/parameters.hpp index ac5a083..736ab2e 100644 --- a/glider/include/glider/utils/parameters.hpp +++ b/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 diff --git a/glider/launch/glider-node.launch.py b/glider/launch/glider-node.launch.py index 46a9be5..42ba77a 100644 --- a/glider/launch/glider-node.launch.py +++ b/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 diff --git a/glider/src/glider.cpp b/glider/src/glider.cpp index f19cdfb..e19cc1d 100644 --- a/glider/src/glider.cpp +++ b/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()); +} } diff --git a/glider/src/parameters.cpp b/glider/src/parameters.cpp index 1b25814..ee998af 100644 --- a/glider/src/parameters.cpp +++ b/glider/src/parameters.cpp @@ -31,6 +31,7 @@ Glider::Parameters::Parameters(const std::string& path) frame = config["frame"]["imu"].as(); log = config["logging"]["stdout"].as(); + log_dir = config["logging"]["directory"].as(); smooth = config["optimizer"]["smooth"].as(); lag_time = config["optimizer"]["lag_time"].as(); diff --git a/glider/test/test_factor_manager.cpp b/glider/test/test_factor_manager.cpp index 07b87a5..32f64c9 100644 --- a/glider/test/test_factor_manager.cpp +++ b/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 diff --git a/glider/test/test_glider.cpp b/glider/test/test_glider.cpp index 24e0bd9..28f3b42 100644 --- a/glider/test/test_glider.cpp +++ b/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); diff --git a/glider/test/test_odometry_w_cov.cpp b/glider/test/test_odometry_w_cov.cpp index 473f8fc..ec6f9c0 100644 --- a/glider/test/test_odometry_w_cov.cpp +++ b/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; diff --git a/glider/test/test_params.cpp b/glider/test/test_params.cpp index f2b9950..7ab2674 100644 --- a/glider/test/test_params.cpp +++ b/glider/test/test_params.cpp @@ -8,7 +8,7 @@ TEST(ParamsTestSuite, Constants) { - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); // gravity can be positive or negative depending on // imu frame @@ -22,7 +22,7 @@ TEST(ParamsTestSuite, Constants) TEST(ParamsTestSuite, Covariances) { - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); // covariances should be positive ASSERT_GE(params.accel_cov, 0.0); @@ -36,7 +36,7 @@ TEST(ParamsTestSuite, Covariances) TEST(ParamsTestSuite, Frame) { - Glider::Parameters params = Glider::Parameters::Load("../config/graph-params.yaml"); + Glider::Parameters params = Glider::Parameters::Load("../config/glider-params.yaml"); EXPECT_TRUE(params.frame == "ned" || params.frame == "enu"); }