From 9521abb96fb18701723e5468a274f03f0548bc2c Mon Sep 17 00:00:00 2001 From: Adrian Sochaniwsky <6884645+adrian-soch@users.noreply.github.com> Date: Sun, 24 Mar 2024 17:22:43 -0400 Subject: [PATCH] Add filter implementation and modules (#54) * Add inline * Add utils module and unittests * Update templates and test * Add initializers module and unit tests * Update quat initializers * Make quat intialization faster * Fix bug and update test cases * fix bug * Update overlaods and tests * update comments and tests * Update comments * cleanup * Add code and test structure * update cmake * Update github stuff * Update build script * Fix egien include * Add flags * update ci * upadte ci * fix again * another try * fix eigen dep * add codecov yaml to ignore test folder coverage * Update scipts and configs * Update code and tests * Fix bugs and update unit tests * Cleanup * Fix bug and update unit tests --- .github/ISSUE_TEMPLATE/custom_template.md | 7 +- .github/ISSUE_TEMPLATE/review_request.md | 1 - .github/workflows/ci.yaml | 39 ++- CMakeLists.txt | 1 + build.sh | 23 +- scripts/generate_test_cases.py | 118 +++++++++ scripts/utils/read_mat_data.py | 16 +- setup.sh | 1 + src/README.md | 5 - src/attitude_check.cpp | 146 ++++++++++- src/attitude_check.hpp | 62 ++++- src/initializers.hpp | 56 +++++ src/quaternion.hpp | 143 ++++++++++- src/utilities.hpp | 71 ++++++ test/CMakeLists.txt | 4 + test/attitude_check_test.cpp | 283 +++++++++++++++++++++- test/initializers_test.cpp | 96 ++++++++ test/quaternion_test.cpp | 95 +++++++- test/utilities_test.cpp | 74 ++++++ 19 files changed, 1157 insertions(+), 84 deletions(-) create mode 100644 scripts/generate_test_cases.py delete mode 100644 src/README.md create mode 100644 src/initializers.hpp create mode 100644 src/utilities.hpp create mode 100644 test/initializers_test.cpp create mode 100644 test/utilities_test.cpp diff --git a/.github/ISSUE_TEMPLATE/custom_template.md b/.github/ISSUE_TEMPLATE/custom_template.md index 6eb748b..5068bc7 100644 --- a/.github/ISSUE_TEMPLATE/custom_template.md +++ b/.github/ISSUE_TEMPLATE/custom_template.md @@ -3,11 +3,6 @@ name: Custom Template about: For other types of issues. title: '' labels: '' -assignees: adrian-soch +assignees: --- - -**Describe the problem/comment/request** -A clear and concise description. - - diff --git a/.github/ISSUE_TEMPLATE/review_request.md b/.github/ISSUE_TEMPLATE/review_request.md index c09fde2..50d6363 100644 --- a/.github/ISSUE_TEMPLATE/review_request.md +++ b/.github/ISSUE_TEMPLATE/review_request.md @@ -3,7 +3,6 @@ name: Review Request about: Review documentation to help us improve title: '' labels: '' -assignees: adrian-soch --- diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 7e4c2da..0dbc15c 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -1,38 +1,31 @@ -name: C/C++ CI # The name of the workflow +name: C/C++ CI -on: # The trigger for the workflow - push: # When you push to any branch - branches: [ main ] # Only for the main branch - pull_request: # When you create or update a pull request - branches: [ main ] # Only for the main branch +on: + push: + branches: [main] + pull_request: + branches: [main] -jobs: # The jobs for the workflow - build-and-unittest: # The name of the job +jobs: + build-and-unittest: if: github.event.pull_request.draft == false - runs-on: ubuntu-latest # The operating system to run the job on - steps: # The steps for the job - - name: Checkout # The name of the step - # uses: actions/checkout@v2 # The action to use for the step + runs-on: ubuntu-latest + steps: + - name: Checkout uses: actions/checkout@v4 with: sparse-checkout: | .github src test - - name: Install dependencies # The name of the step - run: sudo apt-get install -y cmake lcov cppcheck # The command to run for the step + - name: Install dependencies + run: sudo apt-get install -y cmake lcov cppcheck + - name: Install Eigen3 + run: sudo apt-get install -y libeigen3-dev && sudo ln -s /usr/include/eigen3/Eigen /usr/local/include/Eigen - name: Install gtest run: sudo apt-get install libgtest-dev && cd /usr/src/gtest && sudo cmake CMakeLists.txt - name: Run custom build script - run: bash build.sh test - - name: Run unit tests - run: bash build.sh test - - name: Generate coverage report - run: | - lcov --capture --directory . --output-file coverage.info - lcov --remove coverage.info '/usr/*' --output-file coverage.info - lcov --list coverage.info - genhtml coverage.info --output-directory out + run: bash ./build.sh test - name: Upload coverage reports to Codecov uses: codecov/codecov-action@v4 with: diff --git a/CMakeLists.txt b/CMakeLists.txt index c12934f..141d021 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,6 +7,7 @@ project(attitude_check) # Default to C++17 set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17") # Add cppCheck for static checks # set(CMAKE_CXX_CPPCHECK "cppcheck") diff --git a/build.sh b/build.sh index 8bdbbe0..c871b19 100755 --- a/build.sh +++ b/build.sh @@ -1,34 +1,37 @@ #!/bin/bash # Usage: ./build.sh [test|clean|debug] +ROOT="$PWD" +BUILD="$ROOT/build" + # Create a build directory if it does not exist -mkdir -p build && cd build +mkdir -p $BUILD && cd $BUILD # Check the first argument case $1 in test) # Build the project with tests enabled - cmake -DBUILD_TESTS=ON -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS="-fprofile-arcs -ftest-coverage" .. - make - cd ./test + cmake -DBUILD_TESTS=ON .. + make || { echo "Build failed, stopping script."; exit 1; } + cd "$BUILD/test" # Run the tests - ctest + ctest --rerun-failed --output-on-failure # Generate the coverage report using gcov and lcov - cd ../build/test/CMakeFiles/attitude_check_test.dir + cd "$BUILD/test/CMakeFiles/attitude_check_test.dir" - lcov --capture --directory . --output-file coverage.info + lcov --capture --rc lcov_branch_coverage=1 --directory . --output-file coverage.info lcov --remove coverage.info '/usr/*' '*/test*' --output-file coverage.info lcov --list coverage.info - genhtml coverage.info --output-directory out + genhtml coverage.info --output-directory $BUILD/test/lcov_out ;; clean) # Remove the build directory - cd .. && rm -rf build + cd "$ROOT" && rm -rf build ;; debug) # Build the project with debug mode enabled - cmake -DBUILD_TESTS=ON -DCMAKE_BUILD_TYPE=Debug .. + cmake -DBUILD_TESTS=ON -DCMAKE_BUILD_TYPE=RelWithDebInfo .. make ;; *) diff --git a/scripts/generate_test_cases.py b/scripts/generate_test_cases.py new file mode 100644 index 0000000..bc59b99 --- /dev/null +++ b/scripts/generate_test_cases.py @@ -0,0 +1,118 @@ +''' +This script will use the python AHRS (https://ahrs.readthedocs.io/en/latest/) +as a psuedo oracle for generating test cases. +''' + +from ahrs.common.orientation import acc2q, am2angles, ecompass, rpy2q +from ahrs.filters import Madgwick +import numpy as np + + +def generate_acc2q_tests(acc): + # acc = + quat = acc2q(a=acc) + + print(f'In: {acc} Out:', end=" ") + for q in quat: + print(f'{q:.9f}', end=" ") + print("\n") + + +def generate_ecompass_tests(acc, mag): + quat = ecompass(acc, mag, frame='NED', representation='quaternion') + + print(f'Acc: {acc}, Mag: {mag} Out:', end=" ") + for q in quat: + print(f'{q:.9f}', end=" ") + print("\n") + + +def generate_am2q_tests(acc, mag): + print(f'Acc: {acc}, Mag: {mag} Out:', end=" ") + + rpy = am2angles(acc, mag) + print(np.flip(rpy)) + quat = rpy2q(np.flip(rpy)) + + print(quat) + + +am2angles + + +def get_mag_to_quat_cases(): + + generate_am2q_tests(np.array( + [-6.382152, -7.969839, 0.099505]), np.array([36.700461, 22.613240, 8.881961])) + + generate_am2q_tests(np.array([0.0, 0.0, 9.81]), + np.array([16676.8, -3050.9, 49916.9])) + + +def get_acc_to_quat_cases(): + + generate_acc2q_tests([0.090941, -0.031273, 9.759028]) + + # gt = # 0.802582 & 0.209335 & 0.088698 & 0.551520 + generate_acc2q_tests([0.283217, 2.540909, 7.708738]) + + # -0.093491 & -0.892442 & 0.090310 & 0.432031 \\ + generate_acc2q_tests([7.550523, 2.391483, -0.958994]) + + generate_acc2q_tests([-9.81, 0, 0]) + + +def get_marg_estimate_cases(): + + q0 = [0.49666186227,0.034292027603,-0.0510015643620,0.86576549471] + + q_actual = [[0.49593818106, 0.0326998015547, -0.0510947167457, 0.86623632656], + [0.49519980631, 0.0312979556660, -0.0513729764280, 0.86669395238], + [0.49443437746, 0.0302718565058, -0.0520166472497, 0.86712890015], + [0.49366783997, 0.029245689468, -0.0526602014369, 0.867561903615]] + + gyr = np.array([[-0.5006895838366212, 0.8074957770569486, 0.5528888911052677], + [-0.5326483077698894, 0.6743323911175373, 0.5262569120490862], + [-0.5624759846864724, 0.5560845889656695, 0.5166698184678814], + [-0.5880433128989375, 0.39415992228264346, 0.5017542346803379]], dtype=np.float32) + acc = np.array([[0.47451228800383594, 1.0183972122654374, 8.461165125378162], + [0.3783742880038359, 0.5534032122654374, 8.264965125378161], + [0.27340728800383585, 0.05015021226543746, 8.015791125378161], + [0.3067612880038358, -0.2853517877345626, 8.159017125378163]], dtype=np.float32) + mag = np.array([[10.04650728048766, -6.088486551348383, -43.828022623283225], + [11.084734479270914, -6.77198980959856, -44.35222010806493], + [10.19482545174241, -6.923879422543039, -43.97779333322085], + [9.30491642421391, -7.075769035487526, -43.60336655837678]], dtype=np.float32) + est = Madgwick(gyr=gyr, acc=acc, mag=mag, frequency=285.71428571, q0=q0, gain_imu=0.033, gain_marg=0.041) + print(est.Q) + +def get_imu_estimate_cases(): + + q0 = [0.49666186227,0.034292027603,-0.0510015643620,0.86576549471] + + q_actual = [[0.49593818106, 0.0326998015547, -0.0510947167457, 0.86623632656], + [0.49519980631, 0.0312979556660, -0.0513729764280, 0.86669395238], + [0.49443437746, 0.0302718565058, -0.0520166472497, 0.86712890015], + [0.49366783997, 0.029245689468, -0.0526602014369, 0.867561903615]] + + gyr = np.array([[-0.5006895838366212, 0.8074957770569486, 0.5528888911052677], + [-0.5326483077698894, 0.6743323911175373, 0.5262569120490862], + [-0.5624759846864724, 0.5560845889656695, 0.5166698184678814], + [-0.5880433128989375, 0.39415992228264346, 0.5017542346803379]], dtype=np.float32) + acc = np.array([[0.47451228800383594, 1.0183972122654374, 8.461165125378162], + [0.3783742880038359, 0.5534032122654374, 8.264965125378161], + [0.27340728800383585, 0.05015021226543746, 8.015791125378161], + [0.3067612880038358, -0.2853517877345626, 8.159017125378163]], dtype=np.float32) + est = Madgwick(gyr=gyr, acc=acc, frequency=285.71428571, q0=q0, gain_imu=0.033, gain_marg=0.041) + print(est.Q) + + +def main(): + # get_acc_to_quat_cases() + # get_mag_to_quat_cases() + get_marg_estimate_cases() + get_imu_estimate_cases() + + +if __name__ == "__main__": + main() diff --git a/scripts/utils/read_mat_data.py b/scripts/utils/read_mat_data.py index 687fa71..67fca23 100644 --- a/scripts/utils/read_mat_data.py +++ b/scripts/utils/read_mat_data.py @@ -3,6 +3,7 @@ import pandas as pd file_path = '/home/adrian/Downloads/07_undisturbed_fast_rotation_B.mat' +# file_path = '/home/adrian/Downloads/01_undisturbed_slow_rotation_A.mat' # Load data from 'file.mat' data = sio.loadmat(file_path) @@ -18,8 +19,15 @@ total = np.hstack([acc, gyro, mag, gt_quat]) df = pd.DataFrame(total) -df = df.dropna(axis = 0, how = 'any') +# df = df.dropna(axis = 0, how = 'any') -head = df.head(10) -latex_table = head.style.to_latex() -print(latex_table) \ No newline at end of file +i = 13249 +data = df.iloc[i:i+5] +df.drop(df.columns[[0]], axis=1, inplace=True) +# df.to_csv('filename.csv', index=False) + +csv_string = data.to_csv(sep=',', index=False) +print(csv_string) + +# latex_table = data.style.to_latex() +# print(latex_table) diff --git a/setup.sh b/setup.sh index 2b473a7..28846c5 100644 --- a/setup.sh +++ b/setup.sh @@ -2,5 +2,6 @@ pip install pre-commit pre-commit install sudo apt install cmake lcov cppcheck sudo apt-get install libgtest-dev && cd /usr/src/gtest && sudo cmake CMakeLists.txt +sudo apt-get install -y libeigen3-dev && sudo ln -s /usr/include/eigen3/Eigen /usr/local/include/Eigen echo "Done setup." diff --git a/src/README.md b/src/README.md deleted file mode 100644 index 50aa3a2..0000000 --- a/src/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# Project Name Source Code - -The folders and files for this project are as follows: - -... diff --git a/src/attitude_check.cpp b/src/attitude_check.cpp index fbba835..7418af6 100644 --- a/src/attitude_check.cpp +++ b/src/attitude_check.cpp @@ -1,15 +1,145 @@ +#include + #include "attitude_check.hpp" -AttitudeCheck::AttitudeCheck() +namespace q = quaternion; +typedef Eigen::Vector3f Vec3f; +typedef q::Quaternion Quat; + +namespace attitude_check { +AttitudeCheck::AttitudeCheck(float imu_gain, float marg_gain) +{ + set_gain(imu_gain, marg_gain); +} + +AttitudeCheck::AttitudeCheck(float imu_gain, float marg_gain, float q0_w, float q0_x, float q0_y, float q0_z) +{ + set_gain(imu_gain, marg_gain); + set_quaternion(q0_w, q0_x, q0_y, q0_z); +} + +Quat AttitudeCheck::update(Vec3f& acc, Vec3f& gyr, Vec3f& mag, float dt) +{ + m_q.normalize(); + + dt = static_cast(dt); + if(dt < DT_MIN_SEC) { + throw std::invalid_argument("Dt is too small."); + } + + if(gyr.norm() <= 0.0f) { + return m_q; + } else if(mag.norm() <= 0.0f) { + return update(acc, gyr, dt); + } + + Quat q_dot = (m_q * Quat(0.0f, gyr[0], gyr[1], gyr[2])) * 0.5f; + if(acc.norm() <= 0.0f) { + m_q = m_q + (q_dot * dt); + m_q.normalize(); + return m_q; + } + + acc.normalize(); + mag.normalize(); + + Quat h = m_q * (Quat(0.0f, mag[0], mag[1], mag[2]) * m_q.conjugate()); + float bx = std::sqrt(h.x() * h.x() + h.y() * h.y()); + float bz = h.z(); + + /* *INDENT-OFF* */ + Eigen::Matrix func; + func << 2.0f * (m_q.x() * m_q.z() - m_q.w() * m_q.y()) - acc[0], + 2.0f * (m_q.w() * m_q.x() + m_q.y() * m_q.z()) - acc[1], + 2.0f * (0.5f - m_q.x() * m_q.x() - m_q.y() * m_q.y()) - acc[2], + 2.0f * bx *(0.5f - m_q.y() * m_q.y() - m_q.z() * m_q.z()) + 2.0f * bz * (m_q.x() * m_q.z() - m_q.w() * m_q.y()) - mag[0], + 2.0f * bx * (m_q.x() * m_q.y() - m_q.w() * m_q.z()) + 2.0f * bz * (m_q.w() * m_q.x() + m_q.y() * m_q.z()) - mag[1], + 2.0f * bx * (m_q.w() * m_q.y() + m_q.x() * m_q.z()) + 2.0f * bz * (0.5f - m_q.x() * m_q.x() - m_q.y() * m_q.y()) - mag[2]; + + Eigen::Matrix Jt; + Jt << -2.0f * m_q.y(), 2.0f * m_q.x(), 0.0f, -2.0f * bz * m_q.y(), -2.0f * bx * m_q.z() + 2.0f * bz * m_q.x(), 2.0f * bx * m_q.y(), + + 2.0f * m_q.z(), 2.0f * m_q.w(), -4.0f * m_q.x(), 2.0f * bz * m_q.z(), 2.0f * bx * m_q.y() + 2.0f * bz * m_q.w(), 2.0f * bx * m_q.z() - 4.0f * bz * m_q.x(), + + -2.0f * m_q.w(), 2.0f * m_q.z(), -4.0f * m_q.y(), -4.0f * bx * m_q.y() - 2.0f * bz * m_q.w(), 2.0f * bx * m_q.x() + 2.0f * bz * m_q.z(), 2.0f * bx * m_q.w() - 4.0f * bz * m_q.y(), + + 2.0f * m_q.x(), 2.0f * m_q.y(), 0.0f, -4.0f * bx * m_q.z() + 2.0f * bz * m_q.x(), -2.0f * bx * m_q.w() + 2.0f * bz * m_q.y(), 2.0f * bx * m_q.x(); + /* *INDENT-ON* */ + + Eigen::Matrix grad = Jt * func; + grad.normalize(); + q_dot -= (Quat(grad[0], grad[1], grad[2], grad[3]) * m_marg_gain); + + m_q = m_q + (q_dot * dt); + m_q.normalize(); + + return m_q; +} // AttitudeCheck::update + +Quat AttitudeCheck::update(Vec3f& acc, Vec3f& gyr, float dt) { - // input_handler(dt, gain); + m_q.normalize(); + dt = static_cast(dt); + + if(gyr.norm() <= 0.0f) { + return m_q; + } + + Quat q_dot = (m_q * Quat(0.0f, gyr[0], gyr[1], gyr[2])) * 0.5; + if(acc.norm() <= 0.0f) { + m_q = m_q + (q_dot * dt); + m_q.normalize(); + return m_q; + } + + acc.normalize(); + + Eigen::Matrix func; + func << 2.0f * (m_q.x() * m_q.z() - m_q.w() * m_q.y()) - acc[0], + 2.0f * (m_q.w() * m_q.x() + m_q.y() * m_q.z()) - acc[1], + 2.0f * (0.5 - m_q.x() * m_q.x() - m_q.y() * m_q.y()) - acc[2]; + + Eigen::Matrix Jt; + Jt << -2.0f * m_q.y(), 2.0f * m_q.x(), 0.0, + 2.0f * m_q.z(), 2.0f * m_q.w(), -4.0f * m_q.x(), + -2.0f * m_q.w(), 2.0f * m_q.z(), -4.0f * m_q.y(), + 2.0f * m_q.x(), 2.0f * m_q.y(), 0.0; + + Eigen::Matrix grad = Jt * func; + grad.normalize(); + q_dot -= (Quat(grad[0], grad[1], grad[2], grad[3]) * m_imu_gain); + m_q = m_q + (q_dot * dt); + m_q.normalize(); + + return m_q; +} // AttitudeCheck::update + +inline void AttitudeCheck::set_quaternion(float q_w, float q_x, float q_y, float q_z) +{ + try { + m_q.set(q_w, q_x, q_y, q_z); + } catch(const std::invalid_argument& e) { + throw std::invalid_argument("Initial quaternion must have a norm > 0."); + } + + m_q.normalize(); } -// AttitudeCheck::~AttitudeCheck(){ } +inline void AttitudeCheck::set_gain(const float imu_gain, const float marg_gain) +{ + if((imu_gain < GAIN_MIN || imu_gain > GAIN_MAX) || + (marg_gain < GAIN_MIN || marg_gain > GAIN_MAX)) + { + throw std::invalid_argument("Gain must be within [0.0, 1.0]."); + } -// void AttitudeCheck::input_handler() -// { -// assert(dt > 0.0); // check time is positive -// assert((gain >= 0.0) && (gain <= 1.0)); // check gain is within bounds -// } + m_imu_gain = imu_gain; + m_marg_gain = marg_gain; +} + +inline std::tuple AttitudeCheck::get_gain() +{ + return std::tuple(m_imu_gain, m_marg_gain); +} +} // End namespace attitude_check diff --git a/src/attitude_check.hpp b/src/attitude_check.hpp index 55474af..c129c97 100644 --- a/src/attitude_check.hpp +++ b/src/attitude_check.hpp @@ -1,22 +1,58 @@ #pragma once -#include -class AttitudeCheck -{ +#include + +#include "quaternion.hpp" + +namespace attitude_check { +class AttitudeCheck { public: + AttitudeCheck(float imu_gain, float marg_gain); + + AttitudeCheck(float imu_gain, float marg_gain, float q0_w, float q0_x, float q0_y, float q0_z); + + quaternion::Quaternion update(Eigen::Vector3f& acc, Eigen::Vector3f& gyr, Eigen::Vector3f& mag, float dt); + + quaternion::Quaternion update(Eigen::Vector3f& acc, Eigen::Vector3f& gyr, float dt); + + /** + * @brief Manually set the quaternion component. Useful when a new initial quaternion + * for subsequent calcualtion is desired. For example, after determining absolute + * orientation through another source, the quaternion can be reset such that the orientation + * is relative to the new known orientation. + * + * @param q_w + * @param q_x + * @param q_y + * @param q_z + */ + void set_quaternion(float q_w, float q_x, float q_y, float q_z); + + /** + * @brief Set the filter gain values. + * + * @param imu_gain Gain when only Accel and Gyro are used. + * @param marg_gain Gain when Accel, Gyro, and Mag are used. + */ + void + set_gain(float imu_gain, float marg_gain); - AttitudeCheck(); + /** + * @brief Get the gains via tuple. + * Example: `auto [imu_gain, marg_gain] = a.get_gain();` + * + * @return std::tuple + */ + std::tuple get_gain(); private: + const float RATE_MAX_HZ { 10000.0 }; + const float DT_MIN_SEC { 1 / RATE_MAX_HZ }; + const float GAIN_MIN { 0.0 }, GAIN_MAX { 1.0 }; - // float m_dt { 100.0 }; // [ms] time delta between measurements (sensor rate) - // float m_gain { 0.5 }; // filter gain parameter + quaternion::Quaternion m_q; - // /** - // * @brief Check input values are valid - // * - // * @param dt - // * @param gain - // */ - // void input_handler(float dt, float gain); + float m_dt { 100.0 }; + float m_imu_gain { 0.5 }, m_marg_gain { 0.5 }; }; +} // End namespace attitude_check diff --git a/src/initializers.hpp b/src/initializers.hpp new file mode 100644 index 0000000..4bf4af6 --- /dev/null +++ b/src/initializers.hpp @@ -0,0 +1,56 @@ +/** + * @file initializers.hpp + * @brief Creates inital orientation estimates given normalized acceleration or + * normalized acceleration and magnetometer measurements + * + * @copyright Copyright (c) 2024 + * + */ +#pragma once + +#include "quaternion.hpp" +#include "utilities.hpp" +#include + +namespace init { +template +using Vector3T = Eigen::Matrix; + +/** + * @brief Computes a quaternion from a single normalized accerleration measurement. + * + * @tparam T float or double + * @param acc Normalized 3x1 accerleration vector + * @return quaternion::Quaternion + */ +template +inline quaternion::Quaternion acc_to_quat(const Vector3T& acc) +{ + T roll = std::atan2(acc[1], acc[2]); + T pitch = std::atan2(-acc[0], + std::sqrt(std::pow(acc[1], 2.0) + std::pow(acc[2], 2.0))); + + return quaternion::Quaternion(utils::euler_to_quat(roll, pitch, static_cast(0.0))); +} + +/** + * @brief Computes a quaternion from normalized accelerometer and magnetometer data. + * + * @tparam T float or double + * @param acc Normalized 3x1 accerleration vector + * @param mag Normalized 3x1 magnetometer vector + * @return quaternion::Quaternion + */ +template +inline quaternion::Quaternion mag_to_quat(const Vector3T& acc, const Vector3T& mag) +{ + T roll = std::atan2(acc[1], acc[2]); + T pitch = std::atan(-acc[0]/std::sqrt(std::pow(acc[1], 2.0) + std::pow(acc[2], 2.0))); + + T s_roll = std::sin(roll), c_roll = std::cos(roll); + T yaw = std::atan2(mag[2] * s_roll - mag[1] * c_roll, + mag[0] * std::cos(pitch) + std::sin(pitch)*(mag[1] * s_roll + mag[2] * c_roll)); + + return quaternion::Quaternion(utils::euler_to_quat(roll, pitch, yaw)); +} +} // End namespace init diff --git a/src/quaternion.hpp b/src/quaternion.hpp index 89fc9a7..0f8cc44 100644 --- a/src/quaternion.hpp +++ b/src/quaternion.hpp @@ -15,11 +15,56 @@ namespace quaternion { template class Quaternion { public: + Quaternion() : m_q{static_cast(1.0), static_cast(0.0), + static_cast(0.0), static_cast(0.0)}{ }; + Quaternion(T w, T x, T y, T z) : m_q{w, x, y, z} { - if(std::sqrt(w * w + x * x + y * y + z * z) == 0.0f) { + if((std::abs(w) + std::abs(x) + std::abs(y) + std::abs(z)) <= EPS) { + throw std::invalid_argument("Magnitude of quaternion cannot be zero."); + } + } + + Quaternion(const Quaternion& b) + { + m_q[0] = b.w(); + m_q[1] = b.x(); + m_q[2] = b.y(); + m_q[3] = b.z(); + } + + /** + * @brief Overload the assigment operator for this class + * + * @param other + * @return Quaternion& + */ + Quaternion& operator=(const Quaternion& b) { + if (this != &b) { + m_q[0] = b.w(); + m_q[1] = b.x(); + m_q[2] = b.y(); + m_q[3] = b.z(); + } + return *this; + } + + /** + * @brief Set quaternion values + * + * @param w float/double + * @param x float/double + * @param y float/double + * @param z float/double + */ + inline void set(const T w, const T x, const T y, const T z) { + if((std::abs(w) + std::abs(x) + std::abs(y) + std::abs(z)) <= EPS) { throw std::invalid_argument("Magnitude of quaternion cannot be zero."); } + m_q[0] = w; + m_q[1] = x; + m_q[2] = y; + m_q[3] = z; } /** @@ -64,10 +109,10 @@ class Quaternion { * @brief Overload the * (multiplication operator) to perform * quaternion multiplication (hamiltonian product) * - * @param b + * @param b Quaternion * @return Quaternion */ - Quaternion operator * (Quaternion const& b) const + inline Quaternion operator * (Quaternion const& b) const { return Quaternion( (m_q[0] * b.w() - m_q[1] * b.x() - m_q[2] * b.y() - m_q[3] * b.z()), (m_q[0] * b.x() + m_q[1] * b.w() + m_q[2] * b.z() - m_q[3] * b.y()), @@ -75,11 +120,59 @@ class Quaternion { (m_q[0] * b.z() + m_q[3] * b.w() + m_q[1] * b.y() - m_q[2] * b.x())); } + /** + * @brief Overload the * (multiplication operator) to perform + * scalar multiplication (scalar must be on the right) + * + * @param k float/double + * @return Quaternion + */ + inline Quaternion operator * (T const& k) const + { + return Quaternion(k * m_q[0], k * m_q[1], k * m_q[2], k * m_q[3]); + } + + /** + * @brief Overload the + operator to add 2 Quaternions. + * + * @param b Quaternion + * @return Quaternion + */ + inline Quaternion operator + (Quaternion const& b) const + { + return Quaternion(m_q[0] + b.w(), m_q[1] + b.x(), m_q[2] + b.y(), m_q[3] + b.z()); + } + + /** + * @brief Overload the - operator to subtract 2 Quaternions. + * + * @param b Quaternion + * @return Quaternion + */ + inline Quaternion operator - (Quaternion const& b) const + { + return Quaternion(m_q[0] - b.w(), m_q[1] - b.x(), m_q[2] - b.y(), m_q[3] - b.z()); + } + + /** + * @brief Overload the -= operator to subtract 2 Quaternions. + * + * @param b Quaternion + * @return Quaternion + */ + inline void operator -= (Quaternion const& b) + { + m_q[0] -= b.w(); + m_q[1] -= b.x(); + m_q[2] -= b.y(); + m_q[3] -= b.z(); + } + /** * @brief Normalize the quaternion such that it becomes a unit quaternion. L2 norm = 1. * */ - void normalize() + inline void normalize() { T euclid_dist = std::sqrt( std::pow(m_q[0], 2.0) + std::pow(m_q[1], 2.0) @@ -92,6 +185,48 @@ class Quaternion { } private: + const T EPS { 0.0000001 }; std::array m_q; }; + +// float fast_inv_sqrt(float x) { +// float halfx = 0.5f * x; +// float y = x; +// long i = *(long*)&y; +// i = 0x5f3759df - (i>>1); +// y = *(float*)&i; +// y = y * (1.5f - (halfx * y * y)); +// y = y * (1.5f - (halfx * y * y)); +// return y; +// } + +// float fast_inv_sqrt(float x) { +// union { +// int i; +// float y; +// long z; +// } data; + +// // float data; + +// float halfx = 0.5f * x; +// data.y = x; +// long i = *(long*)&data.y; +// i = 0x5f3759df - (i>>1); +// data.y = *(float*)&i; +// data.y = data.y * (1.5f - (halfx * data.y * data.y)); +// data.y = data.y * (1.5f - (halfx * data.y * data.y)); +// return data.y; +// } + +// template <> +// void Quaternion::normalize() { +// float i_sqrr = fast_inv_sqrt(std::pow(m_q[0], 2.0) + std::pow(m_q[1], 2.0) +// + std::pow(m_q[2], 2.0) + std::pow(m_q[3], 2.0)); + +// m_q[0] *= i_sqrr; +// m_q[1] *= i_sqrr; +// m_q[2] *= i_sqrr; +// m_q[3] *= i_sqrr; +// } } // End namespace quaternion diff --git a/src/utilities.hpp b/src/utilities.hpp new file mode 100644 index 0000000..c184444 --- /dev/null +++ b/src/utilities.hpp @@ -0,0 +1,71 @@ +/** + * @file utilities.hpp + * @brief Utilities for converting other rotation fromats into quaternions + * + * @copyright Copyright (c) 2024 + * + */ +#pragma once + +#include "quaternion.hpp" +#include +#include + +namespace utils { +/** + * @brief Convert Euler angles to a Quaternion + * This follows the ZYX, or Yall*Pitch*Roll convention + * + * @tparam T + * @param roll + * @param pitch + * @param yaw + * @return Quaternion + */ +template +quaternion::Quaternion euler_to_quat(const T& roll, const T& pitch, const T& yaw) +{ + T c_pitch = cos(pitch * 0.5); + T c_roll = cos(roll * 0.5); + T s_pitch = sin(pitch * 0.5); + T s_roll = sin(roll * 0.5); + + if(std::abs(yaw) <= 0.0000001) { + return quaternion::Quaternion(c_roll * c_pitch, s_roll * c_pitch, c_roll * s_pitch, -s_roll * s_pitch); + } + + T c_yaw = cos(yaw * 0.5); + T s_yaw = sin(yaw * 0.5); + + T c_yaw_c_pitch = c_yaw * c_pitch; + T s_yaw_s_pitch = s_yaw * s_pitch; + + return quaternion::Quaternion(c_yaw_c_pitch * c_roll + s_yaw_s_pitch * s_roll, + c_yaw_c_pitch * s_roll - s_yaw_s_pitch * c_roll, + s_yaw * c_pitch * s_roll + c_yaw * s_pitch * c_roll, + s_yaw * c_pitch * c_roll - c_yaw * s_pitch * c_roll); +} + +template +using Matrix3T = Eigen::Matrix; + +/** + * @brief Convert Rotation matrix to Quaternion + * + * @tparam T + * @param R rotation matrix + * @return quaternion::Quaternion + */ +template +quaternion::Quaternion rotm_to_quat(const Matrix3T& R) +{ + T w = std::sqrt(static_cast(1.0) + R(0, 0) + R(1, 1) + R(2, 2)) / 2.0; + + const T FOUR_W { w * static_cast(4.0) }; + T x = (R(2, 1) - R(1, 2)) / FOUR_W; + T y = (R(0, 2) - R(2, 0)) / FOUR_W; + T z = (R(1, 0) - R(0, 1)) / FOUR_W; + + return quaternion::Quaternion(w, x, y, z); +} +} // End namespace utils diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 371b565..86c28d5 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -15,11 +15,15 @@ find_package(GTest REQUIRED) enable_testing() +set(CMAKE_CXX_FLAGS "-fprofile-arcs -ftest-coverage") + add_executable( ${BINARY} main.cpp attitude_check_test.cpp quaternion_test.cpp + utilities_test.cpp + initializers_test.cpp ) target_link_libraries( diff --git a/test/attitude_check_test.cpp b/test/attitude_check_test.cpp index d63713a..f70358c 100644 --- a/test/attitude_check_test.cpp +++ b/test/attitude_check_test.cpp @@ -1,6 +1,283 @@ +#include + #include "gtest/gtest.h" -#include "attitude_check.hpp" -TEST(blaTest, test1) { - EXPECT_EQ (69, 69); +#include "attitude_check.cpp" +// #include "attitude_check.hpp" + +const float MAX_ABS_ERROR { 0.002 }; // About 0.115 of a degree + +namespace ac = attitude_check; + +class ACheck_Test_Fixture : public ::testing::Test { +protected: + float dt; // dt = 1/Hz + Eigen::Vector3f acc { 0.0f, 0.0f, 9.81f }; + Eigen::Vector3f mag { 16676.8f, -3050.9f, 49916.9f }; + Eigen::Vector3f gyr { 1.0f, 1.0f, 1.0f }; + + void SetUp() override + { + dt = 1.0 / 100.0; + acc.normalize(); + mag.normalize(); + mag.normalize(); + } +}; + +TEST_F(ACheck_Test_Fixture, invalid_init){ + EXPECT_THROW({ + try + { + ac::AttitudeCheck ac(-1.0, 0.5); + } + catch(const std::invalid_argument& e) + { + EXPECT_STREQ("Gain must be within [0.0, 1.0].", e.what() ); + throw; + } + }, std::invalid_argument); + + EXPECT_THROW({ + try + { + ac::AttitudeCheck ac(1.0, 2.0); + } + catch(const std::invalid_argument& e) + { + EXPECT_STREQ("Gain must be within [0.0, 1.0].", e.what() ); + throw; + } + }, std::invalid_argument); + + EXPECT_THROW({ + try + { + ac::AttitudeCheck ac(0.2f, 0.2f, 0.0f, 0.0f, 0.0f, 0.0f); + } + catch(const std::invalid_argument& e) + { + EXPECT_STREQ("Initial quaternion must have a norm > 0.", e.what() ); + throw; + } + }, std::invalid_argument); +} + + +TEST_F(ACheck_Test_Fixture, invalid_update){ + ac::AttitudeCheck ac(0.8, 0.999); + + EXPECT_THROW({ + try + { + ac.update(acc, gyr, mag, -0.00001f); + } + catch(const std::invalid_argument& e) + { + EXPECT_STREQ("Dt is too small.", e.what() ); + throw; + } + }, std::invalid_argument); +} + +TEST_F(ACheck_Test_Fixture, marg_zero_gyro){ + gyr.setZero(); + + ac::AttitudeCheck ac(0.5f, 0.5f, -69.0f, -69.0f, -69.0f, -69.0f); + quaternion::Quaternion out = ac.update(acc, gyr, mag, dt); + + EXPECT_NEAR(out.w(), -0.5f, MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), -0.5f, MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), -0.5f, MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), -0.5f, MAX_ABS_ERROR); +} + +TEST_F(ACheck_Test_Fixture, marg_zero_mag){ + mag.setZero(); + acc.setZero(); + + quaternion::Quaternion expected(-69.0f, -69.0f, -69.0f, -69.0f); + expected = expected * (quaternion::Quaternion(0.0f, gyr[0], gyr[1], gyr[2]) * 0.5); + expected = quaternion::Quaternion(-69.0f, -69.0f, -69.0f, -69.0f) + (expected * dt); + expected.normalize(); + + ac::AttitudeCheck ac(0.5f, 0.5f, -69.0f, -69.0f, -69.0f, -69.0f); + quaternion::Quaternion out = ac.update(acc, gyr, mag, dt); + + EXPECT_NEAR(out.w(), expected.w(), MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), expected.x(), MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), expected.y(), MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), expected.z(), MAX_ABS_ERROR); +} + +TEST_F(ACheck_Test_Fixture, marg_zero_acc){ + acc.setZero(); + + quaternion::Quaternion expected(-69.0f, -69.0f, -69.0f, -69.0f); + expected = expected * (quaternion::Quaternion(0.0f, gyr[0], gyr[1], gyr[2]) * 0.5); + expected = quaternion::Quaternion(-69.0f, -69.0f, -69.0f, -69.0f) + (expected * dt); + expected.normalize(); + + ac::AttitudeCheck ac(0.5f, 0.5f, -69.0f, -69.0f, -69.0f, -69.0f); + quaternion::Quaternion out = ac.update(acc, gyr, mag, dt); + + EXPECT_NEAR(out.w(), expected.w(), MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), expected.x(), MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), expected.y(), MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), expected.z(), MAX_ABS_ERROR); +} + +TEST_F(ACheck_Test_Fixture, imu_zero_gyro){ + gyr.setZero(); + + ac::AttitudeCheck ac(0.5f, 0.5f, -69.0f, -69.0f, -69.0f, -69.0f); + quaternion::Quaternion out = ac.update(acc, gyr, dt); + + EXPECT_NEAR(out.w(), -0.5f, MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), -0.5f, MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), -0.5f, MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), -0.5f, MAX_ABS_ERROR); +} + + +TEST_F(ACheck_Test_Fixture, imu_zero_acc){ + acc.setZero(); + + quaternion::Quaternion expected(-69.3333f, -69.0f, -69.0f, -69.0f); + expected = expected * (quaternion::Quaternion(0.0f, gyr[0], gyr[1], gyr[2]) * 0.5); + expected = quaternion::Quaternion(-69.3333f, -69.0f, -69.0f, -69.0f) + (expected * dt); + expected.normalize(); + + ac::AttitudeCheck ac(0.5f, 0.5f, -69.3333f, -69.0f, -69.0f, -69.0f); + quaternion::Quaternion out = ac.update(acc, gyr, mag, dt); + + EXPECT_NEAR(out.w(), expected.w(), MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), expected.x(), MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), expected.y(), MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), expected.z(), MAX_ABS_ERROR); +} + +TEST_F(ACheck_Test_Fixture, set_quaternion){ + ac::AttitudeCheck ac(0.5f, 0.5f, -69.0f, -69.0f, -69.0f, -69.0f); + + ac.set_quaternion(1.0f, 0.0f, 0.0f, 0.0f); + gyr.setZero(); + + quaternion::Quaternion out = ac.update(acc, gyr, mag, dt); + + EXPECT_NEAR(out.w(), 1.0f, MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), 0.0f, MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), 0.0f, MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), 0.0f, MAX_ABS_ERROR); +} + +TEST_F(ACheck_Test_Fixture, set_get_gain){ + ac::AttitudeCheck ac(0.5f, 0.5f, -69.0f, -69.0f, -69.0f, -69.0f); + float in1 { 1.0f }, in2 { 1.0f }; + + ac.set_gain(in1, in2); + + auto [imu, marg] = ac.get_gain(); + + ASSERT_FLOAT_EQ(in1, imu); + ASSERT_FLOAT_EQ(in2, marg); +} + +class ACheck_Estimator_Test_Fixture : public ::testing::Test { +protected: + // Tuple(Acc, Gyr, Mag, Expected_Quaternion) + typedef std::tuple T; + std::vector test_data; + + void SetUp() override { } +}; + +TEST_F(ACheck_Estimator_Test_Fixture, update_marg_with_intitial){ + ac::AttitudeCheck ac(0.033f, 0.041f, 0.49666186227f, 0.034292027603f, -0.0510015643620f, 0.86576549471f); + + /* *INDENT-OFF* */ + test_data = { + T(Eigen::Vector3f(0.47451228800383594, 1.0183972122654374, 8.461165125378162), + Eigen::Vector3f(-0.5006895838366212, 0.8074957770569486, 0.5528888911052677), + Eigen::Vector3f(10.04650728048766, -6.088486551348383, -43.828022623283225), + Eigen::Vector4f(0.49598288, 0.03266141,-0.05120194, 0.86620585), + 1.0/285.71428571), + + T(Eigen::Vector3f(0.3783742880038359, 0.5534032122654374, 8.264965125378161), + Eigen::Vector3f(-0.5326483077698894, 0.6743323911175373, 0.5262569120490862), + Eigen::Vector3f(11.084734479270914, -6.77198980959856, -44.35222010806493), + Eigen::Vector4f(0.49530668, 0.03117841, -0.05155569, 0.86662634), + 1.0/285.71428571), + + T(Eigen::Vector3f(0.27340728800383585, 0.05015021226543746, 8.015791125378161), + Eigen::Vector3f(-0.5624759846864724, 0.5560845889656695, 0.5166698184678814), + Eigen::Vector3f(10.19482545174241, -6.923879422543039, -43.97779333322085), + Eigen::Vector4f(0.49463786, 0.0299162, -0.05209187, 0.86702067), + 1.0/285.71428571), + + T(Eigen::Vector3f(0.3067612880038358, -0.2853517877345626, 8.159017125378163), + Eigen::Vector3f(-0.5880433128989375, 0.39415992228264346, 0.5017542346803379), + Eigen::Vector3f(9.30491642421391, -7.075769035487526, -43.60336655837678), + Eigen::Vector4f(0.49463786, 0.0299162, -0.05209187, 0.86702067), + 1.0/285.71428571) + }; + /* *INDENT-ON* */ + + size_t count = 0; + for(auto tuple : test_data) { + auto [t_acc, t_gyr, t_mag, q_exp, t_dt] = tuple; + + quaternion::Quaternion out = ac.update(t_acc, t_gyr, t_mag, t_dt); + + SCOPED_TRACE("At iteration " + std::to_string(count++)); + EXPECT_NEAR(out.w(), q_exp[0], MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), q_exp[1], MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), q_exp[2], MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), q_exp[3], MAX_ABS_ERROR); + } +} + +TEST_F(ACheck_Estimator_Test_Fixture, update_imu_with_intitial){ + ac::AttitudeCheck ac(0.033f, 0.041f, 0.49666186227f, 0.034292027603f, -0.0510015643620f, 0.86576549471f); + + /* *INDENT-OFF* */ + test_data = { + T(Eigen::Vector3f(0.47451228800383594, 1.0183972122654374, 8.461165125378162), + Eigen::Vector3f(-0.5006895838366212, 0.8074957770569486, 0.5528888911052677), + Eigen::Vector3f(10.04650728048766, -6.088486551348383, -43.828022623283225), + Eigen::Vector4f(0.49596236, 0.03276414, -0.05113933, 0.86621742), + 1.0/285.71428571), + + T(Eigen::Vector3f(0.3783742880038359, 0.5534032122654374, 8.264965125378161), + Eigen::Vector3f(-0.5326483077698894, 0.6743323911175373, 0.5262569120490862), + Eigen::Vector3f(11.084734479270914, -6.77198980959856, -44.35222010806493), + Eigen::Vector4f(0.49526518, 0.03134777, -0.05143123, 0.86665134), + 1.0/285.71428571), + + T(Eigen::Vector3f(0.27340728800383585, 0.05015021226543746, 8.015791125378161), + Eigen::Vector3f(-0.5624759846864724, 0.5560845889656695, 0.5166698184678814), + Eigen::Vector3f(10.19482545174241, -6.923879422543039, -43.97779333322085), + Eigen::Vector4f(0.49457367, 0.03011946, -0.05192297, 0.86706038), + 1.0/285.71428571), + + T(Eigen::Vector3f(0.3067612880038358, -0.2853517877345626, 8.159017125378163), + Eigen::Vector3f(-0.5880433128989375, 0.39415992228264346, 0.5017542346803379), + Eigen::Vector3f(9.30491642421391, -7.075769035487526, -43.60336655837678), + Eigen::Vector4f(0.49457367, 0.03011946, -0.05192297, 0.86706038), + 1.0/285.71428571) + }; + /* *INDENT-ON* */ + + size_t count = 0; + for(auto tuple : test_data) { + auto [t_acc, t_gyr, t_mag, q_exp, t_dt] = tuple; + + quaternion::Quaternion out = ac.update(t_acc, t_gyr, t_dt); + + SCOPED_TRACE("At iteration " + std::to_string(count++)); + EXPECT_NEAR(out.w(), q_exp[0], MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), q_exp[1], MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), q_exp[2], MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), q_exp[3], MAX_ABS_ERROR); + } } diff --git a/test/initializers_test.cpp b/test/initializers_test.cpp new file mode 100644 index 0000000..1ad69a4 --- /dev/null +++ b/test/initializers_test.cpp @@ -0,0 +1,96 @@ +#include "gtest/gtest.h" +#include "initializers.hpp" + +const float MAX_ABS_ERROR { 0.002 }; // About 0.115 of a degree + +/** + * @brief Test acc_to_quat + * + */ +TEST(initializers_test_suite, acc_d){ + Eigen::Vector3d acc { 0.090941, -0.031273, 9.759028 }; + acc.normalize(); + + quaternion::Quaternion out = init::acc_to_quat(acc); + out.normalize(); + + EXPECT_NEAR(out.w(), 0.999987863, MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), -0.001602236, MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), -0.004659145, MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), -0.000007465, MAX_ABS_ERROR); +} + +TEST(initializers_test_suite, acc1_f){ + Eigen::Vector3f acc { 0.090941f, -0.031273f, 9.759028f }; + acc.normalize(); + + quaternion::Quaternion out = init::acc_to_quat(acc); + out.normalize(); + + EXPECT_NEAR(out.w(), 0.999987f, MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), -0.0016022f, MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), -0.0046f, MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), 0.0f, MAX_ABS_ERROR); +} + +TEST(initializers_test_suite, acc2_f){ + Eigen::Vector3f acc { -9.81f, 0.0f, 0.0f }; + acc.normalize(); + + quaternion::Quaternion out = init::acc_to_quat(acc); + out.normalize(); + + EXPECT_NEAR(out.w(), 0.7071f, MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), 0.0f, MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), 0.7071f, MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), 0.0f, MAX_ABS_ERROR); +} + +TEST(initializers_test_suite, mag_d){ + Eigen::Vector3d acc { 0.0, 0.0, 9.81 }; + acc.normalize(); + + Eigen::Vector3d mag { 16676.8, -3050.9, 49916.9 }; + mag.normalize(); + + quaternion::Quaternion out = init::mag_to_quat(acc, mag); + out.normalize(); + + EXPECT_NEAR(out.w(), 0.99591029, MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), 0.0, MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), 0.0, MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), 0.09034758, MAX_ABS_ERROR); +} + +TEST(initializers_test_suite, mag1_f){ + Eigen::Vector3f acc { 0.0f, 0.0f, 9.81f }; + acc.normalize(); + + Eigen::Vector3f mag { 16676.8f, -3050.9f, 49916.9f }; + mag.normalize(); + + quaternion::Quaternion out = init::mag_to_quat(acc, mag); + out.normalize(); + + EXPECT_NEAR(out.w(), 0.99591029f, MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), 0.0f, MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), 0.0f, MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), 0.09034758f, MAX_ABS_ERROR); +} + +// TEST(initializers_test_suite, mag2_f){ +// Eigen::Vector3f acc { -6.382152f, -7.969839f, 0.099505f }; +// acc.normalize(); + +// Eigen::Vector3f mag { 36.700461f, 22.613240f, 8.881961f }; +// mag.normalize(); + +// quaternion::Quaternion out = init::mag_to_quat(acc, mag); +// out.normalize(); + +// EXPECT_NEAR(out.w(), 0.70953f, MAX_ABS_ERROR); +// EXPECT_NEAR(out.x(), -0.57186f, MAX_ABS_ERROR); +// EXPECT_NEAR(out.y(), 0.409993f, MAX_ABS_ERROR); +// EXPECT_NEAR(out.z(), 0.037796f, MAX_ABS_ERROR); + +// } diff --git a/test/quaternion_test.cpp b/test/quaternion_test.cpp index 417e3b6..b6496b7 100644 --- a/test/quaternion_test.cpp +++ b/test/quaternion_test.cpp @@ -13,6 +13,18 @@ TEST(quat_test_suite, invalid_init){ throw; } }, std::invalid_argument); + + EXPECT_THROW({ + try + { + quaternion::Quaternion a(0.00000001, 0.0, 0.0, 0.0); + } + catch(const std::invalid_argument& e) + { + EXPECT_STREQ("Magnitude of quaternion cannot be zero.", e.what() ); + throw; + } + }, std::invalid_argument); } TEST(quat_test_suite, conjugate_f){ @@ -46,19 +58,88 @@ TEST(quat_test_suite, product){ EXPECT_DOUBLE_EQ(0.0, c.z()); } -TEST(quat_test_suite, norm){ +TEST(quat_test_suite, norm_d){ quaternion::Quaternion a(-19.0, -2.1, 3.3, 6.9); a.normalize(); auto norm = std::sqrt( - std::pow(a.w(), 2.0) + std::pow(a.x(), 2.0) + - std::pow(a.y(), 2.0) + std::pow(a.z(), 2.0)); + std::pow(a.w(), 2.0) + std::pow(a.x(), 2.0) + + std::pow(a.y(), 2.0) + std::pow(a.z(), 2.0)); EXPECT_DOUBLE_EQ(1.0, norm); - // EXPECT_DOUBLE_EQ(-0.92281951, a.w()); - // EXPECT_DOUBLE_EQ(-0.1019958, a.x()); - // EXPECT_DOUBLE_EQ(0.16027, a.y()); - // EXPECT_DOUBLE_EQ(0.3351, a.z()); + EXPECT_DOUBLE_EQ(-0.92281951518771288, a.w()); + EXPECT_DOUBLE_EQ(-0.10199584115232617, a.x()); + EXPECT_DOUBLE_EQ(0.16027917895365537, a.y()); + EXPECT_DOUBLE_EQ(0.3351291923576431, a.z()); +} + +TEST(quat_test_suite, norm_f){ + quaternion::Quaternion a(-19.0f, -2.1f, 3.3f, 6.9f); + + a.normalize(); + + auto norm = std::sqrt( + std::pow(a.w(), 2.0f) + std::pow(a.x(), 2.0f) + + std::pow(a.y(), 2.0f) + std::pow(a.z(), 2.0f)); + + EXPECT_FLOAT_EQ(1.0f, norm); + EXPECT_FLOAT_EQ(-0.92281951f, a.w()); + EXPECT_FLOAT_EQ(-0.1019958f, a.x()); + EXPECT_FLOAT_EQ(0.16027917f, a.y()); + EXPECT_FLOAT_EQ(0.3351292f, a.z()); +} + +TEST(quat_test_suite, scalar_f){ + quaternion::Quaternion a(2.0f, -2.0f, 3.0f, 6.0f); + + a = a * 0.5; + + EXPECT_FLOAT_EQ(1.0f, a.w()); + EXPECT_FLOAT_EQ(-1.0f, a.x()); + EXPECT_FLOAT_EQ(1.5f, a.y()); + EXPECT_FLOAT_EQ(3.0f, a.z()); +} + +TEST(quat_test_suite, add_f){ + quaternion::Quaternion a(2.0f, -2.0f, 3.0f, 6.0f); + + a = a + a; + + EXPECT_FLOAT_EQ(4.0f, a.w()); + EXPECT_FLOAT_EQ(-4.0f, a.x()); + EXPECT_FLOAT_EQ(6.0f, a.y()); + EXPECT_FLOAT_EQ(12.0f, a.z()); +} + +TEST(quat_test_suite, subtract_f){ + quaternion::Quaternion a(2.0f, -2.0f, 3.0f, 6.0f); + + a = a - a*0.5; + + EXPECT_FLOAT_EQ(1.0f, a.w()); + EXPECT_FLOAT_EQ(-1.0f, a.x()); + EXPECT_FLOAT_EQ(1.5f, a.y()); + EXPECT_FLOAT_EQ(3.0f, a.z()); +} + +TEST(quat_test_suite, subtract_equals_f){ + quaternion::Quaternion a(2.0f, -2.0f, 3.0f, 6.0f); + + a -= a*0.5; + + EXPECT_FLOAT_EQ(1.0f, a.w()); + EXPECT_FLOAT_EQ(-1.0f, a.x()); + EXPECT_FLOAT_EQ(1.5f, a.y()); + EXPECT_FLOAT_EQ(3.0f, a.z()); +} + +TEST(quat_test_suite, set_f){ + quaternion::Quaternion a(2.0f, -2.0f, 3.0f, 6.0f); + a.set(-100.0f, -101.0f, -102.0f, -103.0f); + EXPECT_FLOAT_EQ(-100.0f, a.w()); + EXPECT_FLOAT_EQ(-101.0f, a.x()); + EXPECT_FLOAT_EQ(-102.0f, a.y()); + EXPECT_FLOAT_EQ(-103.0f, a.z()); } diff --git a/test/utilities_test.cpp b/test/utilities_test.cpp new file mode 100644 index 0000000..efd5844 --- /dev/null +++ b/test/utilities_test.cpp @@ -0,0 +1,74 @@ +#include "gtest/gtest.h" +#include "utilities.hpp" + +const float MAX_ABS_ERROR { 0.002 }; // About 0.115 of a degree + +TEST(utilities_test_suite, euler_d){ + quaternion::Quaternion out = utils::euler_to_quat(0.0, 0.0, 0.0); + out.normalize(); + + EXPECT_DOUBLE_EQ(out.w(), 1.0); + EXPECT_DOUBLE_EQ(out.x(), 0.0); + EXPECT_DOUBLE_EQ(out.y(), 0.0); + EXPECT_DOUBLE_EQ(out.z(), 0.0); +} + +TEST(utilities_test_suite, euler_f){ + quaternion::Quaternion out = utils::euler_to_quat(0.0f, 0.0f, 0.0f); + out.normalize(); + + EXPECT_FLOAT_EQ(out.w(), 1.0f); + EXPECT_FLOAT_EQ(out.x(), 0.0f); + EXPECT_FLOAT_EQ(out.y(), 0.0f); + EXPECT_FLOAT_EQ(out.z(), 0.0f); +} + +TEST(utilities_test_suite, euler1_f){ + quaternion::Quaternion out = utils::euler_to_quat(1.55f, 0.06f, -2.59f); + out.normalize(); + + EXPECT_NEAR(out.w(), 0.1742636f, MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), 0.2110758f, MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), -0.6671344f, MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), -0.6928282f, MAX_ABS_ERROR); +} + +TEST(utilities_test_suite, euler2_f){ + quaternion::Quaternion out = utils::euler_to_quat(-20.0f, -0.005f, 3.140f); + out.normalize(); + + EXPECT_NEAR(out.w(), -0.0020282f, MAX_ABS_ERROR); + EXPECT_NEAR(out.x(), -0.0016645f, MAX_ABS_ERROR); + EXPECT_NEAR(out.y(), 0.5440209f, MAX_ABS_ERROR); + EXPECT_NEAR(out.z(), -0.8390676f, MAX_ABS_ERROR); +} + +TEST(utilities_test_suite, rotm_d){ + Eigen::Matrix3d in; + in << 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0; + + quaternion::Quaternion out = utils::rotm_to_quat(in); + out.normalize(); + + EXPECT_DOUBLE_EQ(out.w(), 1.0); + EXPECT_DOUBLE_EQ(out.x(), 0.0); + EXPECT_DOUBLE_EQ(out.y(), 0.0); + EXPECT_DOUBLE_EQ(out.z(), 0.0); +} + +TEST(utilities_test_suite, rotm_f){ + Eigen::Matrix3f in; + in << 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0; + + quaternion::Quaternion out = utils::rotm_to_quat(in); + out.normalize(); + + EXPECT_FLOAT_EQ(out.w(), 1.0f); + EXPECT_FLOAT_EQ(out.x(), 0.0f); + EXPECT_FLOAT_EQ(out.y(), 0.0f); + EXPECT_FLOAT_EQ(out.z(), 0.0f); +}