- Home /
Problem fixing quaternions
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);
}
}
To begin with, calibrate your system by noting the first quaternion that gets returned, and storing its inverse. Then for subsequent quaternions, multiply them by that inverse before assigning them to the camera. This means that the camera's initial position should not jump when you first start applying the quaternions, so if you ensure the head tracker's initial orientation is sensible when you calibrate then it makes the next steps easier.
Next, carefully keeping the head tracker in that initial position, rotate it slightly to the left and observe what happens to the camera - it should rotate about one of the axes, but possibly not the correct one, so it could move left, right, up, down, or roll clockwise or anticlockwise. Then return the head tracker to its initial position again, and rotate it slightly upwards, and again note which way the camera moved. Finally, after returning the head tracker to its initial state, roll it about its forwards axis, and again note what happens to the camera.
If you post those results here - basically which axis the camera rotated around, and in which direction, for each movement of the head tracker - then I might be able to suggest how you should convert the orientations between bases.
Okay. I did what you said. The results are:
Rotate device to the left over the table (person looking left) -> looks to the left
Rotate device to the righ over the table (person looking right)-> looks to the right
Rotate device upwards over the table (person looking up) -> skyline rotates anti-clockwise and becomes vertical
Rotate device downwards over the table (person looking down) -> skyline rotates clockwise and becomes vertical
Rotate device anti-clockwise (right side of the device lifts from the table) -> looks up
Rotate device clockwise (left side of the device lifts from the table)-> looks down
So, Rotate device upwards should do what Rotate device anti-clockwise is doing and Rotate device downwards should do what Rotate device clockwise is doing.
The code used now is:
using UnityEngine;
using System;
using System.Collections;
using System.IO.Ports;
using System.$$anonymous$$anagement;
public class YEIHeadTracking : $$anonymous$$onoBehaviour{
public static SerialPort sp = new SerialPort("CO$$anonymous$$4");
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;
public Quaternion InitialQuaternionInversed;
void Start(){
sp.BaudRate = 115200;
sp.Parity = Parity.None;
sp.DataBits = 8;
sp.StopBits = StopBits.One;
// Set the read/write timeouts
sp.WriteTimeout = 100;
sp.ReadTimeout = 100;
sp.Open();
sp.Write(send_bytes,0,3);
}
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);
}
void Update(){
counter++;
byte[] read_bytes = new byte[16];
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 $$anonymous$$ono 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);
if (counter == 1) {
Quaternion InitialQuaternion = new Quaternion (x, y, z, w);
InitialQuaternionInversed = Quaternion.Inverse (InitialQuaternion);
}
Quaternion SubsequentQuaternion = new Quaternion(x,y,z,w);
SubsequentQuaternion = SubsequentQuaternion * InitialQuaternionInversed;
Camera.main.transform.rotation = SubsequentQuaternion;
// Send command
sp.Write(send_bytes,0,3);
}
}
That's good - so your camera is behaving as if it was looking along the device's right vector. To correct that you just need to multiply additionally by a 90-degree rotation about the Y axis, i.e. Quaternion.Euler(0, 90, 0) or possibly -90. Try each and see if either works.
Okay. So now I did this:
if (counter == 1) {
Quaternion InitialQuaternion = new Quaternion (x, y, z, w);
InitialQuaternionInversed = Quaternion.Inverse (InitialQuaternion);
}
// Create a quaternion
Quaternion SubsequentQuaternion = new Quaternion(x,y,z,w);
SubsequentQuaternion = SubsequentQuaternion * InitialQuaternionInversed;
SubsequentQuaternion = SubsequentQuaternion * Quaternion.Euler (0, -90, 0);
Camera.main.transform.rotation = SubsequentQuaternion;
It seems that it partially solved the problem. When I look up and down I have a mix of up+anti-clockwise rotation of the skyline and when I look down I have down+clockwise rotation of the skyline. When I look left or right I also have this mix but in a very $$anonymous$$or scale. Also, movimentation is changed too. The keyboard key that should walk ahead now walks backwards in diagonal. The one that should go backwards now goes ahead in diagonal. Same thing with left and right... they are exchanged and moves in diagonal. Using -90 ins$$anonymous$$d of 90 gives the same mix result, but with Up+clockwise and Down+anti-clockwise. $$anonymous$$ovimentation doesn't stay exchanged, it just goes in diagonal.
Did you try pre-multiplying the correction, ins$$anonymous$$d of post-multiplying it?
Yes. Pre-multiplying doesn't work either. In fact, makes it worst because the mix I described above is stronger and the Up and Down directions are inverted.
Answer by gfoot · Jan 20, 2014 at 04:51 AM
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:
Hold the device level and call CalibrateOrigin()
Point the device up a bit (pitch only, be careful not to roll or yaw) and call CalibrateUp()
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;
}
}
Hey man. It finally worked. I didn't used this final code you wrote for calibration, but I followed everything you said before and adjusted the angles using Quaternion.Euler. Another thing that made a significant change was switching:
Camera.main.transform.rotation
to
GameObject.Find ("First Person Controller").transform.rotation
Thanks a lot!
I also posted this question about the same device: http://answers.unity3d.com/questions/617664/problem-identifying-usb.html
Do you think you can also help me with it?
Hey, I'm trying to use your code for calibrating the device but its not working for some reason I don't understand. It seems like RawOrientation and InverseOrigin stays the same after CalibrateOrigin executes and Correction is (0,0,0,1). I put a Debug.Log to see the value of CorrectedOrientation and it also is (0,0,0,1).
I've made the code so the calibration starts automatically when the scene starts. Here is the code:
public class YEIHeadTracking : $$anonymous$$onoBehaviour{
public static SerialPort sp = new SerialPort("CO$$anonymous$$4");
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;
public Quaternion RawOrientation;
private Quaternion InverseOrigin = Quaternion.identity;
private Vector3 ObservedAxisX = Vector3.right;
private Quaternion Correction = Quaternion.identity;
private string StringUp = "Look UP and press ENTER to confirm direction";
private bool ActivateUp = false;
private string StringRight = "Look RIGHT and press SPACE to confirm direction";
private bool ActivateRight = false;
//void Start is the same of the first question above. I omitted because of the character limit.
//Here goes the function bytesToFloat from the first question.
void OnGUI()
{
Rect $$anonymous$$yRect = new Rect (0, 0, 100, 100);
if (ActivateUp)
{
StringUp = GUI.TextArea($$anonymous$$yRect, StringUp);
}
if (ActivateRight)
{
ActivateUp = false;
StringRight = GUI.TextArea($$anonymous$$yRect, StringRight);
}
}
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;
}
}
void Update(){
counter++;
// Here goes the code between lines 56 and 81 from the first question.
if (counter == 1)
{
RawOrientation = new Quaternion (x, y, z, w);
CalibrateOrigin ();
ActivateUp = true;
}
if (Input.Get$$anonymous$$ey ($$anonymous$$eyCode.Return))
{
CalibrateUp ();
ActivateUp = false;
ActivateRight = true;
}
if (Input.Get$$anonymous$$ey ($$anonymous$$eyCode.Space))
{
CalibrateRight ();
ActivateRight = false;
}
Debug.Log (CorrectedOrientation);
GameObject.Find ("First Person Controller").transform.rotation = CorrectedOrientation;
sp.Write(send_bytes,0,3);
}
}
RawOrientation needs to return the current up-to-date data from the sensor. It was meant to be a property rather than a variable but it doesn't matter much so long as you update it every frame.
So just moving its assignment just before the "if (counter == 1)" should work.
Yeap... it worked. I'd thought this RawOrientation should be captured just once that's why I left it inside the "if". Thanks again!
Your answer
Follow this Question
Related Questions
Flip over an object (smooth transition) 3 Answers
How to rotate on one axis while keeping the other axes open to be rotated by other scripts? 1 Answer
Object Local Y-Up being forced to be World Y-Up 1 Answer
Rotating on a plane 2 Answers
How to Change rotation while preserving local horizontal rotation 1 Answer