"Average" of multiple quaternions?
I tried Slerping the quaternions as suggested here but that didn't work for what I'm trying to do (model was distorted), so I simply ended up transforming the vectors by each quaternion and then doing an average (until I can find a better solution).
Contrary to popular belief in the computer graphics industry, there is a straightforward algorithm to solve this problem which is robust, accurate, and simple that comes from the aerospace industry. It runs in time linear in the number of quaternions being averaged plus a (largish) constant factor.
Let Q = [a1q1, a2q2, ..., anqn],
where ai is the weight of the ith quaternion, and qi is the ith quaternion being averaged, as a column vector. Q is therefore a 4×N matrix.
The normalized eigenvector corresponding to the largest eigenvalue of QQT is the weighted average. Since QQT is self-adjoint and at least positive semi-definite, fast and robust methods of solving that eigenproblem are available. Computing the matrix-matrix product is the only step that grows with the number of elements being averaged.
See this technical note in the Journal of Guidance, Control, and Dynamics from 2007, which is a summary paper of this and other methods. In the modern era, the method I quoted above makes a good tradeoff for implementation reliability and robustness, and was already published in textbooks in 1978!
Unfortunately it isn't terribly simple to do, but it is possible. Here's a whitepaper explaining the math behind it: http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20070017872_2007014421.pdf
Check out the Unity3D Wiki page (The below code sample is from the same article): http://wiki.unity3d.com/index.php/Averaging_Quaternions_and_Vectors
//Get an average (mean) from more then two quaternions (with two, slerp would be used).
//Note: this only works if all the quaternions are relatively close together.
//Usage:
//-Cumulative is an external Vector4 which holds all the added x y z and w components.
//-newRotation is the next rotation to be added to the average pool
//-firstRotation is the first quaternion of the array to be averaged
//-addAmount holds the total amount of quaternions which are currently added
//This function returns the current average quaternion
public static Quaternion AverageQuaternion(ref Vector4 cumulative, Quaternion newRotation, Quaternion firstRotation, int addAmount){
float w = 0.0f;
float x = 0.0f;
float y = 0.0f;
float z = 0.0f;
//Before we add the new rotation to the average (mean), we have to check whether the quaternion has to be inverted. Because
//q and -q are the same rotation, but cannot be averaged, we have to make sure they are all the same.
if(!Math3d.AreQuaternionsClose(newRotation, firstRotation)){
newRotation = Math3d.InverseSignQuaternion(newRotation);
}
//Average the values
float addDet = 1f/(float)addAmount;
cumulative.w += newRotation.w;
w = cumulative.w * addDet;
cumulative.x += newRotation.x;
x = cumulative.x * addDet;
cumulative.y += newRotation.y;
y = cumulative.y * addDet;
cumulative.z += newRotation.z;
z = cumulative.z * addDet;
//note: if speed is an issue, you can skip the normalization step
return NormalizeQuaternion(x, y, z, w);
}
public static Quaternion NormalizeQuaternion(float x, float y, float z, float w){
float lengthD = 1.0f / (w*w + x*x + y*y + z*z);
w *= lengthD;
x *= lengthD;
y *= lengthD;
z *= lengthD;
return new Quaternion(x, y, z, w);
}
//Changes the sign of the quaternion components. This is not the same as the inverse.
public static Quaternion InverseSignQuaternion(Quaternion q){
return new Quaternion(-q.x, -q.y, -q.z, -q.w);
}
//Returns true if the two input quaternions are close to each other. This can
//be used to check whether or not one of two quaternions which are supposed to
//be very similar but has its component signs reversed (q has the same rotation as
//-q)
public static bool AreQuaternionsClose(Quaternion q1, Quaternion q2){
float dot = Quaternion.Dot(q1, q2);
if(dot < 0.0f){
return false;
}
else{
return true;
}
}
Also this post: http://forum.unity3d.com/threads/86898-Average-quaternions
Here is the implementation for MATLAB function that I use to average Quaternions for orientation estimation. It is straightforward to convert the MATLAB to any other language, except that this particular method (Markley 2007) requires the calculation of the eigenvectors and eigenvalues. There are many libraries (including Eigen C++) that can do this for you.
You can read the description/header of the file to see the math from the original paper.
matlab file taken from http://www.mathworks.com/matlabcentral/fileexchange/40098-tolgabirdal-averaging-quaternions :
% by Tolga Birdal
% Q is an Mx4 matrix of quaternions. weights is an Mx1 vector, a weight for
% each quaternion.
% Qavg is the weightedaverage quaternion
% This function is especially useful for example when clustering poses
% after a matching process. In such cases a form of weighting per rotation
% is available (e.g. number of votes), which can guide the trust towards a
% specific pose. weights might then be interpreted as the vector of votes
% per pose.
% Markley, F. Landis, Yang Cheng, John Lucas Crassidis, and Yaakov Oshman.
% "Averaging quaternions." Journal of Guidance, Control, and Dynamics 30,
% no. 4 (2007): 1193-1197.
function [Qavg]=quatWAvgMarkley(Q, weights)
% Form the symmetric accumulator matrix
A=zeros(4,4);
M=size(Q,1);
wSum = 0;
for i=1:M
q = Q(i,:)';
w_i = weights(i);
A=w_i.*(q*q')+A; % rank 1 update
wSum = wSum + w_i;
end
% scale
A=(1.0/wSum)*A;
% Get the eigenvector corresponding to largest eigen value
[Qavg, ~]=eigs(A,1);
end