Need help to solve the problem with rotation of tracking objects

Hello, everyone

I am looking for solution of following problem:

I am using Optitrack Motive to track the markers that i weared on my finger and using Unity to create 3d objects.

And middleVR (free edition) for 3d classes.

If i move my finger, i will see the 3d object for finger move with my finger too. It works.
But if i sometimes rotate my finger 90 degrees or 180 degrees, the 3d finger object will jump to the opposite site and rotates 180 degrees back…

Video for problem:
http://de.tinypic.com/player.php?v=dqmxiu&s=8#.Vhf0Xvntmko

Have your any idea about my problem?

Thank your!

regards,

Carvin

Could be any number of things, I’d try to isolate the cause first by checking:

  • Is it the tracking software that’s sending bad positional / rotation data?
  • Is it my code that’s rotating the 3D object?
  • Maybe it’s a sign problem (minus / plus same angle value), are you using Euler angles instead of quaternions?

Hope it helps!

Hi, DrBlort

  1. The tracking software is Optitrack Motive, it sends the correct data of positions and rotations of markers.
  2. What is your code? I am using a 3rd party code to read the data from Motive to Unity.
  3. I am using Quaternion…

Have you any idea?

Hey,

About 2, I meant the question as if you were making it. Bad choice from my part :slight_smile:

If I’m not misunderstanding, you’re not using the position/rotation from Optitrack data (given by that 3rd party software) directly over your model.

So the questions that you should be checking are:
Is the 3rd party software doing a conversion between the Optitrack data and Unity rotations? If so, check if there’s a way to read it raw and see what happens.
If instead you’re making a conversion, check if there isn’t a wrong assumption there.

Hi, DrBlort

Thank your for your answer again.

I am using the class of the 3rd party code to give die tracking transform to my unity (finger) objects directly, i am sure about it.

But i am not sure that the function which reads the position and rotation data to my finger objects will prevent that problem in my video…because it also uses EulerAngles function…

In the 3rd party code is the following function of the class to read the data from Motive:

// Unpack RigidBody data
        private static void ReadRigidBody(Byte[] b, ref int offset, OptiTrackRigidBody rb)
        {
            try
            {
                int[] iData = new int[100];
                float[] fData = new float[100];
              
                // RB ID
                Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
                //int iSkelID = iData[0] >> 16;           // hi 16 bits = ID of bone's parent skeleton
                //int iBoneID = iData[0] & 0xffff;       // lo 16 bits = ID of bone
                rb.ID = iData[0]; // already have it from data descriptions
              
                // RB pos
                float[] pos = new float[3];
                Buffer.BlockCopy(b, offset, pos, 0, 4 * 3); offset += 4 * 3;
                rb.position.x = pos[0]; rb.position.y = pos[1]; rb.position.z = pos[2];
              
                // RB ori
                float[] ori = new float[4];
                Buffer.BlockCopy(b, offset, ori, 0, 4 * 4); offset += 4 * 4;
                rb.orientation.x = ori[0]; rb.orientation.y = ori[1]; rb.orientation.z = ori[2]; rb.orientation.w = ori[3];
                Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
                int nMarkers = iData[0];
                Buffer.BlockCopy(b, offset, fData, 0, 4 * 3 * nMarkers); offset += 4 * 3 * nMarkers;
              
                Buffer.BlockCopy(b, offset, iData, 0, 4 * nMarkers); offset += 4 * nMarkers;
              
                Buffer.BlockCopy(b, offset, fData, 0, 4 * nMarkers); offset += 4 * nMarkers;
              
                Buffer.BlockCopy(b, offset, fData, 0, 4); offset += 4;       
            } catch (Exception e)
            {
                Debug.LogError(e.ToString());
            }
        }

And in the following function of another class to get the positions and rotations from that class:

