Integrating InertiaCube and Vicon data

[roman_suvorov]

I hope it's okay to ask a completely different question here regarding the same project..
I am now trying to get position tracking to work properly. Simply doing something along these lines in mytimer callback function doesn't work as expected, as I found out:

...
def mytimer(num):
if (num == ANIMATE):

...
viz.MainView.setPosition(vicon.getPosition())
viz.MainView.setQuat(inertiaCube.getQuat())
...
...

I decided to form a transformation matrix myself and use viz.MainView.setMatrix method instead:

...
def mytimer(num):
if (num == ANIMATE):

...
mainViewMtx.postTrans(subLists(vicon.getPosition(), prevPosition))
mainViewMtx.postQuat(inertiaCube.getQuat())
viz.MainView.setMatrix(mainViewMtx)
...
mainViewMtx = vicon.getMatrix()
prevPosition = vicon.getPosition()

Some comments are due:

1. mainViewMtx is the matrix I construct and eventually assign to viz.MainView every frame. It is updated from its previous value by post-multiplying it first by the change in position (which I get via a call to subLists(minuend, subtrahend), a trivial custom function which returns a list the elements of which are: minuend[0]-subtrahend[0], minuend[1]-subtrahend[1], minuend[2]-subtrahend[2], etc.), and then post-multiplying it by the quaternion I get from InertiaCube.
2. The change in position is obtained by substracting the position at the previous iteration (prevPosition) from the currect position (vicon.getPosition()). prevPosition and mainViewMtx are updated at the end of each iteration using Vicon's currect data.

As of now, position tracking actually works (that is to say, MainView moves forward as I move forward, etc.), but looking up or down has severe side effects of translating the view in some direction, rotating or tilting of it. Rotating left/right works but also translates the view.

Now I can finally ask the question itself: the underlined bits of code in the second listing seem to me as the source of this problem. What exactly am I doing wrong? Is it their order (i.e., deal with quats before I apply the change in position)? Is it using preTrans/preQuat rather than postTrans/postQuat (by the way, what is the difference in terms of the effect on MainView?)? Is it both?

And another question: I assume the calls vicon.getPosition() and vicon.getMatrix() do not block, that is to say, the execution is not suspended to get the data from Vicon but rather a pre-fetched cached data is used. Is this correct? Otherwise, given that we estimate our Vicon system has a lag of 100 ms, my entire script is WRONG.

In short, I'm lost. I kind of postponed my efforts of integrating Vicon and InertiaCube data in favor of getting this position tracking to work and I seem to be walking into another problem. Any help would be appreciated..

[farshizzo]

Applying a post rotation to a matrix which already has position data will cause the position data to be rotated as well. So if the position is 1 meter forward and you post rotate the matrix 90 degrees around the Y-axis, the new position will be 1 meter to the right. It seems like you just want to set the rotation of the matrix to the intersense rotation. In that case use setQuat instead.

mainViewMtx.setQuat(inertiaCube.getQuat())

The Vicon plugin receives data in a separate thread, so it does not block the main graphics thread. So getting sensor data from the plugin will return cached data from the last update.