ANYbotics/kindr

uint8 quaternion implementation

simonlynen opened this issue · 7 comments

We would like to store data more compact and thus are interested in an quaternion with 4 bytes only.

Further reduction to 3 elements can be done by using the unit-norm constraint.

It seems there are already implementations of fixed point quaternions around:
https://github.com/PetteriAimonen/libfixmatrix/blob/master/fixquat.h

I propose looking into these before starting to change the default of kindr to this.

@HannesSommer any thoughts on this?

Do you also need to compute with those 4 byte quaternions or only store and load?
Computing with that low resolution seems pretty bad to me. You will loose a lot or need to be very careful with the operations (maybe work on non unit quaternions (using the full unit cube instead)).

Otherwise I would not extend kindr this way but rather have a compress to n (e.g. =4) byte and uncompress from n byte. We could then try to be as smart as possible to get lowest redundancy in those 4 or what ever bytes - there should be theory on how to do that given the target range of unit quaternion.

Do you need the precision in a S^3 symmetric way or maybe more precision close to identity? In the latter you are clearly better with 3 bytes for rotation vectors.

I can't afford to think more on that for today but maybe the next days?

http://www.gamedev.net/topic/461253-compressed-quaternions/

The best answer there from johnb is:

"A better solution is to store the smallest three of X, Y, Z and W, with a couple of bits to say which you've stored. In 32 bits this needs 10 bits per float, 2 bits to select the largest and so which three are stored, by e.g. describing how many times {X, Y, Z, W} needs to be permuted after decompression. "

and choose the sign of the quaternion before compression such that the largest component is positive.

Here is some quick code that I just wrote (completely untested of course)

So it will need verification that I packed and unpacked the bits correctly with their sign bits.

uint32 compressQuat(double w, double x, double y, double z)
{
  int max_index = 0;
  double max_val = fabs(w);
  if (fabs(x) > max_val) { max_val = fabs(x); max_index = 1;}
  if (fabs(y) > max_val) { max_val = fabs(y); max_index = 2;}
  if (fabs(z) > max_val) { max_val = fabs(z); max_index = 3;}

  int sign_max; 
  int a,b,c;
  const int kSF = 722; // floor(sqrt(2)*511), the largest scale factor such that 2nd largest component fits in 9 bits

  switch(max_index) {
   case 0:
    sign_max = w > 0 ? kSF : -kSF;
    a = sign_max * x; b = sign_max * y; c = sign_max * z;
    break;
   case 1:
    sign_max = x > 0 ? kSF : -kSF;
    a = sign_max * w; b = sign_max * y; c = sign_max * z;
    break;
   case 2:
    sign_max = y > 0 ? kSF : -kSF;
    a = sign_max * x; b = sign_max * w; c = sign_max * z;
    break;
   case 3:
    sign_max = z > 0 ? kSF : -kSF;
    a = sign_max * x; b = sign_max * y; c = sign_max * w;
    break;
  }

  const int k10bitmask = 0x3FF;

  uint32 compressed_quat = (max_index << 30) | ((a & k10bitmask) << 20) | ((b & k10bitmask) << 10) | (c & k10bitmask);
  return compressed_quat;
}

uncompressQuaterion(uint32 cq, double *wOut, double *xOut, double *yOut, double *zOut)
{
  const int k10bitmask = 0x3FF;
  const double kSF = 1.0/722;
  int max_ind = cq >> 30;
  int a = (cq >> 20) & k10bitmask;
  int b = (cq >> 10) & k10bitmask;
  int c = cq & k10bitmask;

  // turn back into a 10bit signed value
  if (a >= 512) { a -= 1024; }
  if (b >= 512) { b -= 1024; }
  if (c >= 512) { c -= 1024; }

  double w,x,y,z;
  switch (max_ind) {
   case 0:
    x = a*kSF; y = b*kSF; z = c*kSF;
    w = sqrt(1.0 - x * x - y * y - z * z);
    break;
   case 1:
    w = a*kSF; y = b*kSF; z = c*kSF;
    x = sqrt(1.0 - w * w - y * y - z * z);
    break;
   case 2:
    x = a*kSF; w = b*kSF; z = c*kSF;
    y = sqrt(1.0 - x * x - w * w - z * z);
    break;
   case 3:
    x = a*kSF; y = b*kSF; w = c*kSF;
    z = sqrt(1.0 - x * x - y * y - w * w);
    break;
  }

  *wOut = w;
  *xOut = x;
  *yOut = y;
  *zOut = z;
}

This is awesome.

This is not a feature of the current implementation.