public Vector3 getPosition(int rigidbodyIndex)
    {
        if(OptitrackManagement.DirectMulticastSocketClient.IsInit())
        {
            DataStream networkData = OptitrackManagement.DirectMulticastSocketClient.GetDataStream();
            Vector3 pos = origin + networkData.getRigidbody(rigidbodyIndex).position * scale;
            pos.x = -pos.x; // not really sure if this is the best way to do it
            //pos.y = pos.y; // these may change depending on your configuration and calibration
            //pos.z = -pos.z;
            return pos;
        }
        else
        {
            return Vector3.zero;
        }
    }

    public Quaternion getOrientation(int rigidbodyIndex)
    {
        // should add a way to filter it
        if(OptitrackManagement.DirectMulticastSocketClient.IsInit())
        {
            DataStream networkData = OptitrackManagement.DirectMulticastSocketClient.GetDataStream();
            Quaternion rot = networkData.getRigidbody(rigidbodyIndex).orientation;

            // change the handedness from motive
            //rot = new Quaternion(rot.z, rot.y, rot.x, rot.w); // depending on calibration
          
            // Invert pitch and yaw
            Vector3 euler = rot.eulerAngles;
            rot.eulerAngles = new Vector3(euler.x, -euler.y, euler.z); // these may change depending on your calibration

            return rot;
        }
        else
        {
            return Quaternion.identity;
        }
    }

How should i only use quaternion instead of euler angles ???

Thank you very much.

regards, Carvin

I would change the second function to return the original rotation, which is being modified to invert the Y angle, to this:

 public Quaternion getOrientation(int rigidbodyIndex)
    {
        // should add a way to filter it
        if(OptitrackManagement.DirectMulticastSocketClient.IsInit())
        {
            DataStream networkData = OptitrackManagement.DirectMulticastSocketClient.GetDataStream();
            Quaternion rot = networkData.getRigidbody(rigidbodyIndex).orientation;
            // change the handedness from motive
            //rot = new Quaternion(rot.z, rot.y, rot.x, rot.w); // depending on calibration
        
            // Invert pitch and yaw
            //Vector3 euler = rot.eulerAngles;
            //rot.eulerAngles = new Vector3(euler.x, -euler.y, euler.z); // these may change depending on your calibration
            return rot;
        }
        else
        {
            return Quaternion.identity;
        }
    }

This way you’ll have the original rotation value. Check what that the original rotation looks like, and then if you have to change it (very likely, I’m guessing that that was the motive to add the lines inverting the Y angle), I’d multiply it by a second quaternion having a rotation that compensates it.

Something like this (obviously untested, and your variable names will surely be different):

Quaternion myDeviceThingOrientation = theDevice.getOrientation(index);

// This supposedly will have the same effect than the method commented lines 
myDeviceThingOrientation *= Quaternion.Euler(0.0f, 180.0f, 0.0f);

As a brief explanation with my limited knowledge of quaternions, you “add” rotations by multiplication, which is what the *= is doing (multiplicating with itself, you could expand that to myDeviceThingOrientation = myDeviceThingOrientation * Quaternion.Euler(0.0f, 180.0f, 0.0f); if you like. It would be the same).
Have in mind that if you multiplicate several rotations the order is important.

Could be that you’ll have to add a conditional, something like “if the Y angle of the rotation is > 180, then rotate, else do nothing”, but that’ll depend on the effect of the rotation sent by the device.

Hi, DrBlort
Thank your for your answer:)

Sorry, i forgot to post another function i used to update the positions and orientations of my finger objects, as script component, i meant.

like following code:

using UnityEngine;
using System.Collections;

public class OptiTrackObject : MonoBehaviour {

    public int rigidbodyIndex;
    public Vector3 rotationOffset;

    // Use this for initialization
    void Start () {

    }
  
    // Update is called once per frame
    void Update () {
        Vector3 pos = OptiTrackManager.Instance.getPosition(rigidbodyIndex);
        Quaternion rot = OptiTrackManager.Instance.getOrientation(rigidbodyIndex);
        rot = rot * Quaternion.Euler(rotationOffset);
        this.transform.position = pos;
        this.transform.rotation = rot;
    }
}

It was original code from the 3rd party programm, i already have changed the position with some offsets to my project…

Thanks :wink:

Well, I think it’s still a good idea to remove the modification to the original rotation in the 3rd party software, and apply those offsets yourself in you code, when you know what’s doing and why (I mean the conditional code to modify the rotation depending on what’s happening).

