Skip to content

Latest commit

 

History

History
217 lines (199 loc) · 6.5 KB

README.md

File metadata and controls

217 lines (199 loc) · 6.5 KB
Raw

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