Spherical interpolation of large numpy array

Question

I have a numpy array which has 8 fields and has 20 million samples.

The fields are [time,X,Y,Z,QW,QX,QY,QZ] where x,y,z are spatial points and QW,QX,QY,QZ is a quaternion

There are 2 different kinds of samples, one which contains [time,X,Y,Z] data, and one which contains [time,QW,QX,QY,QZ]. Since the array is homogeneous, a sample of the first type will look like [time,X,Y,Z,nan,nan,nan,nan] and a sample of the second type will look like [time,nan,nan,nan,QW,QX,QY,QZ]. There are many more samples of type [time,X,Y,Z,nan,nan,nan,nan]. There are only 20,000 samples of type [time,nan,nan,nan,QW,QX,QY,QZ] in the array.

So the array would look something like:

[time,nan,nan,nan,QW,QX,QY,QZ]
[time,X,Y,Z,nan,nan,nan,nan]
[time,X,Y,Z,nan,nan,nan,nan]
[time,X,Y,Z,nan,nan,nan,nan]
...
[time,X,Y,Z,nan,nan,nan,nan]
[time,nan,nan,nan,QW,QX,QY,QZ]
[time,X,Y,Z,nan,nan,nan,nan]
...
[time,nan,nan,nan,QW,QX,QY,QZ]

my question is, how can I use SLERP (spherical linear interpolation) from pyquaternion to interpolate the QW,QX,QY,QZ values between observations?

The pyquaternion interpolate function takes arguments (quaternion1,quaternion2,ratio)

where ratio is (TimeOfPoint-TimeQuaternion1)/(TimeQuaternion2-TimeQuaternion1)

the issue is, for a given point time,x,y,z, how to quickly search for the nearest upper and lower quaternion observation.

For an example point 1000000 I have tried:

data.shape = (20000000,8)

quat1=Quaternion(data[np.where((data[1000000,0]>=data[:,0]) & (~np.isnan(data[:,4])))[0][-1],4:8])

quat2=Quaternion(data[np.where((data[1000000,0]<=data[:,0]) & (~np.isnan(data[:,4])))[0][0],4:8])

this works, but it takes 2 seconds per sample.

I am looking for a faster way to find the upper and lower quaternion observation for each x,y,z point in order to SLERP.

Possible duplicate of [How to interpolate rotations?](https://stackoverflow.com/questions/2879441/how-to-interpolate-rotations) — Joe, Jan 31 '18 at 06:45
Observation 1: You don't need to search for every sample because many will share the same upper and lower quaternion. Observation 2: Maybe better to turn the approach around and interpolate all points between two given quaternions - no need to search. — MB-F, Jan 31 '18 at 07:51
@kazemakase both good points. Is there a way around creating large logical masks to search? — Ian Campbell Moore, Jan 31 '18 at 21:26

MB-F · Answer 1 · 2018-02-01T08:19:03.333

I do not usually work with quaternions so I do not have any implementation available. However, as the problem seems to be how to find the points to interpolate, this is not directly related to quaternions.

This approach iterates over pairs of available observations and interpolates all samples that lie inbetween:

# Column 0: time
# Columns 1 - 3: quaternion (well, not really, but it should get the idea across)
x = np.array([[0, 0, 1, 1],
              [1, np.nan, np.nan, np.nan],
              [2, np.nan, np.nan, np.nan],
              [3, 1, 0, 0],
              [4, np.nan, np.nan, np.nan],
              [5, 0, 1, 0],
              [6, np.nan, np.nan, np.nan],
              [7, np.nan, np.nan, np.nan],
              [8, np.nan, np.nan, np.nan],
              [9, 0, 0, 0]], dtype=float)


qidx = np.flatnonzero(~np.isnan(x[:, -1]))

for a, b in zip(qidx[:-1], qidx[1:]):
    ta, tb = x[a, 0], x[b, 0]
    qa, qb = x[a, 1:], x[b, 1:]
    xi = x[a+1:b]
    ratio = (xi[:, 0] - ta) / (tb - ta)

    # perform linear interpolation
    # should be replaced with quaternion interpolation
    xi[:, 1:] = qa + (qb - qa) * ratio[:, np.newaxis]

It should be easily possible to adapt the array layout and plug in a quaternion interpolation function that takes two quaternions (qa and qb and a ratio). If the function is not vectorized (it does not take an array of ratios) you will need to loop over the elements in ratio.

Timing tests (this version vs previous version) using test data of the form

n = 2000000
k = 1500
x = np.concatenate([np.arange(n).reshape(-1, 1), np.zeros((n, 3)) + np.nan], axis=1)
x[0, 1:] = [0, 0, 0]
x[-1, 1:] = [0, 0, 0]
x[np.random.randint(n, size=k), 1:] = np.random.rand(k, 3)

Results:

       n/k     |   old   |  new   | speedup
---------------+---------+--------+------------
   20000/15    |    6 ms |   2 ms |  x3
  200000/150   |  313 ms |  23 ms |  x13
 2000000/1500  |   34 s  | 250 ms |  x136
20000000/15000 |    memory error

This works but there is still the speed issue with the mask. Each loop takes .2seconds with 15000 x2 points and 20million-15000 x1 points. Creating the large logical masks takes .14seconds alone. .2*15000=50minutes of computation time — Ian Campbell Moore, Jan 31 '18 at 21:22
You have a huge amount of data, don't expect the computation time can be reduced to nothing ;) However, you are right that the logical mask is somewhat wasteful because it is built globally over `x1` whereas we only need a few local elements. — MB-F, Feb 01 '18 at 07:04
@IanCampbellMoore I've updated the answer to avoid the mask. It is *much* faster now and scales better to larger arrays. However, I don't know how well the quaternion interpolation performs. It will certainly be slower than linear interpolation and may become the new bottleneck. — MB-F, Feb 01 '18 at 08:21
I actually solved the problem by ripping code from scipy.interpolate.interp1d which uses np.searchsorted to find where in an existing array new samples should be placed. This lets me find upper and lower quaternion solutions in less than 2s. I further optimized by ripping the slerp function from https://stackoverflow.com/questions/2879441/how-to-interpolate-rotations and vectorizing it. — Ian Campbell Moore, Feb 01 '18 at 22:33
But it was your suggestions that led me to this solution so I upvoted. I will post my code later when I clean it up — Ian Campbell Moore, Feb 01 '18 at 22:34

Spherical interpolation of large numpy array

1 Answers1