EDIT:
One check for that conditional code that I can think of, would be to check if the current rotation is bigger than, say, 90 degrees or more (whichever value is reasonable) than in the previous frame, because you know that the finger couldn’t have rotated that much in so short of a time.
That could indicate the need to add the offset.

Hi, DrBlort.

Thanks again !!!

The 3 classes i posted are only the original code, i have modified it to my own project.

In the function “public Quaternion getOrientation(int rigidbodyIndex)” i only have modified rot.eulerAngles = new Vector3(euler.x, -euler.y, euler.z); TO rot.eulerAngles = new Vector3(-euler.x, -euler.y, euler.z); because Motive and Unity have different coordinate systems.

As you suggested i modified the getOrientation(int rigidbodyIndex) function → without using eulerAngles, just return rot.
And also changed rot = rot *Quaternion.Euler(rotationOffset); TO rot *= Quaternion.Euler(180.0f, 180.0f, 0) or rot *= Quaternion.Euler(-180.0f, -180.0f, 0)…But my problem is still there, the finger objects are also not at the right position i wished and they are still flipping if i rotate my finger…

Have you seen my video?

Thanks!

Carvin

I’ve seen it, yes. Maybe the object center (the pivot point) is not correct, or Optitrack Motive assumes a different hierarchy than you have in your objects, i.e. you have your object parented differently.

Try with the conditional adjustment if the rotation changes too much from one frame to the next.

Hi, DrBlort

Thanks for your answer, the finger objects dont have any parent. As you suggested i will check the changing of angles in frames, if i rotate my finger…

Hi everyone,
I’ve a similar problem. I was looking in the internet but not enough lucky to find a solution. It would be nice you anyone can help me with solusions, suggestion or any guidelines.

Problem: I have a physical pen that is tracked by OptiTrack (Motiv 2.3.5) and showed in unity VR. Pen’s translation in (X, Y, Z) axis works fine but the problem in rotation. When the rotation angle bellow 180 Degree, everything is fine but if the pen rotate more than 180 Degree in Y axis, rotation added extra offset in each axis.
for example: if I rotate the pen 180 degree in Y axis, then if I rotate the pen in any direction, it is not rotated as expected. (jumped, or added axtra angel, similar to this problem)

My setup: in unity- Parent object > Penbody (child).
Script: For rotation, I used Quaternion, no Euler angle was used in code.
I have a custom script that actually calibrate OptiTrack coordinate to the unity coordinate.
I use this code: unity-vive-reality-mapper/Assets/Scripts/Calibration/VRCalibration.cs at master · felixkosmalla/unity-vive-reality-mapper · GitHub

The following theory is a great help for finding optimal rotation in 3D
https://nghiaho.com/?page_id=671
It’s a bit of wired that translation and rotation works fine until it rotates bellow 180 in Y axis.

OptiTrack Coordinate of the tracked pen and display in VR

void UpdatePose()
    {
        OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState( RigidBodyId, NetworkCompensation);
        if ( rbState != null )
        {
            // this.transform.localPosition = rbState.Pose.Position;
            // this.transform.localRotation = rbState.Pose.Orientation;

            //Raw coordinate from OptiTrack
            Vector3 penOptiTrackPosition = rbState.Pose.Position;
            Quaternion penOptiTrackRotation = rbState.Pose.Orientation;
            penPosition = new Vector3(penOptiTrackPosition.x, penOptiTrackPosition.y, penOptiTrackPosition.z);
            penOrientation = new Quaternion(penOptiTrackRotation.x, penOptiTrackRotation.y, penOptiTrackRotation.z, 1);

            transform.localPosition = penPosition;
            transform.localRotation = penOrientation;
                 
        //    print("Pen rotation: " + penOrientation.eulerAngles.y);
                       
        }
    }

for coordinate calibration, I used the following implementation

  1. theory: Finding optimal rotation and translation between corresponding 3D points | Nghia Ho
  2. public void svd_matrix_algorithm() from this- unity-vive-reality-mapper/Assets/Scripts/Calibration/VRCalibration.cs at master · felixkosmalla/unity-vive-reality-mapper · GitHub