Easy vector, quaternion library designed for rotations and filters in C++
Switch branches/tags
Nothing to show
Clone or download
Fetching latest commit…
Cannot retrieve the latest commit at this time.
Permalink
Failed to load latest commit information.
LICENSE
README.md
sensor.cpp
sensor.h
vector3.cpp
vector3.h

README.md

vector3

Easy vector, quaternion library designed for rotations and sensor fusion filters in C++14. It is also mobile device agnostic as mobile devices,such as iphones use y axis front and pitch is around X axis.

quaternions

  • quaternion from euler all the angles should be in radians
    auto roll = rad(40.0);
    auto pitch = rad(40.0);
    auto yaw = rad(40.0);
    auto q = Quaternion(roll,pitch,yaw);
  • change euler order in constructor of specific quaternion,ex: first roll then pitch then yaw
    auto roll = rad(40.0);
    auto pitch = rad(40.0);
    auto yaw = rad(40.0);
    auto q = Quaternion(roll,pitch,yaw,RotationOrder::rpy);
  • to euler as roll,pitch,yaw
    auto roll =  Quaternion(roll,pitch,yaw).roll();
    auto pitch =  Quaternion(roll,pitch,yaw).pitch();
    auto yaw =  Quaternion(roll,pitch,yaw).yaw();
  • to euler as Vector3
    Vector3 euler =  Quaternion(roll,pitch,yaw).euler();
  • concatanating rotations is as simple as multiplying quaternions (q1,q2,q3,q4) in respective order q1*q2*q3*q4
  • rotating vector3 is as simple as multiplying with quaternion v1*q1
  • reverse rotation is negating -q1
  • rotation between two quaternions -q1*q2
  • slerp is as simple as multiplying quaternion with scalar ex half angle rotation of q1 is 0.5*q1
  • mid rotation between two quaternions is q1*(0.5*(-q1*q2))

vectors

  • constructor and constant static variables Vector3 v(0.5,0.7,0.3);
  • from constant static variables Vector3 v = Vector3::X;
  • c++11 version auto v = Vector3::X;
  • to Vector2
    Vector2 xy = Vector3::X.xy(); 
    auto xz = Vector3::X.xz(); 
    auto yz = Vector3::X.yz(); 
  • negating auto negx = -Vector3::X;
  • arithmetic operations auto v = (0.3*(Vector3::X+Vector3::Y)).normalized();
  • multiplying with scalar all xyz auto v2 = v1*0.5
  • multiplying with vector
        Vector3 coef(0.5,0.3,0.1);
        auto v = coef*Vector3::X;
  • angle between two vectors
        auto angleInRad = Vector3::X.angle(Vector3::Y);
        auto angleInDegree = deg(angleInRad);
  • using in filtering
       auto beta = 0.1;
       auto result = Vector3::X*beta + Vector3::Y*(1-beta);
  • rotation quaternion between two vectors Quaternion q = Vector3::X.rotation(Vector3::Y);
  • rotation quaternion around specific axis (not that there are many axis that can yield to same roation result) Quaternion q = Vector3::X.rotation(Vector3::Y,-Vector3::Z);
  • to array vector<scalar> v = Vector3::X;
  • from array or initializer_list Vector3 v = {0.2,0.30,0.5};
  • from vector iterator
       vector<scalar> vec = {0.2,0.30,0.5,0.45};
       Vector3 v = vec.begin()+1;

vector3 arithmetic helper functions

  • degree to radian and vice versa conversion

       inline scalar rad(scalar deg);
       inline scalar deg(scalar rad);
    
  • max,min,pow,abs

        inline Vector3 pow(const Vector3& v,scalar p);
        inline Vector3 abs(const Vector3& v);
        inline Vector3 max(const Vector3& v1,const Vector3& v2);
        inline Vector3 min(const Vector3& v1,const Vector3& v2);
    

mobile device vs others

  • mobile devices definition of roll,pitch and reference frame XYZ is different than other engineering areas, define MOBILE if your application is for mobile or mobile data ``` #define MOBILE ````
  • it is also automatically defined in header as
    #ifdef TARGET_OS_IOS  or TARGET_IPHONE_SIMULATOR  or __ANDROID__             
        #define MOBILE
    #endif
    

sensors

  • sensor type
    enum SensorType{
        none = 0,accelerometer,gyroscope,magnetometer,coreMotion,position,velocity,userAcceleration,pixels
    };
  • sensor data
    class SensorData{
        public:
        double time = 0; // in secs
        SensorType type = SensorType::none;
        Vector3 v = Vector3::Zero;
     };

file operations

File read/write operations are simplified

  • reading vector3 list from file
        vector<Vector3> rtn;
        ifstream(path)>>rtn;
  • reading SensorData list from file
        vector<SensorData> rtn;
        ifstream(path)>>rtn;
  • writing vector3 list to file
        vector<Vector3> rtn = {Vector3::X,Vector3::Y,Vector3::Z};
        ofstream(path)<<rtn<<endl;
  • writing SensorData list to file
        vector<SensorData> rtn;
        ofstream(path)<<rtn<<endl;

n dimensional ternary search

  • use n dimensional search when local minima is equal to global minima, otherwise it probably will find one of local minimas
    vector<scalar> ternary(vector<scalar> start, vector<scalar> end, function<scalar(vector<scalar>)> eval);
  • exmaple code for finding calibration parameters of magnetometer
    void findMagBiasCollectively(const vector<SensorData>& list){
       if(list.size()==0) return;
       auto mlist = SensorData::filter(list, SensorType::magnetometer);
       Vector3 mx = mlist[0].v,mn=mlist[0].v;
       for(auto i:mlist){
           mx = vector3::max(i.v,mx);
           mn = vector3::min(i.v,mn);
       }
    
       auto biasList =
       vector3::ternary( mn.array()   , mx , [&](vector<scalar> p){
           Vector3 v(p);
           auto r = 0.0;
           for(auto i : mlist){
               r += ((i.v - v)).length();
           }
           r /= mlist.size();
           auto error = 0.0;
           for(auto i : mlist){
               auto t = ((i.v - v)).length() - r;
               error += t*t;
           }
           return error;
       });
       magBias = biasList;
    }

Example codes:

    using namespace vector3;
    
    auto v = Vector3::X;
    
    auto roll = rad(40.0);
    auto pitch = rad(40.0);
    auto yaw = rad(40.0);
    
    auto q = Quaternion(roll,pitch,yaw);
    
    q = Quaternion(roll,pitch,yaw,RotationOrder::rpy);
    
    q = q*Quaternion(roll,0,0);
    
    Vector3 to = v*q;
    
    Quaternion q1 = v.rotation(to);
    
    std::cout << "roll: " << deg(q.roll())<<", pitch: " << deg(q.pitch()) 
              << ", yaw: " << deg(q.yaw()) << endl;
    
    std::cout << "roll: " << deg(to.roll())<<", pitch: " << deg(to.pitch()) 
              << ", yaw: " << deg(to.yaw()) << endl;

by Maksim Piriyev @ https://dron.ee