Problem fixing quaternions

Questions & Answers
1

Hello everyone. I’m having a problem with quaternions in my code. I’m using a YEI 3 Space Embedded for head tracking. I’ve found a code here on UnityAnswers for using this device and adapted for what I need.
Problem is: the axes are wrong. For example, when it should rotate around X it rotates around Z and I don’t know how to correct this. Everything I’ve tried until now has failed.

Can anyone help me?

My code is:

using UnityEngine;
using System;
using System.Collections;
using System.IO.Ports;
using System.Management;
 
 
public class YEIHeadTracking : MonoBehaviour{
 
    public static SerialPort sp = new SerialPort("COM3");
 
    // Command packet for getting the filtered tared orientation as a quaternion
    // {header byte, command byte, [data bytes], checksum byte}
    // checksum = (command byte + data bytes) % 256
    public static byte[] send_bytes = {0xf7, 0x00, 0x00};
    public static byte[] button_bytes = {0xf7, 0xfa, 0xfa};
    public static byte[] tare_bytes = {0xf7, 0x60, 0x60};
 
    public int counter = 0;
 
    // Use this for initialization
 
 
    void Start(){
 
       sp.BaudRate = 115200;
       sp.Parity = Parity.None;
       sp.DataBits = 8;
       sp.StopBits = StopBits.One;
       sp.WriteTimeout = 100;
       sp.ReadTimeout = 100;
 
       sp.Open();
 
 
       sp.Write(send_bytes,0,3);
    }
 
    // Helper function for taking the bytes read from the 3-Space Sensor and converting them into a float
    float bytesToFloat(byte[] raw_bytes, int offset){
       byte[] big_bytes = new byte[4];
       big_bytes[0] = raw_bytes[offset+3];
       big_bytes[1] = raw_bytes[offset+2];
       big_bytes[2] = raw_bytes[offset+1];
       big_bytes[3] = raw_bytes[offset+0];
       return BitConverter.ToSingle(big_bytes,0);
    }
 
 
    // Update is called once per frame
    void Update(){
 
       counter++;
        // A quaternion consists of 4 floats which is 16 bytes
        byte[] read_bytes = new byte[16];
 
        // Mono, for some reason, seems to randomly fail on the first read after a wirte so we must loop
        // through to make sure the bytes are read and Mono also seems not to always read the amount asked
        // so we must also read one byte at a time
        int read_counter = 100;
        int byte_idx = 0;
        while (read_counter > 0){
            try{
                byte_idx += sp.Read(read_bytes, byte_idx, 1);
            }
            catch{
                // Failed to read from serial port
            }
            if (byte_idx == 16){
                break;
            }
            if (read_counter <= 0){
                throw new System.Exception("Failed to read quaternion from port too many times." +
                    " This could mean the port is not open or the Mono serial read is not responding.");
            }
            --read_counter;
        }
 
       // Convert bytes to floats
       float x = bytesToFloat(read_bytes,0);
       float y = bytesToFloat(read_bytes,4);
       float z = bytesToFloat(read_bytes,8);
       float w = bytesToFloat(read_bytes,12);
 
       //Quaternion rotateX = Quaternion.AngleAxis(-x, Vector3.left);
       //Quaternion rotateY = Quaternion.AngleAxis(-y, Vector3.up);
       //Quaternion rotateZ = Quaternion.AngleAxis(z, Vector3.forward);
 
 
       // Create a quaternion 
       Quaternion quat = new Quaternion(x,y,z,w);
 
       // Perform rotation
        Camera.main.transform.rotation = quat;
       //Camera.main.transform.rotation = rotateY * rotateX * rotateZ;
 
 
       sp.Write(send_bytes,0,3);
    }
}
2

In case you prefer a more practical approach, I dug up some calibration code I’ve used before. You might not want end users exposed to this, but you can still use it to work out the correction you need for your device, then hard code that, if it seems to be consistent.

You need to provide a RawOrientation property which returns whatever you get from the device. Then:

  1. Hold the device level and call CalibrateOrigin()
  2. Point the device up a bit (pitch only, be careful not to roll or yaw) and call CalibrateUp()
  3. Level the device again, and turn it to face a bit to the right (again, yaw only, be careful not to roll or pitch), and call CalibrateRight()

After that, the CorrectedOrientation property should return sensible values, and if you’re happy with the result you could look at the _correction quaternion and maybe hard-code it if it’s always the same for your device.

You don’t have to point a long way up or to the right during calibration, but larger angles are likely to give more precise results.

	private Quaternion _inverseOrigin = Quaternion.identity;
	private Vector3 _observedAxisX = Vector3.right;
	private Quaternion _correction = Quaternion.identity;
	
	public void CalibrateOrigin()
	{
		_inverseOrigin = Quaternion.Inverse(RawOrientation);
	}

	public void CalibrateUp()
	{
		var observedRotationX = _inverseOrigin * RawOrientation;
		float angle;
		observedRotationX.ToAngleAxis (out angle, out _observedAxisX);
	}

	public void CalibrateRight()
	{
		var observedRotationY = _inverseOrigin * RawOrientation;
		float angle;
		Vector3 axisY;
		observedRotationY.ToAngleAxis (out angle, out axisY);

		var axisZ = Vector3.Cross (-_observedAxisX, axisY);

		_correction = Quaternion.LookRotation (axisZ, axisY);
	}

	public Quaternion CorrectedOrientation
	{
		get
		{
			return Quaternion.Inverse(_correction) * _inverseOrigin * RawOrientation * _correction;
		}
	}