diff --git a/.DS_Store b/.DS_Store deleted file mode 100644 index 4dae19f..0000000 Binary files a/.DS_Store and /dev/null differ diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml new file mode 100644 index 0000000..b45de18 --- /dev/null +++ b/.github/workflows/cmake.yml @@ -0,0 +1,42 @@ +name: Build +on: + push: + +jobs: + build-project: + strategy: + matrix: + config: + - { + os: ubuntu-latest, + cmake_options: "", + dependencies: "sudo apt update -yqq && sudo apt -yqq install libboost-dev" + } + - { + os: macos-latest, + cmake_options: "CMAKE_PREFIX_PATH=/opt/homebrew", + dependencies: "brew install boost eigen && brew link boost eigen" + } + - { + os: windows-latest, + cmake_options: "BOOST_ROOT=c:/local/boost_1_86_0", + dependencies: "choco install eigen -y --no-progress && choco install boost-msvc-14.3 -y --no-progress" + } + + runs-on: ${{ matrix.config.os }} + steps: + - name: Checkout Project + uses: actions/checkout@v4 + with: + submodules: recursive + + - name: Dependencies + run: ${{ matrix.config.dependencies }} + + - name: Build Project + uses: threeal/cmake-action@v2.0.0 + with: + options: | + PUARA_GESTURES_ENABLE_STANDALONE=0 + ${{ matrix.config.cmake_options }} + diff --git a/.gitignore b/.gitignore index eebb7c8..b5fabc7 100644 --- a/.gitignore +++ b/.gitignore @@ -3,4 +3,3 @@ build/ .vscode/ *.user *code-workspace -.DS_Store diff --git a/.gitmodules b/.gitmodules index 4ca015b..ae14b99 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,3 @@ [submodule "IMU_Sensor_Fusion"] - path = descriptors/IMU_Sensor_Fusion + path = 3rdparty/IMU_Sensor_Fusion url = https://github.com/malloch/IMU_Sensor_Fusion diff --git a/descriptors/IMU_Sensor_Fusion b/3rdparty/IMU_Sensor_Fusion similarity index 100% rename from descriptors/IMU_Sensor_Fusion rename to 3rdparty/IMU_Sensor_Fusion diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..d30a4bf --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,78 @@ +cmake_minimum_required(VERSION 3.24...3.30) + +project( + puara-gestures + VERSION 0.2 + LANGUAGES CXX +) + +### Library options ### +option(PUARA_GESTURES_ENABLE_TESTING "Enable building and running Puara gestures tests" ON) +option(PUARA_GESTURES_ENABLE_STANDALONE "Enable building and running Puara standalone" OFF) + +### Dependencies ### +if(POLICY CMP0167) + cmake_policy(SET CMP0167 OLD) +endif() +if(NOT TARGET Boost::headers) + find_package(Boost REQUIRED) +endif() + +### Main library ### +add_library(puara_gestures + 3rdparty/IMU_Sensor_Fusion/imu_orientation.h + 3rdparty/IMU_Sensor_Fusion/imu_orientation.cpp + + include/puara/descriptors/button.h + include/puara/descriptors/jab.h + include/puara/descriptors/roll.h + include/puara/descriptors/shake.h + include/puara/descriptors/tilt.h + include/puara/descriptors/touch.h + + include/puara/utils/calibration.h + include/puara/utils/circularbuffer.h + include/puara/utils/leakyintegrator.h + include/puara/utils/maprange.h + include/puara/utils/rollingminmax.h + include/puara/utils/smooth.h + include/puara/utils/threshold.h + include/puara/utils/wrap.h + + include/puara/structs.h + include/puara/gestures.h + include/puara/utils.h + + src/puara_gestures.cpp +) + +target_compile_definitions(puara_gestures PUBLIC $<$:_USE_MATH_DEFINES>) +target_include_directories(puara_gestures PUBLIC include 3rdparty) +target_compile_features(puara_gestures PUBLIC cxx_std_20) +target_link_libraries(puara_gestures PUBLIC Boost::headers) + +### Tests ### +if(PUARA_GESTURES_ENABLE_TESTING) + include(CTest) + +target_sources(Continuous + PRIVATE + tests/testing_touch.cpp + +) +add_executable(testing_roll tests/testing_roll.cpp) + add_executable(testing_tilt tests/testing_tilt.cpp) + add_executable(testing_touch tests/testing_touch.cpp) + target_compile_definitions(testing_roll PRIVATE "-DPUARA_TESTS_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/testing files\"") + target_compile_definitions(testing_tilt PRIVATE "-DPUARA_TESTS_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/testing files\"") + target_compile_definitions(testing_touch PRIVATE "-DPUARA_TESTS_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/testing files\"") + target_link_libraries(testing_roll PRIVATE puara_gestures) + target_link_libraries(testing_tilt PRIVATE puara_gestures) + target_link_libraries(testing_touch PRIVATE puara_gestures) + +endif() + +### Example ### +if(PUARA_GESTURES_ENABLE_STANDALONE) + add_subdirectory(standalone) +endif() diff --git a/descriptors/puara-gestures.code-workspace b/descriptors/puara-gestures.code-workspace deleted file mode 100644 index 93b8dcb..0000000 --- a/descriptors/puara-gestures.code-workspace +++ /dev/null @@ -1,87 +0,0 @@ -{ - "folders": [ - { - "path": ".." - } - ], - "settings": { - "files.associations": { - "__bit_reference": "cpp", - "__config": "cpp", - "__hash_table": "cpp", - "__locale": "cpp", - "__node_handle": "cpp", - "__split_buffer": "cpp", - "__threading_support": "cpp", - "__tree": "cpp", - "__verbose_abort": "cpp", - "any": "cpp", - "array": "cpp", - "bitset": "cpp", - "cctype": "cpp", - "cfenv": "cpp", - "charconv": "cpp", - "cinttypes": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "codecvt": "cpp", - "complex": "cpp", - "condition_variable": "cpp", - "csignal": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdint": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "deque": "cpp", - "execution": "cpp", - "forward_list": "cpp", - "fstream": "cpp", - "future": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "ios": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "list": "cpp", - "locale": "cpp", - "map": "cpp", - "mutex": "cpp", - "new": "cpp", - "optional": "cpp", - "ostream": "cpp", - "queue": "cpp", - "ratio": "cpp", - "regex": "cpp", - "semaphore": "cpp", - "set": "cpp", - "shared_mutex": "cpp", - "source_location": "cpp", - "span": "cpp", - "sstream": "cpp", - "stack": "cpp", - "stdexcept": "cpp", - "streambuf": "cpp", - "string": "cpp", - "string_view": "cpp", - "tuple": "cpp", - "typeindex": "cpp", - "typeinfo": "cpp", - "unordered_map": "cpp", - "unordered_set": "cpp", - "valarray": "cpp", - "variant": "cpp", - "vector": "cpp", - "algorithm": "cpp", - "thread": "cpp", - "*.ipp": "cpp", - "csetjmp": "cpp" - } - } -} \ No newline at end of file diff --git a/descriptors/puara-utils.h b/descriptors/puara-utils.h deleted file mode 100644 index 7735024..0000000 --- a/descriptors/puara-utils.h +++ /dev/null @@ -1,717 +0,0 @@ -//********************************************************************************// -// Puara Gestures - Utilities (.h) // -// https://github.com/Puara/puara-gestures // -// Société des Arts Technologiques (SAT) - https://sat.qc.ca // -// Input Devices and Music Interaction Laboratory (IDMIL) - https://www.idmil.org // -// Edu Meneses (2024) - https://www.edumeneses.com // -//********************************************************************************// - -#pragma once - -#include "puara-structs.h" - -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include - -namespace puara_gestures -{ - -namespace utils -{ - -/** - * @brief Simple leaky integrator implementation. - */ -class LeakyIntegrator -{ -public: - double current_value; - double old_value; - double leak; - int frequency; // leaking frequency (Hz) - unsigned long long timer; - - LeakyIntegrator( - double currentValue = 0, double oldValue = 0, double leakValue = 0.5, - int freq = 100, unsigned long long timerValue = 0) - : current_value(currentValue) - , old_value(oldValue) - , leak(leakValue) - , frequency(freq) - , timer(timerValue) - { - } - - /** - * @brief Call integrator - * - * @param reading new value to add into the integrator - * @param custom_leak between 0 and 1 - * @param time in microseconds - * @return double - */ - - double integrate( - double reading, double custom_old_value, double custom_leak, int custom_frequency, - unsigned long long& custom_timer) - { - auto currentTimePoint = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast( - currentTimePoint.time_since_epoch()); - unsigned long long current_time = duration.count(); - - if(custom_frequency <= 0) - { - current_value = reading + (custom_old_value * custom_leak); - } - else if((current_time / 1000LL) - (1000 / frequency) < custom_timer) - { - current_value = reading + old_value; - } - else - { - current_value = reading + (custom_old_value * custom_leak); - timer = (current_time / 1000LL); - } - old_value = current_value; - return current_value; - } - - double integrate(double reading, double leak, unsigned long long& time) - { - return LeakyIntegrator::integrate(reading, old_value, leak, frequency, time); - } - - double integrate(double reading, double custom_leak) - { - return LeakyIntegrator::integrate(reading, old_value, custom_leak, frequency, timer); - } - - double integrate(double reading) - { - return LeakyIntegrator::integrate(reading, old_value, leak, frequency, timer); - } -}; - -/** - * @brief Wrapper class undoes modular effects of angle measurements - * Ensures that instead of "wrapping" (e.g. moving from - PI to PI), - * measurements continue to decrease or increase - */ -class Unwrap -{ -public: - double prev_angle; - double accum; - double range; - bool empty; - - /** - * @brief Constructor for Unwrap class - * - * Initaliazes accumulated at 0 to indicate the data has not "wrapped" yet - * Initalizes empty at True - * @param Min minimum input value - * @param Max maximum input value - */ - Unwrap(double Min, double Max) - : accum(0) - , range(Max - Min) - , empty(true) - { - } - - /** - * @brief Calculates whether angle has "wrapped" based on previous angle - * and updates accordingly - */ - double unwrap(double reading) - { - double diff; - - // check if update hasn't been called before or the unwrap class - // has been cleared - if(empty) - { - diff = 0; - empty = false; - } - else - { - diff = reading - prev_angle; - } - prev_angle = reading; - if(diff > (range / 2)) - { - accum -= 1; - } - if(diff < (range * -1 / 2)) - { - accum += 1; - } - return reading + accum * range; - } - - /** - * @brief resets the unwrap object - */ - void clear() - { - accum = 0; - empty = true; - } -}; - -/** - * @brief Wraps a value around a given minimum and maximum. - * Algorithm from Jean-Michael Celerier. "Authoring interactive media : a logical - * & temporal approach." Computation and Language [cs.CL]. Université de Bordeaux, - * 2018. English. ffNNT : 2018BORD0037ff. fftel-01947309 - */ -class Wrap -{ -public: - double min; - double max; - - /** - * @brief Constructor for Wrap class - * - * @param Min minimum value of desired range for "wrapper" object - * @param Max maximum value of desired range for "wrapper" object - */ - Wrap(double Min, double Max) - : min(Min) - , max(Max) - { - } - - double wrap(double reading) - { - double diff = std::fabs(max - min); - if(min <= reading && reading <= max) - { - return reading; - } - else if(reading >= max) - { - return min + (reading - std::fmod(min, diff)); - } - return max - (min - std::fmod(reading, diff)); - } -}; - -/** - * @brief "Smoothing" algorithm takes the average of a given number of previous - * inputs. Can stabilize an erratic input stream. - */ -class Smooth -{ -public: - std::list list; - double size; - - /** - * @brief Constructor for Smooth class - * - * @param Size number of previous values that "smoother" object averages - */ - Smooth(double Size) - : list() - , size(Size) - { - } - - /** - * @brief Calls updateList then finds average of previous inputs - */ - double smooth(double reading) - { - updateList(reading); - double sumList = std::accumulate(list.begin(), list.end(), 0.0); - return (sumList / list.size()); - } - - /** - * @brief Updates list of previous inputs with current input - */ - void updateList(double reading) - { - list.push_front(reading); - // ensure list stays at desired size - while(list.size() > size) - { - list.pop_back(); - } - } - - /** - * @brief Clears list of all previous inputs - */ - void clear() { list.clear(); } -}; - -/** - * @brief Simple class to renge values according to min and max (in and out) - * established values. - */ -class MapRange -{ -public: - double current_in = 0; - double inMin = 0; - double inMax = 0; - double outMin = 0; - double outMax = 0; - double range(double in) - { - current_in = in; - if(outMin != outMax) - { - return (in - inMin) * (outMax - outMin) / (inMax - inMin) + outMin; - } - else - { - return in; - } - } - - float range(float in) - { - double casted_in = static_cast(in); - return static_cast(range(casted_in)); - } - - int range(int in) - { - double casted_in = static_cast(in); - return static_cast(range(casted_in)); - } -}; - -/** - * Simple circular buffer. - * This was created to ensure compatibility with older ESP SoCs - */ -class CircularBuffer -{ -public: - int size = 10; - std::deque buffer; - double add(double element) - { - buffer.push_front(element); - if(buffer.size() > size) - { - buffer.pop_back(); - } - return element; - } -}; - -/** - * Simple Threshold class to ensure a value doesn't exceed settable max and min values. - */ -class Threshold -{ -public: - double min; - double max; - double current; - - Threshold(double Min = -10.0, double Max = 10.0) - : min(Min) - , max(Max) - { - } - - double update(double reading) - { - current = reading; - if(reading < min) - { - return min; - } - if(reading > max) - { - return max; - } - return reading; - } -}; - -template -/** - * @brief Calculates the minimum and maximum values of the last N updates. - * The default N value is 10, modifiable during initialization. - * Ported from https://github.com/celtera/avendish/blob/56b89e52e367c67213be0c313d2ed3b9fb1aac19/examples/Ports/Puara/Jab.hpp#L15 - */ -class RollingMinMax -{ -public: - RollingMinMax(size_t buffer_size = 10) - : buf(buffer_size) - { - } - - puara_gestures::MinMax current_value; - puara_gestures::MinMax update(T value) - { - puara_gestures::MinMax ret{.min = value, .max = value}; - buf.push_back(value); - for(const T value : buf) - { - if(value < ret.min) - ret.min = value; - if(value > ret.max) - ret.max = value; - } - current_value = ret; - return ret; - } - -private: - boost::circular_buffer buf; -}; - -/** - * Calibrates the raw magnetometer values - */ -class Calibration -{ -public: - Imu9Axis myCalIMU; - std::vector rawMagData; - Eigen::MatrixXd softIronMatrix; - Eigen::VectorXd hardIronBias; - - Calibration() - : softIronMatrix(3, 3) - , hardIronBias(3) - { - softIronMatrix << 1, 1, 1, 1, 1, 1, 1, 1, 1; - - hardIronBias << 0, 0, 0; - } - - /** - * The gravitation field is used to calculate the soft iron matrices and should be modified according to the follownig formula: - * 1- Get the Total Field for your location from: - * https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml#igrfwmm - * 2- Convert the Total Field value to Gauss (1nT = 10E-5G) - * 3- Convert Total Field to Raw value Total Field, which is the - * Raw Gravitation Field we are searching for - * Read your magnetometer datasheet and find your gain value, - * Which should be the same of the collected raw points - * - * Reference: https://github.com/nliaudat/magnetometer_calibration/blob/main/calibrate.py - * - * - */ - int gravitationField - = 234; // should be 1000 by default, this was calculated for the LSM9DS1 in Montreal - - void applyMagnetometerCalibration(const Imu9Axis& myRawIMU) - { - myCalIMU.magn.x = softIronMatrix(0, 0) * (myRawIMU.magn.x - hardIronBias(0)) - + softIronMatrix(0, 1) * (myRawIMU.magn.y - hardIronBias(1)) - + softIronMatrix(0, 2) * (myRawIMU.magn.z - hardIronBias(2)); - - myCalIMU.magn.y = softIronMatrix(1, 0) * (myRawIMU.magn.x - hardIronBias(0)) - + softIronMatrix(1, 1) * (myRawIMU.magn.y - hardIronBias(1)) - + softIronMatrix(1, 2) * (myRawIMU.magn.z - hardIronBias(2)); - - myCalIMU.magn.z = softIronMatrix(2, 0) * (myRawIMU.magn.x - hardIronBias(0)) - + softIronMatrix(2, 1) * (myRawIMU.magn.y - hardIronBias(1)) - + softIronMatrix(2, 2) * (myRawIMU.magn.z - hardIronBias(2)); - } - - /** - * Records the raw magnetometer data and saves it in a vector. - * The user needs to call this a minimum amount of time in order to generate at least 1500 data points - */ - int recordRawMagData(const Coord3D& magData) - { - rawMagData.push_back(magData); - - return rawMagData.size(); - } - - /** - * Fits an ellipsoid to 3D points by creating matrices from the input, solving for eigenvalues, - * and returning the ellipsoid's shape matrix M, center vector n, and scalar offset d - */ - std::tuple - ellipsoid_fit(const Eigen::MatrixXd& s) - { - - // std::cout << "s " << std::endl << s << std::endl; - - Eigen::MatrixXd D(10, s.cols()); - - D.row(0) = s.row(0).array().square(); - D.row(1) = s.row(1).array().square(); - D.row(2) = s.row(2).array().square(); - D.row(3) = 2.0 * s.row(1).array() * s.row(2).array(); - D.row(4) = 2.0 * s.row(0).array() * s.row(2).array(); - D.row(5) = 2.0 * s.row(0).array() * s.row(1).array(); - D.row(6) = 2.0 * s.row(0).array(); - D.row(7) = 2.0 * s.row(1).array(); - D.row(8) = 2.0 * s.row(2).array(); - D.row(9) = Eigen::MatrixXd::Ones(1, 105); - - // std::cout << std::fixed << std::setprecision(30); - - // std::cout << std::scientific << std::setprecision(8) << "D " << std::endl << D << std::endl; - - Eigen::MatrixXd S = D * D.transpose(); - - Eigen::MatrixXd S_11 = S.block(0, 0, 6, 6); - Eigen::MatrixXd S_12 = S.block(0, 6, 6, S.cols() - 6); - Eigen::MatrixXd S_21 = S.block(6, 0, S.rows() - 6, 6); - Eigen::MatrixXd S_22 = S.block(6, 6, S.rows() - 6, S.cols() - 6); - - // std::cout << "S " << std::endl << S << std::endl; - - Eigen::MatrixXd C(6, 6); - - C << -1, 1, 1, 0, 0, 0, 1, -1, 1, 0, 0, 0, 1, 1, -1, 0, 0, 0, 0, 0, 0, -4, 0, 0, 0, - 0, 0, 0, -4, 0, 0, 0, 0, 0, 0, -4; - - Eigen::MatrixXd C_inv = C.inverse(); - - Eigen::MatrixXd E = C_inv * (S_11 - (S_12 * (S_22.inverse() * S_21))); - - Eigen::EigenSolver solver(E); - Eigen::VectorXd E_w = solver.eigenvalues().real(); - Eigen::MatrixXd E_v = solver.eigenvectors().real(); - - int maxIndex = 0; - E_w.maxCoeff(&maxIndex); - Eigen::VectorXd v_1 = E_v.col(maxIndex); - - if(v_1(0) < 0) - v_1 = -v_1; - - Eigen::VectorXd v_2 = (-S_22.inverse() * S_21) * v_1; - - Eigen::MatrixXd M(3, 3); - Eigen::VectorXd n(3); - double d = v_2[3]; - - M << v_1(0), v_1(5), v_1(4), v_1(5), v_1(1), v_1(3), v_1(4), v_1(3), v_1(2); - - n << v_2(0), v_2(1), v_2(2); - - return std::make_tuple(M, n, d); - } - - /** - * Generates magnetometer calibration matrices based on saved raw dataset by fitting an ellipsoid to a set of 3D coordinates, - * deriving the hard-iron bias and soft-iron matrix based on the pre-defined gravitational field. - */ - int generateMagnetometerMatrices(std::vector customRawMagData) - { - - if(customRawMagData.empty()) - { - return 0; - } - - Eigen::MatrixXd s(customRawMagData.size(), 3); - - for(int i = 0; i < customRawMagData.size(); ++i) - { - s(i, 0) = customRawMagData[i].x; - s(i, 1) = customRawMagData[i].y; - s(i, 2) = customRawMagData[i].z; - } - - Eigen::MatrixXd M; - Eigen::VectorXd n; - double d; - - std::tie(M, n, d) = ellipsoid_fit(s.transpose()); - - Eigen::MatrixXd M_1 = M.inverse(); - hardIronBias = -(M_1 * n); - - softIronMatrix - = (gravitationField / std::sqrt((n.transpose() * (M_1 * n) - d)) * M.sqrt()); - - return 1; - } - - void generateMagnetometerMatrices() { generateMagnetometerMatrices(rawMagData); } -}; - -/** - * @brief Simple function to get the current elapsed time in microseconds. - */ -inline unsigned long long getCurrentTimeMicroseconds() -{ - auto currentTimePoint = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast( - currentTimePoint.time_since_epoch()); - return duration.count(); -} - -/** - * @brief Function used to reduce feature arrays into single values. - * E.g., brush uses it to reduce multiBrush instances - */ -inline double arrayAverageZero(double* Array, int ArraySize) -{ - double sum = 0; - int count = 0; - double output = 0; - for(int i = 0; i < ArraySize; ++i) - { - if(Array[i] != 0) - { - sum += Array[i]; - count++; - } - } - if(count > 0) - { - output = sum / count; - } - return output; -} - -/** - * @brief Legacy function used to calculate 1D blob detection in older - * digital musical instruments - */ -inline void bitShiftArrayL(int* origArray, int* shiftedArray, int arraySize, int shift) -{ - for(int i = 0; i < arraySize; ++i) - { - shiftedArray[i] = origArray[i]; - } - for(int k = 0; k < shift; ++k) - { - for(int i = 0; i < arraySize; ++i) - { - if(i == (arraySize - 1)) - { - shiftedArray[i] = (shiftedArray[i] << 1); - } - else - { - shiftedArray[i] = (shiftedArray[i] << 1) | (shiftedArray[i + 1] >> 7); - } - } - } -} - -} - -namespace convert -{ - -/** - * @brief Convert g's to m/s^2 - * - */ -inline double g_to_ms2(double reading) -{ - return reading * 9.80665; -} - -/** - * @brief Convert m/s^2 to g's - * - */ -inline double ms2_to_g(double reading) -{ - return reading / 9.80665; -} - -/** - * @brief Convert DPS to radians per second - * - */ -inline double dps_to_rads(double reading) -{ - return reading * M_PI / 180; -} - -/** - * @brief Convert radians per second to DPS - * - */ -inline double rads_to_dps(double reading) -{ - return reading * 180 / M_PI; -} - -/** - * @brief Convert Gauss to uTesla - * - */ -inline double gauss_to_utesla(double reading) -{ - return reading / 10000; -} - -/** - * @brief Convert uTesla to Gauss - * - */ -inline double utesla_to_gauss(double reading) -{ - return reading * 10000; -} - -/** - * @brief Convert polar coordinates to cartesian - * - */ -Coord3D polar_to_cartesian(Spherical polarCoords) -{ - Coord3D cartesianCoords; - - cartesianCoords.x = polarCoords.r * cos(polarCoords.theta) * cos(polarCoords.phi); - cartesianCoords.y = polarCoords.r * cos(polarCoords.theta) * sin(polarCoords.phi); - cartesianCoords.z = polarCoords.r * sin(polarCoords.theta); - - return cartesianCoords; -} - -/** - * @brief Convert cartesian coordinates to polar - * - */ -Spherical cartesian_to_polar(Coord3D cartesianCoords) -{ - Spherical polarCoords; - - polarCoords.r = sqrt( - cartesianCoords.x * cartesianCoords.x + cartesianCoords.y * cartesianCoords.y - + cartesianCoords.z * cartesianCoords.z); - - polarCoords.theta = atan2( - cartesianCoords.z, sqrt( - cartesianCoords.x * cartesianCoords.x - + cartesianCoords.y * cartesianCoords.y)); - - polarCoords.phi = atan2(cartesianCoords.y, cartesianCoords.x); - - return polarCoords; -} - -} -} diff --git a/descriptors/puara-button.h b/include/puara/descriptors/button.h similarity index 75% rename from descriptors/puara-button.h rename to include/puara/descriptors/button.h index 7ad5c20..9f349b0 100644 --- a/descriptors/puara-button.h +++ b/include/puara/descriptors/button.h @@ -1,7 +1,7 @@ #pragma once -#include "puara-utils.h" +#include namespace puara_gestures { @@ -146,55 +146,5 @@ class button buttonCount = 0; } } - - // unsigned int getButtonCount() { - // return buttonCount; - // } - - // bool getButtonTouch() { - // return buttonPress; - // } - - // unsigned int getButtonValue() { - // return buttonValue; - // } - - // unsigned int getButtonTap() { - // return buttonTap; - // } - - // unsigned int getButtonDTap() { - // return buttonDtap; - // } - - // unsigned int getButtonTTap() { - // return buttonTtap; - // } - - // unsigned int getButtonThreshold() { - // return buttonValue; - // } - - // unsigned int setButtonThreshold(int value) { - // buttonThreshold = value; - // return 0; - // } - - // unsigned int getButtonPressTime() { - // return buttonPressTime; - // } - - // bool getButtonHold() { - // return buttonHold; - // } - - // unsigned int getButtonHoldInterval() { - // return buttonHoldInterval; - // } - - // unsigned int setButtonHoldInterval(int value) { - // buttonHoldInterval = value; - // return 0; - // } }; } diff --git a/descriptors/puara-jab.h b/include/puara/descriptors/jab.h similarity index 73% rename from descriptors/puara-jab.h rename to include/puara/descriptors/jab.h index 94e172b..160a561 100644 --- a/descriptors/puara-jab.h +++ b/include/puara/descriptors/jab.h @@ -7,11 +7,8 @@ //********************************************************************************// #pragma once -#include "puara-structs.h" -#include "puara-utils.h" - -#include -#include +#include +#include namespace puara_gestures { @@ -25,26 +22,34 @@ namespace puara_gestures class Jab { public: - Jab() - : tied_value(nullptr) - , threshold(5) + int threshold{}; + + Jab() noexcept + : threshold(5) + , tied_value(nullptr) , minmax(10) { } - Jab(double* tied) - : tied_value(tied) - , threshold(5) + + Jab(const Jab&) noexcept = default; + Jab(Jab&&) noexcept = default; + Jab& operator=(const Jab&) noexcept = default; + Jab& operator=(Jab&&) noexcept = default; + + explicit Jab(double* tied) + : threshold(5) + , tied_value(tied) , minmax(10) { } - Jab(Coord1D* tied) - : tied_value(&(tied->x)) - , threshold(5) + + explicit Jab(Coord1D* tied) + : threshold(5) + , tied_value(&(tied->x)) , minmax(10) { } - int threshold; double update(double reading) { minmax.update(reading); @@ -88,7 +93,7 @@ class Jab } } - double current_value() { return value; } + double current_value() const { return value; } int tie(Coord1D* new_tie) { @@ -97,9 +102,9 @@ class Jab } private: + double* tied_value{}; double value = 0; puara_gestures::utils::RollingMinMax minmax; - double* tied_value; }; /** @@ -111,18 +116,20 @@ class Jab class Jab2D { public: - Jab2D() - : x() - , y() - { - } - Jab2D(Coord2D* tied) + Jab x{}, y{}; + + Jab2D() noexcept = default; + Jab2D(const Jab2D&) noexcept = default; + Jab2D(Jab2D&&) noexcept = default; + Jab2D& operator=(const Jab2D&) noexcept = default; + Jab2D& operator=(Jab2D&&) noexcept = default; + + explicit Jab2D(Coord2D* tied) noexcept : x(&(tied->x)) , y(&(tied->y)) { } - Jab x, y; int update(double readingX, double readingY) { x.update(readingX); @@ -145,7 +152,7 @@ class Jab2D } double frequency(double freq); - Coord2D current_value() + Coord2D current_value() const { Coord2D answer; answer.x = x.current_value(); @@ -157,15 +164,21 @@ class Jab2D class Jab3D { public: - Jab3D() { } - Jab3D(Coord3D* tied) + Jab x{}, y{}, z{}; + + Jab3D() = default; + Jab3D(const Jab3D&) noexcept = default; + Jab3D(Jab3D&&) noexcept = default; + Jab3D& operator=(const Jab3D&) noexcept = default; + Jab3D& operator=(Jab3D&&) noexcept = default; + + explicit Jab3D(Coord3D* tied) : x(&(tied->x)) , y(&(tied->y)) , z(&(tied->z)) { } - Jab x, y, z; int update(double readingX, double readingY, double readingZ) { x.update(readingX); @@ -191,7 +204,7 @@ class Jab3D } double frequency(double freq); - Coord3D current_value() + Coord3D current_value() const { Coord3D answer; answer.x = x.current_value(); diff --git a/descriptors/puara-roll.h b/include/puara/descriptors/roll.h similarity index 94% rename from descriptors/puara-roll.h rename to include/puara/descriptors/roll.h index 71f9346..fb9aa03 100644 --- a/descriptors/puara-roll.h +++ b/include/puara/descriptors/roll.h @@ -10,14 +10,11 @@ #pragma once #include "IMU_Sensor_Fusion/imu_orientation.h" -#include "puara-structs.h" -#include "puara-utils.h" +#include +#include #include -#include -#include - namespace puara_gestures { @@ -44,7 +41,7 @@ class Roll Roll() : unwrapper(-M_PI, M_PI) , smoother(50) - , wrapper(0, 2 * M_PI) + , wrapper{0, 2 * M_PI} { } @@ -53,10 +50,10 @@ class Roll * * @param smoothValue number of previous values that "smoother" object averages */ - Roll(double smoothValue) + explicit Roll(double smoothValue) : unwrapper(-M_PI, M_PI) , smoother(smoothValue) - , wrapper(0, 2 * M_PI) + , wrapper{0, 2 * M_PI} { } @@ -69,7 +66,7 @@ class Roll Roll(double wrapMin, double wrapMax) : unwrapper(-M_PI, M_PI) , smoother(50) - , wrapper(wrapMin, wrapMax) + , wrapper{wrapMin, wrapMax} { } @@ -83,7 +80,7 @@ class Roll Roll(double smoothValue, double wrapMin, double wrapMax) : unwrapper(-M_PI, M_PI) , smoother(smoothValue) - , wrapper(wrapMin, wrapMax) + , wrapper{wrapMin, wrapMax} { } diff --git a/descriptors/puara-shake.h b/include/puara/descriptors/shake.h similarity index 81% rename from descriptors/puara-shake.h rename to include/puara/descriptors/shake.h index 881806c..7ad38db 100644 --- a/descriptors/puara-shake.h +++ b/include/puara/descriptors/shake.h @@ -8,10 +8,8 @@ #pragma once -#include "puara-structs.h" -#include "puara-utils.h" - -#include +#include +#include namespace puara_gestures { @@ -28,25 +26,23 @@ namespace puara_gestures class Shake { public: + utils::LeakyIntegrator integrator{0, 0, 0.6, 10, 0}; + double fast_leak = 0.6; + double slow_leak = 0.3; + Shake() : tied_value(nullptr) - , integrator(0, 0, 0.6, 10, 0) { } - Shake(double* tied) + explicit Shake(double* tied) : tied_value(tied) - , integrator(0, 0, 0.6, 10, 0) { } - Shake(Coord1D* tied) + explicit Shake(Coord1D* tied) : tied_value(&(tied->x)) - , integrator(0, 0, 0.6, 10, 0) { } - utils::LeakyIntegrator integrator; - double fast_leak = 0.6; - double slow_leak = 0.3; double update(double reading) { if(tied_value != nullptr) @@ -90,7 +86,7 @@ class Shake } } - double frequency() { return integrator.frequency; } + double frequency() const { return integrator.frequency; } double frequency(double freq) { @@ -98,7 +94,7 @@ class Shake return freq; } - double current_value() { return integrator.current_value; } + double current_value() const { return integrator.current_value; } int tie(Coord1D* new_tie) { @@ -107,7 +103,7 @@ class Shake } private: - double* tied_value; + double* tied_value{}; }; /** @@ -123,7 +119,12 @@ class Shake class Shake2D { public: - Shake2D() { } + Shake2D() noexcept = default; + Shake2D(const Shake2D&) noexcept = default; + Shake2D(Shake2D&&) noexcept = default; + Shake2D& operator=(const Shake2D&) noexcept = default; + Shake2D& operator=(Shake2D&&) noexcept = default; + Shake2D(Coord2D* tied) : x(&(tied->x)) , y(&(tied->y)) @@ -160,7 +161,7 @@ class Shake2D return freq; } - Coord2D current_value() + Coord2D current_value() const { Coord2D answer; answer.x = x.current_value(); @@ -182,15 +183,21 @@ class Shake2D class Shake3D { public: - Shake3D() { } - Shake3D(Coord3D* tied) + Shake x, y, z; + + Shake3D() noexcept = default; + Shake3D(const Shake3D&) noexcept = default; + Shake3D(Shake3D&&) noexcept = default; + Shake3D& operator=(const Shake3D&) noexcept = default; + Shake3D& operator=(Shake3D&&) noexcept = default; + + explicit Shake3D(Coord3D* tied) : x(&(tied->x)) , y(&(tied->y)) , z(&(tied->z)) { } - Shake x, y, z; int update(double readingX, double readingY, double readingZ) { x.update(readingX); @@ -222,7 +229,8 @@ class Shake3D z.frequency(freq); return freq; } - Coord3D current_value() + + Coord3D current_value() const { Coord3D answer; answer.x = x.current_value(); diff --git a/descriptors/puara-tilt.h b/include/puara/descriptors/tilt.h similarity index 67% rename from descriptors/puara-tilt.h rename to include/puara/descriptors/tilt.h index dfd70b8..0529058 100644 --- a/descriptors/puara-tilt.h +++ b/include/puara/descriptors/tilt.h @@ -10,12 +10,10 @@ #pragma once #include "IMU_Sensor_Fusion/imu_orientation.h" -#include "puara-structs.h" -#include "puara-utils.h" +#include +#include #include -#include -#include namespace puara_gestures { @@ -24,31 +22,12 @@ namespace puara_gestures * @brief This class measures tilt gestures using 3DoF info from an accelerometer, * gyroscope, and magnetometer */ -class Tilt +class Tilt : public utils::Smooth { public: IMU_Orientation orientation; - utils::Smooth smoother; - /** - * @brief Default constructor for Tilt - * - * @return Sets "smoother" object to average the previous 50 objects - */ - Tilt() - : smoother(50) - { - } - - /** - * @brief Constructor for Tilt - * - * @param smoothValue number of previous values that "smoother" object averages - */ - explicit Tilt(double smoothValue) - : smoother(smoothValue) - { - } + using utils::Smooth::Smooth; /** * @brief Calculates tilt (aka "pitch") measurement @@ -69,16 +48,9 @@ class Tilt return orientation.euler.tilt; } - /** - * @brief Option to "smooth" value stream by taking the average of a given - * number of previous values - * Set to 50 in default Tilt constructor - */ - double smooth(double reading) { return smoother.smooth(reading); } - /** * @brief Clears list of all previous inputs */ - void clear_smooth() { smoother.clear(); } + void clear_smooth() { clear(); } }; } diff --git a/descriptors/puara-touch.h b/include/puara/descriptors/touch.h similarity index 58% rename from descriptors/puara-touch.h rename to include/puara/descriptors/touch.h index b3d03fe..912fabe 100644 --- a/descriptors/puara-touch.h +++ b/include/puara/descriptors/touch.h @@ -1,8 +1,9 @@ #pragma once -#include +#include +#include -#include +#include namespace puara_gestures { @@ -10,37 +11,38 @@ namespace puara_gestures class Touch { public: - float touchAll = 0; // f, 0--1 - float touchTop = 0; // f, 0--1 - float touchMiddle = 0; // f, 0--1 - float touchBottom = 0; // f, 0--1 - float brush = 0; // f, 0--? (~cm/s) - float multiBrush[4]; // ffff, 0--? (~cm/s) - float rub; // f, 0--? (~cm/s) - float multiRub[4]; // ffff, 0--? (~cm/s) + float touchAll = 0.0f; // f, 0--1 + float touchTop = 0.0f; // f, 0--1 + float touchMiddle = 0.0f; // f, 0--1 + float touchBottom = 0.0f; // f, 0--1 + float brush = 0.0f; // f, 0--? (~cm/s) + double multiBrush[4]{}; // ffff, 0--? (~cm/s) + float rub{}; // f, 0--? (~cm/s) + double multiRub[4]{}; // ffff, 0--? (~cm/s) // touch array int touchSizeEdge = 4; // amount of touch stripes for top and bottom portions (arbitrary) - float touchAverage(float* touchArrayStrips, int firstStrip, int lastStrip); - float touchAverage(int* touchArrayStrips, int firstStrip, int lastStrip); - int lastState_blobPos[4]; - int maxBlobs = 4; // max amount of blobs to be detected - int blobAmount; // amount of detected blobs - int blobCenter[4]; // shows the "center" (index) of each blob (former blobArray) - int blobPos[4]; // starting position (index) of each blob - float blobSize[4]; // "size" (amount of stripes) of each blob - void blobDetection1D(int* discrete_touch, int touchSize); - const int leakyBrushFreq = 100; // leaking frequency (Hz) - unsigned long leakyBrushTimer = 0; - const int leakyRubFreq = 100; - unsigned long leakyRubTimer = 0; - int brushCounter[4]; - float arrayAverageZero(float* Array, int ArraySize); - void bitShiftArrayL(int* origArray, int* shiftedArray, int arraySize, int shift); - - float leakyIntegrator( - float reading, float old_value, float leak, int frequency, unsigned long& timer); + int lastState_blobPos[4]{}; + int maxBlobs = 4; // max amount of blobs to be detected + int blobAmount{}; // amount of detected blobs + int blobCenter[4]{}; // shows the "center" (index) of each blob (former blobArray) + int blobPos[4]{}; // starting position (index) of each blob + float blobSize[4]{}; // "size" (amount of stripes) of each blob + int brushCounter[4]{}; + + // Arrays of LeakyIntegrator instances + utils::LeakyIntegrator multiBrushIntegrator[4]; + utils::LeakyIntegrator multiRubIntegrator[4]; + + Touch() + { + for(int i = 0; i < 4; ++i) + { + multiBrushIntegrator[i] = utils::LeakyIntegrator(0.0f, 0.0f, 0.7f, 100, 0); + multiRubIntegrator[i] = utils::LeakyIntegrator(0.0f, 0.0f, 0.7f, 100, 0); + } + } /* Expects an array of discrete touch values (int, 0 or 1) and * the size of the array @@ -100,28 +102,40 @@ class Touch } else { - multiBrush[i] = leakyIntegrator( - movement * 0.15, multiBrush[i], 0.7, leakyBrushFreq, leakyBrushTimer); - multiRub[i] = leakyIntegrator( - abs(movement * 0.15), multiRub[i], 0.7, leakyRubFreq, leakyRubTimer); + // multiBrush[i] = multiBrushIntegrator[i].integrate( + // movement * 0.15, multiBrush[i], 0.7, leakyBrushFreq, leakyBrushTimer); + + // multiRub[i] = multiRubIntegrator[i].integrate( + // (std::abs(movement * 0.15)), multiRub[i], 0.7, leakyRubFreq, + // leakyRubTimer); + // + multiBrush[i] = multiBrushIntegrator[i].integrate(movement * 0.15); + multiRub[i] = multiRubIntegrator[i].integrate(std::abs(movement * 0.15)); } } - else if(abs(movement) > 1) + else if(std::abs(movement) > 1) { - multiBrush[i] - = leakyIntegrator(0, multiBrush[i], 0.6, leakyBrushFreq, leakyBrushTimer); + // multiBrush[i] = multiBrushIntegrator[i].integrate( + // 0, multiBrush[i], 0.6, leakyBrushFreq, leakyBrushTimer); + + multiBrush[i] = multiBrushIntegrator[i].integrate(0); } else { - multiBrush[i] = leakyIntegrator( - movement * 0.15, multiBrush[i], 0.8, leakyBrushFreq, leakyBrushTimer); - multiRub[i] = leakyIntegrator( - abs(movement) * 0.15, multiRub[i], 0.99, leakyRubFreq, leakyRubTimer); + // multiBrush[i] = multiBrushIntegrator[i].integrate( + // movement * 0.15, multiBrush[i], 0.8, leakyBrushFreq, leakyBrushTimer); + // multiRub[i] = multiRubIntegrator[i].integrate( + // (std::abs(movement * 0.15)) * 0.15, multiRub[i], 0.99, leakyRubFreq, + // leakyRubTimer); + + multiBrush[i] = multiBrushIntegrator[i].integrate(movement * 0.15); + multiRub[i] = multiRubIntegrator[i].integrate((std::abs(movement * 0.15))); + brushCounter[i] = 0; } } - brush = arrayAverageZero(multiBrush, 4); - rub = arrayAverageZero(multiRub, 4); + brush = utils::arrayAverageZero(multiBrush, 4); + rub = utils::arrayAverageZero(multiRub, 4); } float touchAverage(float* touchArrayStrips, int firstStrip, int lastStrip) @@ -142,6 +156,7 @@ class Touch return ((float)sum) / (lastStrip - firstStrip); } + //TODO: move to utils void blobDetection1D(int* discrete_touch, int touchSize) { blobAmount = 0; diff --git a/puara_gestures.h b/include/puara/gestures.h similarity index 60% rename from puara_gestures.h rename to include/puara/gestures.h index dd347a7..393e0d3 100644 --- a/puara_gestures.h +++ b/include/puara/gestures.h @@ -8,18 +8,10 @@ #pragma once -#include - -#include -#include -#include - -// puara-gesture descriptors -#include "descriptors/IMU_Sensor_Fusion/imu_orientation.h" -#include "descriptors/puara-button.h" -#include "descriptors/puara-jab.h" -#include "descriptors/puara-roll.h" -#include "descriptors/puara-shake.h" -#include "descriptors/puara-tilt.h" -#include "testing_roll.h" -#include "testing_tilt.h" +#include +#include +#include +#include +#include +#include +#include diff --git a/descriptors/puara-structs.h b/include/puara/structs.h similarity index 100% rename from descriptors/puara-structs.h rename to include/puara/structs.h diff --git a/include/puara/utils.h b/include/puara/utils.h new file mode 100644 index 0000000..d3f819b --- /dev/null +++ b/include/puara/utils.h @@ -0,0 +1,183 @@ +//********************************************************************************// +// Puara Gestures - Utilities (.h) // +// https://github.com/Puara/puara-gestures // +// Société des Arts Technologiques (SAT) - https://sat.qc.ca // +// Input Devices and Music Interaction Laboratory (IDMIL) - https://www.idmil.org // +// Edu Meneses (2024) - https://www.edumeneses.com // +//********************************************************************************// + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace puara_gestures::utils +{ + +/** + * @brief Simple function to get the current elapsed time in microseconds. + */ +inline unsigned long long getCurrentTimeMicroseconds() +{ + auto currentTimePoint = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast( + currentTimePoint.time_since_epoch()); + return duration.count(); +} + +/** + * @brief Function used to reduce feature arrays into single values. + * E.g., brush uses it to reduce multiBrush instances + */ +inline double arrayAverageZero(double* Array, int ArraySize) +{ + double sum = 0; + int count = 0; + double output = 0; + for(int i = 0; i < ArraySize; ++i) + { + if(Array[i] != 0) + { + sum += Array[i]; + count++; + } + } + if(count > 0) + { + output = sum / count; + } + return output; +} + +/** + * @brief Legacy function used to calculate 1D blob detection in older + * digital musical instruments + */ +inline void bitShiftArrayL(int* origArray, int* shiftedArray, int arraySize, int shift) +{ + for(int i = 0; i < arraySize; ++i) + { + shiftedArray[i] = origArray[i]; + } + for(int k = 0; k < shift; ++k) + { + for(int i = 0; i < arraySize; ++i) + { + if(i == (arraySize - 1)) + { + shiftedArray[i] = (shiftedArray[i] << 1); + } + else + { + shiftedArray[i] = (shiftedArray[i] << 1) | (shiftedArray[i + 1] >> 7); + } + } + } +} + +namespace convert +{ + +/** + * @brief Convert g's to m/s^2 + * + */ +inline double g_to_ms2(double reading) +{ + return reading * 9.80665; +} + +/** + * @brief Convert m/s^2 to g's + * + */ +inline double ms2_to_g(double reading) +{ + return reading / 9.80665; +} + +/** + * @brief Convert DPS to radians per second + * + */ +inline double dps_to_rads(double reading) +{ + return reading * M_PI / 180; +} + +/** + * @brief Convert radians per second to DPS + * + */ +inline double rads_to_dps(double reading) +{ + return reading * 180 / M_PI; +} + +/** + * @brief Convert Gauss to uTesla + * + */ +inline double gauss_to_utesla(double reading) +{ + return reading / 10000; +} + +/** + * @brief Convert uTesla to Gauss + * + */ +inline double utesla_to_gauss(double reading) +{ + return reading * 10000; +} + +/** + * @brief Convert polar coordinates to cartesian + * + */ +inline Coord3D polar_to_cartesian(Spherical polarCoords) +{ + Coord3D cartesianCoords; + + cartesianCoords.x = polarCoords.r * cos(polarCoords.theta) * cos(polarCoords.phi); + cartesianCoords.y = polarCoords.r * cos(polarCoords.theta) * sin(polarCoords.phi); + cartesianCoords.z = polarCoords.r * sin(polarCoords.theta); + + return cartesianCoords; +} + +/** + * @brief Convert cartesian coordinates to polar + * + */ +inline Spherical cartesian_to_polar(Coord3D cartesianCoords) +{ + Spherical polarCoords; + + polarCoords.r = sqrt( + cartesianCoords.x * cartesianCoords.x + cartesianCoords.y * cartesianCoords.y + + cartesianCoords.z * cartesianCoords.z); + + polarCoords.theta = atan2( + cartesianCoords.z, sqrt( + cartesianCoords.x * cartesianCoords.x + + cartesianCoords.y * cartesianCoords.y)); + + polarCoords.phi = atan2(cartesianCoords.y, cartesianCoords.x); + + return polarCoords; +} + +} +} diff --git a/include/puara/utils/calibration.h b/include/puara/utils/calibration.h new file mode 100644 index 0000000..ed7062c --- /dev/null +++ b/include/puara/utils/calibration.h @@ -0,0 +1,192 @@ +//********************************************************************************// +// Puara Gestures - Calibration (.h) // +// https://github.com/Puara/puara-gestures // +// Société des Arts Technologiques (SAT) - https://sat.qc.ca // +// Input Devices and Music Interaction Laboratory (IDMIL) - https://www.idmil.org // +// Edu Meneses (2024) - https://www.edumeneses.com // +// Rochana Fardon (2024) // +//********************************************************************************// + +#pragma once + +#include + +#include +#include +#include + +namespace puara_gestures::utils +{ + +/** + * Calibrates the raw magnetometer values + */ +class Calibration +{ +public: + Imu9Axis myCalIMU; + std::vector rawMagData; + Eigen::MatrixXd softIronMatrix; + Eigen::VectorXd hardIronBias; + + Calibration() + : softIronMatrix(3, 3) + , hardIronBias(3) + { + softIronMatrix << 1, 1, 1, 1, 1, 1, 1, 1, 1; + + hardIronBias << 0, 0, 0; + } + + /** + * The gravitation field is used to calculate the soft iron matrices and should be modified according to the follownig formula: + * 1- Get the Total Field for your location from: + * https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml#igrfwmm + * 2- Convert the Total Field value to Gauss (1nT = 10E-5G) + * 3- Convert Total Field to Raw value Total Field, which is the + * Raw Gravitation Field we are searching for + * Read your magnetometer datasheet and find your gain value, + * Which should be the same of the collected raw points + * + * Reference: https://github.com/nliaudat/magnetometer_calibration/blob/main/calibrate.py + * + * + */ + int gravitationField + = 234; // should be 1000 by default, this was calculated for the LSM9DS1 in Montreal + + void applyMagnetometerCalibration(const Imu9Axis& myRawIMU) + { + myCalIMU.magn.x = softIronMatrix(0, 0) * (myRawIMU.magn.x - hardIronBias(0)) + + softIronMatrix(0, 1) * (myRawIMU.magn.y - hardIronBias(1)) + + softIronMatrix(0, 2) * (myRawIMU.magn.z - hardIronBias(2)); + + myCalIMU.magn.y = softIronMatrix(1, 0) * (myRawIMU.magn.x - hardIronBias(0)) + + softIronMatrix(1, 1) * (myRawIMU.magn.y - hardIronBias(1)) + + softIronMatrix(1, 2) * (myRawIMU.magn.z - hardIronBias(2)); + + myCalIMU.magn.z = softIronMatrix(2, 0) * (myRawIMU.magn.x - hardIronBias(0)) + + softIronMatrix(2, 1) * (myRawIMU.magn.y - hardIronBias(1)) + + softIronMatrix(2, 2) * (myRawIMU.magn.z - hardIronBias(2)); + } + + /** + * Records the raw magnetometer data and saves it in a vector. + * The user needs to call this a minimum amount of time in order to generate at least 1500 data points + */ + int recordRawMagData(const Coord3D& magData) + { + rawMagData.push_back(magData); + + return rawMagData.size(); + } + + /** + * Fits an ellipsoid to 3D points by creating matrices from the input, solving for eigenvalues, + * and returning the ellipsoid's shape matrix M, center vector n, and scalar offset d + */ + std::tuple + ellipsoid_fit(const Eigen::MatrixXd& s) + { + + // std::cout << "s " << std::endl << s << std::endl; + + Eigen::MatrixXd D(10, s.cols()); + + D.row(0) = s.row(0).array().square(); + D.row(1) = s.row(1).array().square(); + D.row(2) = s.row(2).array().square(); + D.row(3) = 2.0 * s.row(1).array() * s.row(2).array(); + D.row(4) = 2.0 * s.row(0).array() * s.row(2).array(); + D.row(5) = 2.0 * s.row(0).array() * s.row(1).array(); + D.row(6) = 2.0 * s.row(0).array(); + D.row(7) = 2.0 * s.row(1).array(); + D.row(8) = 2.0 * s.row(2).array(); + D.row(9) = Eigen::MatrixXd::Ones(1, 105); + + // std::cout << std::fixed << std::setprecision(30); + + // std::cout << std::scientific << std::setprecision(8) << "D " << std::endl << D << std::endl; + + Eigen::MatrixXd S = D * D.transpose(); + + Eigen::MatrixXd S_11 = S.block(0, 0, 6, 6); + Eigen::MatrixXd S_12 = S.block(0, 6, 6, S.cols() - 6); + Eigen::MatrixXd S_21 = S.block(6, 0, S.rows() - 6, 6); + Eigen::MatrixXd S_22 = S.block(6, 6, S.rows() - 6, S.cols() - 6); + + // std::cout << "S " << std::endl << S << std::endl; + + Eigen::MatrixXd C(6, 6); + + C << -1, 1, 1, 0, 0, 0, 1, -1, 1, 0, 0, 0, 1, 1, -1, 0, 0, 0, 0, 0, 0, -4, 0, 0, 0, + 0, 0, 0, -4, 0, 0, 0, 0, 0, 0, -4; + + Eigen::MatrixXd C_inv = C.inverse(); + + Eigen::MatrixXd E = C_inv * (S_11 - (S_12 * (S_22.inverse() * S_21))); + + Eigen::EigenSolver solver(E); + Eigen::VectorXd E_w = solver.eigenvalues().real(); + Eigen::MatrixXd E_v = solver.eigenvectors().real(); + + int maxIndex = 0; + E_w.maxCoeff(&maxIndex); + Eigen::VectorXd v_1 = E_v.col(maxIndex); + + if(v_1(0) < 0) + v_1 = -v_1; + + Eigen::VectorXd v_2 = (-S_22.inverse() * S_21) * v_1; + + Eigen::MatrixXd M(3, 3); + Eigen::VectorXd n(3); + double d = v_2[3]; + + M << v_1(0), v_1(5), v_1(4), v_1(5), v_1(1), v_1(3), v_1(4), v_1(3), v_1(2); + + n << v_2(0), v_2(1), v_2(2); + + return std::make_tuple(M, n, d); + } + + /** + * Generates magnetometer calibration matrices based on saved raw dataset by fitting an ellipsoid to a set of 3D coordinates, + * deriving the hard-iron bias and soft-iron matrix based on the pre-defined gravitational field. + */ + int generateMagnetometerMatrices(std::vector customRawMagData) + { + + if(customRawMagData.empty()) + { + return 0; + } + + Eigen::MatrixXd s(customRawMagData.size(), 3); + + for(int i = 0; i < customRawMagData.size(); ++i) + { + s(i, 0) = customRawMagData[i].x; + s(i, 1) = customRawMagData[i].y; + s(i, 2) = customRawMagData[i].z; + } + + Eigen::MatrixXd M; + Eigen::VectorXd n; + double d; + + std::tie(M, n, d) = ellipsoid_fit(s.transpose()); + + Eigen::MatrixXd M_1 = M.inverse(); + hardIronBias = -(M_1 * n); + + softIronMatrix + = (gravitationField / std::sqrt((n.transpose() * (M_1 * n) - d)) * M.sqrt()); + + return 1; + } + + void generateMagnetometerMatrices() { generateMagnetometerMatrices(rawMagData); } +}; + +} diff --git a/include/puara/utils/circularbuffer.h b/include/puara/utils/circularbuffer.h new file mode 100644 index 0000000..dbb1307 --- /dev/null +++ b/include/puara/utils/circularbuffer.h @@ -0,0 +1,38 @@ +//********************************************************************************// +// Puara Gestures - Utilities (.h) // +// https://github.com/Puara/puara-gestures // +// Société des Arts Technologiques (SAT) - https://sat.qc.ca // +// Input Devices and Music Interaction Laboratory (IDMIL) - https://www.idmil.org // +// Edu Meneses (2024) - https://www.edumeneses.com // +//********************************************************************************// + +#pragma once + +#include + +#include + +namespace puara_gestures::utils +{ +/** + * Simple circular buffer. + * This was created to ensure compatibility with older ESP SoCs + */ +class CircularBuffer +{ +public: + std::size_t size = 10; + std::deque buffer; + + double add(double element) + { + buffer.push_front(element); + if(buffer.size() > size) + { + buffer.pop_back(); + } + return element; + } +}; + +} diff --git a/include/puara/utils/leakyintegrator.h b/include/puara/utils/leakyintegrator.h new file mode 100644 index 0000000..ed8e38b --- /dev/null +++ b/include/puara/utils/leakyintegrator.h @@ -0,0 +1,90 @@ +//********************************************************************************// +// Puara Gestures - Utilities (.h) // +// https://github.com/Puara/puara-gestures // +// Société des Arts Technologiques (SAT) - https://sat.qc.ca // +// Input Devices and Music Interaction Laboratory (IDMIL) - https://www.idmil.org // +// Edu Meneses (2024) - https://www.edumeneses.com // +//********************************************************************************// + +#pragma once + +#include + +namespace puara_gestures::utils +{ +/** + * @brief Simple leaky integrator implementation. + */ +class LeakyIntegrator +{ +public: + double current_value{}; + double old_value{}; + double leak{}; + int frequency{}; // leaking frequency (Hz) + unsigned long long timer{}; + long long getCurrentTimeMicroseconds(); + + explicit LeakyIntegrator( + double currentValue = 0, double oldValue = 0, double leakValue = 0.5, + int freq = 100, unsigned long long timerValue = 0) + : current_value(currentValue) + , old_value(oldValue) + , leak(leakValue) + , frequency(freq) + , timer(timerValue) + { + } + + /** + * @brief Call integrator + * + * @param reading new value to add into the integrator + * @param custom_leak between 0 and 1 + * @param time in microseconds + * @return double + */ + + double integrate( + double reading, double oldValue, double leakValue, int freq, + unsigned long long& timerValue) + { + auto currentTimePoint = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast( + currentTimePoint.time_since_epoch()); + unsigned long long current_time = duration.count(); + + if(freq <= 0) + { + current_value = reading + (oldValue * leakValue); + } + else if((current_time / 1000LL) - (1000 / frequency) < timerValue) + { + current_value = reading + old_value; + } + else + { + current_value = reading + (oldValue * leakValue); + timer = (current_time / 1000LL); + } + old_value = current_value; + return current_value; + } + + double integrate(double reading, double leakValue, unsigned long long& timerValue) + { + return this->integrate(reading, old_value, leakValue, frequency, timerValue); + } + + double integrate(double reading, double leakValue) + { + return this->integrate(reading, old_value, leakValue, frequency, timer); + } + + double integrate(double reading) + { + return this->integrate(reading, old_value, leak, frequency, timer); + } +}; + +} diff --git a/include/puara/utils/maprange.h b/include/puara/utils/maprange.h new file mode 100644 index 0000000..c8fae09 --- /dev/null +++ b/include/puara/utils/maprange.h @@ -0,0 +1,55 @@ +//********************************************************************************// +// Puara Gestures - Utilities (.h) // +// https://github.com/Puara/puara-gestures // +// Société des Arts Technologiques (SAT) - https://sat.qc.ca // +// Input Devices and Music Interaction Laboratory (IDMIL) - https://www.idmil.org // +// Edu Meneses (2024) - https://www.edumeneses.com // +//********************************************************************************// + +#pragma once + +#include + +namespace puara_gestures::utils +{ + +/** + * @brief Simple class to renge values according to min and max (in and out) + * established values. + */ +class MapRange +{ +public: + double current_in = 0; + double inMin = 0; + double inMax = 0; + double outMin = 0; + double outMax = 0; + + double range(double in) + { + current_in = in; + if(outMin != outMax) + { + return (in - inMin) * (outMax - outMin) / (inMax - inMin) + outMin; + } + else + { + return in; + } + } + + float range(float in) + { + double casted_in = static_cast(in); + return static_cast(range(casted_in)); + } + + int range(int in) + { + double casted_in = static_cast(in); + return static_cast(range(casted_in)); + } +}; + +} diff --git a/include/puara/utils/rollingminmax.h b/include/puara/utils/rollingminmax.h new file mode 100644 index 0000000..a9ab045 --- /dev/null +++ b/include/puara/utils/rollingminmax.h @@ -0,0 +1,52 @@ +//********************************************************************************// +// Puara Gestures - Utilities (.h) // +// https://github.com/Puara/puara-gestures // +// Société des Arts Technologiques (SAT) - https://sat.qc.ca // +// Input Devices and Music Interaction Laboratory (IDMIL) - https://www.idmil.org // +// Edu Meneses (2024) - https://www.edumeneses.com // +//********************************************************************************// + +#pragma once + +#include + +#include + +namespace puara_gestures::utils +{ + +template +/** + * @brief Calculates the minimum and maximum values of the last N updates. + * The default N value is 10, modifiable during initialization. + * Ported from https://github.com/celtera/avendish/blob/56b89e52e367c67213be0c313d2ed3b9fb1aac19/examples/Ports/Puara/Jab.hpp#L15 + */ +class RollingMinMax +{ +public: + RollingMinMax(size_t buffer_size = 10) + : buf(buffer_size) + { + } + + puara_gestures::MinMax current_value; + puara_gestures::MinMax update(T value) + { + puara_gestures::MinMax ret{.min = value, .max = value}; + buf.push_back(value); + for(const T value : buf) + { + if(value < ret.min) + ret.min = value; + if(value > ret.max) + ret.max = value; + } + current_value = ret; + return ret; + } + +private: + boost::circular_buffer buf; +}; + +} diff --git a/include/puara/utils/smooth.h b/include/puara/utils/smooth.h new file mode 100644 index 0000000..1370c38 --- /dev/null +++ b/include/puara/utils/smooth.h @@ -0,0 +1,67 @@ +//********************************************************************************// +// Puara Gestures - Utilities (.h) // +// https://github.com/Puara/puara-gestures // +// Société des Arts Technologiques (SAT) - https://sat.qc.ca // +// Input Devices and Music Interaction Laboratory (IDMIL) - https://www.idmil.org // +// Edu Meneses (2024) - https://www.edumeneses.com // +//********************************************************************************// + +#pragma once + +#include + +#include +#include + +namespace puara_gestures::utils +{ +/** + * @brief "Smoothing" algorithm takes the average of a given number of previous + * inputs. Can stabilize an erratic input stream. + */ +class Smooth +{ +public: + std::list list; + double size; + + /** + * @brief Constructor for Smooth class + * + * @param Size number of previous values that "smoother" object averages + */ + explicit Smooth(double Size = 50.) + : size(Size) + { + } + + /** + * @brief Calls updateList then finds average of previous inputs + */ + double smooth(double reading) + { + updateList(reading); + double sumList = std::accumulate(list.begin(), list.end(), 0.0); + return (sumList / list.size()); + } + + /** + * @brief Updates list of previous inputs with current input + */ + void updateList(double reading) + { + list.push_front(reading); + // ensure list stays at desired size + while(list.size() > size) + { + list.pop_back(); + } + } + + /** + * @brief Clears list of all previous inputs + */ + void clear() { list.clear(); } +}; + +} diff --git a/include/puara/utils/threshold.h b/include/puara/utils/threshold.h new file mode 100644 index 0000000..3785fe7 --- /dev/null +++ b/include/puara/utils/threshold.h @@ -0,0 +1,41 @@ +//********************************************************************************// +// Puara Gestures - Utilities (.h) // +// https://github.com/Puara/puara-gestures // +// Société des Arts Technologiques (SAT) - https://sat.qc.ca // +// Input Devices and Music Interaction Laboratory (IDMIL) - https://www.idmil.org // +// Edu Meneses (2024) - https://www.edumeneses.com // +//********************************************************************************// + +#pragma once + +#include + +namespace puara_gestures::utils +{ + +/** + * Simple Threshold class to ensure a value doesn't exceed settable max and min values. + */ +class Threshold +{ +public: + double min{-10.0}; + double max{10.0}; + double current{}; + + double update(double reading) + { + current = reading; + if(reading < min) + { + return min; + } + if(reading > max) + { + return max; + } + return reading; + } +}; + +} diff --git a/include/puara/utils/wrap.h b/include/puara/utils/wrap.h new file mode 100644 index 0000000..60e4345 --- /dev/null +++ b/include/puara/utils/wrap.h @@ -0,0 +1,111 @@ +//********************************************************************************// +// Puara Gestures - Utilities (.h) // +// https://github.com/Puara/puara-gestures // +// Société des Arts Technologiques (SAT) - https://sat.qc.ca // +// Input Devices and Music Interaction Laboratory (IDMIL) - https://www.idmil.org // +// Edu Meneses (2024) - https://www.edumeneses.com // +//********************************************************************************// + +#pragma once + +#include + +namespace puara_gestures::utils +{ +/** + * @brief Wrapper class undoes modular effects of angle measurements + * Ensures that instead of "wrapping" (e.g. moving from - PI to PI), + * measurements continue to decrease or increase + */ +class Unwrap +{ +public: + double prev_angle{}; + double accum{}; + double range{}; + bool empty{}; + + /** + * @brief Constructor for Unwrap class + * + * Initaliazes accumulated at 0 to indicate the data has not "wrapped" yet + * Initalizes empty at True + * @param Min minimum input value + * @param Max maximum input value + */ + Unwrap(double Min, double Max) + : accum(0) + , range(Max - Min) + , empty(true) + { + } + + /** + * @brief Calculates whether angle has "wrapped" based on previous angle + * and updates accordingly + */ + double unwrap(double reading) + { + double diff; + + // check if update hasn't been called before or the unwrap class + // has been cleared + if(empty) + { + diff = 0; + empty = false; + } + else + { + diff = reading - prev_angle; + } + prev_angle = reading; + if(diff > (range / 2)) + { + accum -= 1; + } + if(diff < (range * -1 / 2)) + { + accum += 1; + } + return reading + accum * range; + } + + /** + * @brief resets the unwrap object + */ + void clear() + { + accum = 0; + empty = true; + } +}; + +/** + * @brief Wraps a value around a given minimum and maximum. + * Algorithm from Jean-Michael Celerier. "Authoring interactive media : a logical + * & temporal approach." Computation and Language [cs.CL]. Université de Bordeaux, + * 2018. English. ffNNT : 2018BORD0037ff. fftel-01947309 + */ +class Wrap +{ +public: + double min{}; + double max{}; + + double wrap(double reading) + { + double diff = std::fabs(max - min); + if(min <= reading && reading <= max) + { + return reading; + } + else if(reading >= max) + { + return min + (reading - std::fmod(min, diff)); + } + return max - (min - std::fmod(reading, diff)); + } +}; + +} diff --git a/puara_gestures.cpp b/src/puara_gestures.cpp similarity index 99% rename from puara_gestures.cpp rename to src/puara_gestures.cpp index 3c45913..4b10bd6 100644 --- a/puara_gestures.cpp +++ b/src/puara_gestures.cpp @@ -6,8 +6,10 @@ // Edu Meneses (2024) - 4ttps://www.edumeneses.com // //********************************************************************************// -#include "puara_gestures.h" +#include +// FIXME finish cleaning up this file +#if 0 void PuaraGestures::updateInertialGestures() { updateJabShakeAccl(); @@ -735,4 +737,5 @@ IMU_Orientation::Euler PuaraGestures::getOrientationEuler() // } else { // return in; // } -// } \ No newline at end of file +// } +#endif diff --git a/standalone/.DS_Store b/standalone/.DS_Store deleted file mode 100644 index 16d875f..0000000 Binary files a/standalone/.DS_Store and /dev/null differ diff --git a/standalone/CMakeLists.txt b/standalone/CMakeLists.txt index 3582fd5..04d61a9 100644 --- a/standalone/CMakeLists.txt +++ b/standalone/CMakeLists.txt @@ -20,9 +20,10 @@ set(OSSIA_OVERRIDE_PROTOCOLS "OSC;OSCQuery") FetchContent_Declare( libossia - GIT_REPOSITORY "https://github.com/OSSIA/libossia" + GIT_REPOSITORY "https://github.com/ossia/libossia" GIT_TAG master GIT_PROGRESS true + GIT_SHALLOW 1 ) FetchContent_MakeAvailable(libossia) @@ -43,26 +44,12 @@ FetchContent_MakeAvailable(Eigen) file(GLOB_RECURSE PUARA_SOURCE ../descriptors/*.cpp ../descriptors/*.h) -add_library(puara-gestures STATIC ${PUARA_SOURCE}) -target_link_libraries(puara-gestures PUBLIC - Eigen3::Eigen - Boost::boost -) - # Project setup -add_executable(puara-gestures-standalone) - -target_sources(puara-gestures-standalone - PRIVATE - main.cpp +add_executable(puara-gestures-standalone + main.cpp ) target_link_libraries(puara-gestures-standalone PRIVATE ossia - puara-gestures -) -target_include_directories(puara-gestures-standalone - PRIVATE - ${libossia_SOURCE_DIR}/ossia/include - ${libossia_SOURCE_DIR}/ossia + puara_gestures ) diff --git a/standalone/main.cpp b/standalone/main.cpp index 49f3449..c7bbfbf 100644 --- a/standalone/main.cpp +++ b/standalone/main.cpp @@ -11,21 +11,17 @@ // clang-format -i *.h -#include "../puara_gestures.h" - -#include -#include - #include #include +#include +#include +#include +#include #include #include - -#include -#include #include -#include "../descriptors/IMU_Sensor_Fusion/imu_orientation.h" +#include std::string client_ip = "127.0.0.1"; int client_port = 9000; @@ -42,10 +38,6 @@ puara_gestures::Shake3D shake; puara_gestures::Jab3D jab; puara_gestures::utils::LeakyIntegrator leakyintegrator; IMU_Orientation orientation; -puara_gestures::testing_roll rollTest; -puara_gestures::testing_tilt tiltTest; - -#include // struct Coord3D { // double x, y, z; @@ -75,13 +67,12 @@ int main(int argc, char* argv[]) { leakyintegrator.integrate(accelerometer[0]); }); - //rollTest.test(); - tiltTest.test(); - -// while(true) { -// puara_gestures::Coord3D shakeout = shake.current_value(); -// puara_gestures::Coord3D jabout = jab.current_value(); -// std::cout << "Shake X: " << shakeout.x << ", Jab X: " << jabout.x << ", Integrator: " << leakyintegrator.current_value << std::endl; -// std::this_thread::sleep_for(std::chrono::milliseconds(10)); -// }; + while(true) + { + puara_gestures::Coord3D shakeout = shake.current_value(); + puara_gestures::Coord3D jabout = jab.current_value(); + std::cout << "Shake X: " << shakeout.x << ", Jab X: " << jabout.x + << ", Integrator: " << leakyintegrator.current_value << std::endl; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + }; } diff --git a/testing_roll.h b/testing_roll.h deleted file mode 100644 index 36cc9c8..0000000 --- a/testing_roll.h +++ /dev/null @@ -1,127 +0,0 @@ -//********************************************************************************// -// Puara Gestures standalone - Receive OSC data to generate high-level // -// gestural descriptors // -// https://github.com/Puara/puara-gestures // -// Société des Arts Technologiques (SAT) - https://sat.qc.ca // -// Input Devices and Music Interaction Laboratory (IDMIL) - https://www.idmil.org // -// Edu Meneses (2024) - https://www.edumeneses.com // -//********************************************************************************// - -#include "puara_gestures.h" - -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -namespace puara_gestures -{ - -class testing_roll -{ -public: - Coord3D readinRawCSV(std::string line) - { - Coord3D cart; - int first_space = line.find_first_of(","); - int second_space = line.substr(first_space + 1).find_first_of(",") + first_space + 1; - double x = std::stod(line.substr(0, first_space)); - double y = std::stod(line.substr(first_space + 1, second_space)); - double z = std::stod(line.substr(second_space + 1, line.size())); - // add three doubles to a Coord3D - cart.x = x; - cart.y = y; - cart.z = z; - // return Coord3D - return cart; - } - - double readinRawSingleValue(std::string line) { return std::stod(line); } - - void test() - { - - // declare roll - Roll test; - utils::Threshold thresh; - - // set up common path - std::string common = "../../testing files/roll/"; - - // read in accl data - std::string accl_path = common + "accel_raw.csv"; - std::ifstream accl_file(accl_path); - std::string accl_line; - - // read in gyro data - std::string gyro_path = common + "gyro_raw.csv"; - ; - std::ifstream gyro_file(gyro_path); - std::string gyro_line; - - // read in timestamp data - std::string timestamp_path = common + "timestamp_raw.csv"; - std::ifstream timestamp_file(timestamp_path); - std::string timestamp_line; - - // read in mag data - std::string mag_path = common + "mag_raw.csv"; - std::ifstream mag_file(mag_path); - std::string mag_line; - - // read in roll data - std::string roll_path = common + "roll_raw.csv"; - std::ifstream roll_file(roll_path); - std::string roll_line; - - while(std::getline(accl_file, accl_line, '\n') - && std::getline(gyro_file, gyro_line, '\n') - && std::getline(timestamp_file, timestamp_line, '\n') - && std::getline(mag_file, mag_line, '\n') - && std::getline(roll_file, roll_line, '\n')) - { - if(accl_line.empty() || gyro_line.empty() || timestamp_line.empty() - || mag_line.empty() || roll_line.empty()) - { - break; - } - - Coord3D accl; - accl = readinRawCSV(accl_line); - - Coord3D gyro; - gyro = readinRawCSV(gyro_line); - - double timestamp = readinRawSingleValue(timestamp_line); - - Coord3D mag; - mag = readinRawCSV(mag_line); - - double roll = readinRawSingleValue(roll_line); - - double puara_roll = test.roll(accl, gyro, mag, timestamp); - - std::cout << "Roll Value from Puara = " << puara_roll << ", "; - std::cout << "Roll Value from T-Stick = " << roll << ". "; - std::cout << "Diff = " << (std::fabs(puara_roll - roll)) << ".\n"; - double unwrapped = test.unwrap(roll); - std::cout << "Unwrapped = " << unwrapped << ", "; - double smoothed = test.smooth(unwrapped); - std::cout << "Smoothed = " << smoothed << ", "; - double threshold = thresh.update(smoothed); - std::cout << "In Threshold = " << threshold << ", "; - double wrapped = test.wrap(threshold); - std::cout << "Wrapped = " << wrapped << ".\n"; - } - }; -}; -}; \ No newline at end of file diff --git a/testing_tilt.h b/testing_tilt.h deleted file mode 100644 index 46befac..0000000 --- a/testing_tilt.h +++ /dev/null @@ -1,123 +0,0 @@ -// //********************************************************************************// -// Puara Gestures standalone - Receive OSC data to generate high-level // -// gestural descriptors // -// https://github.com/Puara/puara-gestures // -// Société des Arts Technologiques (SAT) - https://sat.qc.ca // -// Input Devices and Music Interaction Laboratory (IDMIL) - https://www.idmil.org // -// Edu Meneses (2024) - https://www.edumeneses.com // -//********************************************************************************// - -#include "puara_gestures.h" - -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include - -namespace puara_gestures -{ - -class testing_tilt -{ -public: - Coord3D readinRawCSV(std::string line) - { - Coord3D cart; - int first_space = line.find_first_of(","); - int second_space = line.substr(first_space + 1).find_first_of(",") + first_space + 1; - double x = std::stod(line.substr(0, first_space)); - double y = std::stod(line.substr(first_space + 1, second_space)); - double z = std::stod(line.substr(second_space + 1, line.size())); - // add three doubles to a Coord3D - cart.x = x; - cart.y = y; - cart.z = z; - // return Coord3D - return cart; - } - - double readinRawSingleValue(std::string line) { return std::stod(line); } - - void test() - { - - // declare tilt - Tilt test; - utils::Threshold thresh; - - // set up common path - std::string common = "../../testing files/tilt/"; - - // read in accl data - std::string accl_path = common + "accel_raw.csv"; - std::ifstream accl_file(accl_path); - std::string accl_line; - - // read in gyro data - std::string gyro_path = common + "gyro_raw.csv"; - ; - std::ifstream gyro_file(gyro_path); - std::string gyro_line; - - // read in timestamp data - std::string timestamp_path = common + "timestamp_raw.csv"; - std::ifstream timestamp_file(timestamp_path); - std::string timestamp_line; - - // read in mag data - std::string mag_path = common + "mag_raw.csv"; - std::ifstream mag_file(mag_path); - std::string mag_line; - - // read in tilt data - std::string tilt_path = common + "tilt_raw.csv"; - std::ifstream tilt_file(tilt_path); - std::string tilt_line; - - while(std::getline(accl_file, accl_line, '\n') - && std::getline(gyro_file, gyro_line, '\n') - && std::getline(timestamp_file, timestamp_line, '\n') - && std::getline(mag_file, mag_line, '\n') - && std::getline(tilt_file, tilt_line, '\n')) - { - if(accl_line.empty() || gyro_line.empty() || timestamp_line.empty() - || mag_line.empty() || tilt_line.empty()) - { - break; - } - - Coord3D accl; - accl = readinRawCSV(accl_line); - - Coord3D gyro; - gyro = readinRawCSV(gyro_line); - - double timestamp = readinRawSingleValue(timestamp_line); - - Coord3D mag; - mag = readinRawCSV(mag_line); - - double tilt = readinRawSingleValue(tilt_line); - - double puara_tilt = test.tilt(accl, gyro, mag, timestamp); - - std::cout << "Tilt Value from Puara = " << puara_tilt << ", "; - std::cout << "Tilt Value from T-Stick = " << tilt << ". "; - std::cout << "Diff = " << (std::fabs(puara_tilt - tilt)) << ".\n"; - double smoothed = test.smooth(puara_tilt); - std::cout << "Smoothed = " << smoothed << ", "; - double threshold = thresh.update(smoothed); - std::cout << "In Threshold = " << threshold << ", "; - } - }; -}; -}; \ No newline at end of file diff --git a/tests/testing_roll.cpp b/tests/testing_roll.cpp new file mode 100644 index 0000000..e0ff56a --- /dev/null +++ b/tests/testing_roll.cpp @@ -0,0 +1,119 @@ +//********************************************************************************// +// Puara Gestures standalone - Receive OSC data to generate high-level // +// gestural descriptors // +// https://github.com/Puara/puara-gestures // +// Société des Arts Technologiques (SAT) - https://sat.qc.ca // +// Input Devices and Music Interaction Laboratory (IDMIL) - https://www.idmil.org // +// Edu Meneses (2024) - https://www.edumeneses.com // +//********************************************************************************// + +#include + +#include + +#include +#include + +using namespace puara_gestures; +Coord3D readinRawCSV(std::string line) +{ + Coord3D cart; + int first_space = line.find_first_of(","); + int second_space = line.substr(first_space + 1).find_first_of(",") + first_space + 1; + double x = std::stod(line.substr(0, first_space)); + double y = std::stod(line.substr(first_space + 1, second_space)); + double z = std::stod(line.substr(second_space + 1, line.size())); + // add three doubles to a Coord3D + cart.x = x; + cart.y = y; + cart.z = z; + // return Coord3D + return cart; +} + +double readinRawSingleValue(std::string line) +{ + return std::stod(line); +} + +int main() +{ + // declare roll + Roll test; + utils::Threshold thresh; + + // set up common path + std::string common = PUARA_TESTS_DIR "/roll/"; + + // read in accl data + std::string accl_path = common + "accel_raw.csv"; + std::ifstream accl_file(accl_path); + std::string accl_line; + + // read in gyro data + std::string gyro_path = common + "gyro_raw.csv"; + ; + std::ifstream gyro_file(gyro_path); + std::string gyro_line; + + // read in timestamp data + std::string timestamp_path = common + "timestamp_raw.csv"; + std::ifstream timestamp_file(timestamp_path); + std::string timestamp_line; + + // read in mag data + std::string mag_path = common + "mag_raw.csv"; + std::ifstream mag_file(mag_path); + std::string mag_line; + + // read in roll data + std::string roll_path = common + "roll_raw.csv"; + std::ifstream roll_file(roll_path); + std::string roll_line; + + while(std::getline(accl_file, accl_line, '\n') + && std::getline(gyro_file, gyro_line, '\n') + && std::getline(timestamp_file, timestamp_line, '\n') + && std::getline(mag_file, mag_line, '\n') + && std::getline(roll_file, roll_line, '\n')) + { + if(accl_line.empty() || gyro_line.empty() || timestamp_line.empty() + || mag_line.empty() || roll_line.empty()) + { + break; + } + + Coord3D accl; + accl = readinRawCSV(accl_line); + + Coord3D gyro; + gyro = readinRawCSV(gyro_line); + + double timestamp = readinRawSingleValue(timestamp_line); + + Coord3D mag; + mag = readinRawCSV(mag_line); + + double roll = readinRawSingleValue(roll_line); + + double puara_roll = test.roll(accl, gyro, mag, timestamp); + + std::cout << "Roll Value from Puara = " << puara_roll << ", "; + std::cout << "Roll Value from T-Stick = " << roll << ". "; + std::cout << "Diff = " << (std::fabs(puara_roll - roll)) << ".\n"; + double unwrapped = test.unwrap(roll); + std::cout << "Unwrapped = " << unwrapped << ", "; + double smoothed = test.smooth(unwrapped); + std::cout << "Smoothed = " << smoothed << ", "; + double threshold = thresh.update(smoothed); + std::cout << "In Threshold = " << threshold << ", "; + double wrapped = test.wrap(threshold); + std::cout << "Wrapped = " << wrapped << ".\n"; + + //testing arrayAverageZero + std::vector testArray = {1.0, 2.0, 3.0, 4.0, 5.0}; + double average + = puara_gestures::utils::arrayAverageZero(testArray.data(), testArray.size()); + std::cout << "Average: " << average << std::endl; + } +} diff --git a/tests/testing_tilt.cpp b/tests/testing_tilt.cpp new file mode 100644 index 0000000..bf674a5 --- /dev/null +++ b/tests/testing_tilt.cpp @@ -0,0 +1,109 @@ +// //********************************************************************************// +// Puara Gestures standalone - Receive OSC data to generate high-level // +// gestural descriptors // +// https://github.com/Puara/puara-gestures // +// Société des Arts Technologiques (SAT) - https://sat.qc.ca // +// Input Devices and Music Interaction Laboratory (IDMIL) - https://www.idmil.org // +// Edu Meneses (2024) - https://www.edumeneses.com // +//********************************************************************************// + +#include + +#include + +#include +#include + +using namespace puara_gestures; +Coord3D readinRawCSV(std::string line) +{ + Coord3D cart; + int first_space = line.find_first_of(","); + int second_space = line.substr(first_space + 1).find_first_of(",") + first_space + 1; + double x = std::stod(line.substr(0, first_space)); + double y = std::stod(line.substr(first_space + 1, second_space)); + double z = std::stod(line.substr(second_space + 1, line.size())); + // add three doubles to a Coord3D + cart.x = x; + cart.y = y; + cart.z = z; + // return Coord3D + return cart; +} + +double readinRawSingleValue(std::string line) +{ + return std::stod(line); +} + +int main() +{ + // declare tilt + Tilt test; + utils::Threshold thresh; + + // set up common path + std::string common = PUARA_TESTS_DIR "/tilt/"; + + // read in accl data + std::string accl_path = common + "accel_raw.csv"; + std::ifstream accl_file(accl_path); + std::string accl_line; + + // read in gyro data + std::string gyro_path = common + "gyro_raw.csv"; + ; + std::ifstream gyro_file(gyro_path); + std::string gyro_line; + + // read in timestamp data + std::string timestamp_path = common + "timestamp_raw.csv"; + std::ifstream timestamp_file(timestamp_path); + std::string timestamp_line; + + // read in mag data + std::string mag_path = common + "mag_raw.csv"; + std::ifstream mag_file(mag_path); + std::string mag_line; + + // read in tilt data + std::string tilt_path = common + "tilt_raw.csv"; + std::ifstream tilt_file(tilt_path); + std::string tilt_line; + + while(std::getline(accl_file, accl_line, '\n') + && std::getline(gyro_file, gyro_line, '\n') + && std::getline(timestamp_file, timestamp_line, '\n') + && std::getline(mag_file, mag_line, '\n') + && std::getline(tilt_file, tilt_line, '\n')) + { + if(accl_line.empty() || gyro_line.empty() || timestamp_line.empty() + || mag_line.empty() || tilt_line.empty()) + { + break; + } + + Coord3D accl; + accl = readinRawCSV(accl_line); + + Coord3D gyro; + gyro = readinRawCSV(gyro_line); + + double timestamp = readinRawSingleValue(timestamp_line); + + Coord3D mag; + mag = readinRawCSV(mag_line); + + double tilt = readinRawSingleValue(tilt_line); + + double puara_tilt = test.tilt(accl, gyro, mag, timestamp); + + std::cout << "Tilt Value from Puara = " << puara_tilt << ", "; + std::cout << "Tilt Value from T-Stick = " << tilt << ". "; + std::cout << "Diff = " << (std::fabs(puara_tilt - tilt)) << ".\n"; + double smoothed = test.smooth(puara_tilt); + std::cout << "Smoothed = " << smoothed << ", "; + double threshold = thresh.update(smoothed); + std::cout << "In Threshold = " << threshold << ", "; + } +} diff --git a/tests/testing_touch.cpp b/tests/testing_touch.cpp new file mode 100644 index 0000000..fb78453 --- /dev/null +++ b/tests/testing_touch.cpp @@ -0,0 +1,26 @@ +#include + +#include + +using namespace puara_gestures; + +int main() +{ + Touch touch; + + int touchSize = 16; + int discrete_touch[16] = {0}; + + // simulate a touch (1) at a position 5 + discrete_touch[5] = 1; + + // Update the touch data + touch.updateTouchArray(discrete_touch, touchSize); + + // Output the computed values + std::cout << "touchAll: " << touch.touchAll << std::endl; + std::cout << "brush: " << touch.brush << std::endl; + std::cout << "rub: " << touch.rub << std::endl; + + // multi touch +}