I'm trying to make the switch from matrices to quaternions for skeletal animation in my OpenGL program, but I've encountered a problem:
Given a number of unit quaternions, I need to get a quaternion that when used to transform a vector will give a vector that is the average of the vector transformed by each quaternion individually. (with matrices I would simply add the matrices together and divide by the number of matrices)
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
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).
This is my implementation in python of Tolga Birdal's algorithm:
import numpy as np
def quatWAvgMarkley(Q, weights):
'''
Averaging Quaternions.
Arguments:
Q(ndarray): an Mx4 ndarray of quaternions.
weights(list): an M elements list, a weight for each quaternion.
'''
# Form the symmetric accumulator matrix
A = np.zeros((4, 4))
M = Q.shape[0]
wSum = 0
for i in range(M):
q = Q[i, :]
w_i = weights[i]
A += w_i * (np.outer(q, q)) # rank 1 update
wSum += w_i
# scale
A /= wSum
# Get the eigenvector corresponding to largest eigen value
return np.linalg.eigh(A)[1][:, -1]
You cannot add quaternions. What you can do is find a quaternion that rotates continuously between two angles, including halfway. Quaternion interpolation is known as "slerp" and has a wikipedia page. This is a very useful trick for animation. In some respects slerp is the primary reason for using quaternions in computer graphics.
There is a technical report from 2001 which states that the mean is actually quite a good approximation, provided that the quaternions lie close together. (for the case of -q=q, you could just flip the ones that point in the other direction by pre multiplying them by -1, so that all of the quaternions involved life in the same half sphere.
An even better approach is sketched in this paper from 2007, which involves using an SVD. This is the same paper that Nathan referenced. I would like to add that there is not just a C++, but also a Matlab implementation. From executing the test script which comes with the matlab code, I can say that it gives quite good results for small pertubations (0.004 * uniform noise) of the quaternions involved:
qinit=rand(4,1);
Q=repmat(qinit,1,10);
% apply small perturbation to the quaternions
perturb=0.004;
Q2=Q+rand(size(Q))*perturb;
With quaternions you can do same thing, but with small correction: 1. Negate quaternion before averaging if its dot product with previous sum is negative. 2. Normalize average quaternion, a the end of averaging, if your library works with unit quaternions.
The average quaternion will represent approximately average rotation (max error about 5 degree).
WARNING: Average matrix of different orientations can be broken if rotations too different.
Quaternions are not an ideal set of DOF to use for rotations when computing an unconstrained average.
Here is what I use most of the time (
[MethodImpl(MethodImplOptions.AggressiveInlining)]
internal static Vector3 ToAngularVelocity( this Quaternion q )
{
if ( abs(q.w) > 1023.5f / 1024.0f)
return new Vector3();
var angle = acos( abs(q.w) );
var gain = Sign(q.w)*2.0f * angle / Sin(angle);
return new Vector3(q.x * gain, q.y * gain, q.z * gain);
}
[MethodImpl(MethodImplOptions.AggressiveInlining)]
internal static Quaternion FromAngularVelocity( this Vector3 w )
{
var mag = w.magnitude;
if (mag <= 0)
return Quaternion.identity;
var cs = cos(mag * 0.5f);
var siGain = sin(mag * 0.5f) / mag;
return new Quaternion(w.x * siGain, w.y * siGain, w.z * siGain, cs);
}
internal static Quaternion Average(this Quaternion refence, Quaternion[] source)
{
var refernceInverse = refence.Inverse();
Assert.IsFalse(source.IsNullOrEmpty());
Vector3 result = new Vector3();
foreach (var q in source)
{
result += (refernceInverse*q).ToAngularVelocity();
}
return reference*((result / source.Length).FromAngularVelocity());
}
internal static Quaternion Average(Quaternion[] source)
{
Assert.IsFalse(source.IsNullOrEmpty());
Vector3 result = new Vector3();
foreach (var q in source)
{
result += q.ToAngularVelocity();
}
return (result / source.Length).FromAngularVelocity();
}
internal static Quaternion Average(Quaternion[] source, int iterations)
{
Assert.IsFalse(source.IsNullOrEmpty());
var reference = Quaternion.identity;
for(int i = 0;i < iterations;i++)
{
reference = Average(reference,source);
}
return reference;
}`
Since there are different approaches here, I wrote a Matlab script to compare them. These results seem to suggest that simply averaging and normalizing the quaternions (the approach from the unity wiki, called simple_average here) might be enough for cases where the quaternions are sufficiently similar and small deviations are acceptable.
Here's the output:
everything okay, max angle offset == 9.5843
qinit to average: 0.47053 degrees
qinit to simple_average: 0.47059 degrees
average to simple_average: 0.00046228 degrees
loop implementation to matrix implementation: 3.4151e-06 degrees
And here's the code:
%% Generate random unity quaternion
rng(42); % set arbitrary seed for random number generator
M = 100;
qinit=rand(1,4) - 0.5;
qinit=qinit/norm(qinit);
Qinit=repmat(qinit,M,1);
%% apply small perturbation to the quaternions
perturb=0.05; % 0.05 => +- 10 degrees of rotation (see angles_deg)
Q = Qinit + 2*(rand(size(Qinit)) - 0.5)*perturb;
Q = Q ./ vecnorm(Q, 2, 2); % Normalize perturbed quaternions
Q_inv = Q * diag([1 -1 -1 -1]); % calculated inverse perturbed rotations
%% Test if everything worked as expected: assert(Q2 * Q2_inv = unity)
unity = quatmultiply(Q, Q_inv);
Q_diffs = quatmultiply(Qinit, Q_inv);
angles = 2*acos(Q_diffs(:,1));
angles_deg = wrapTo180(rad2deg(angles));
if sum(sum(abs(unity - repmat([1 0 0 0], M, 1)))) > 0.0001
disp('error, quaternion inversion failed for some reason');
else
disp(['everything okay, max angle offset == ' num2str(max(angles_deg))])
end
%% Calculate average using matrix implementation of eigenvalues algorithm
[average,~] = eigs(transpose(Q) * Q, 1);
average = transpose(average);
diff = quatmultiply(qinit, average * diag([1 -1 -1 -1]));
diff_angle = 2*acos(diff(1));
%% Calculate average using algorithm from https://stackoverflow.com/a/29315869/1221661
average2 = quatWAvgMarkley(Q, ones(M,1));
diff2 = quatmultiply(average, average2 * diag([1 -1 -1 -1]));
diff2_angle = 2*acos(diff2(1));
%% Simply add coefficients and normalize the result
simple_average = sum(Q) / norm(sum(Q));
simple_diff = quatmultiply(qinit, simple_average * diag([1 -1 -1 -1]));
simple_diff_angle = 2*acos(simple_diff(1));
simple_to_complex = quatmultiply(simple_average, average * diag([1 -1 -1 -1]));
simple_to_complex_angle = 2*acos(simple_to_complex(1));
%% Compare results
disp(['qinit to average: ' num2str(wrapTo180(rad2deg(diff_angle))) ' degrees']);
disp(['qinit to simple_average: ' num2str(wrapTo180(rad2deg(simple_diff_angle))) ' degrees']);
disp(['average to simple_average: ' num2str(wrapTo180(rad2deg(simple_to_complex_angle))) ' degrees']);
disp(['loop implementation to matrix implementation: ' num2str(wrapTo180(rad2deg(diff2_angle))) ' degrees']);
The easiest implementation (with Numpy in Python>=3.6) of Markley's solution would be:
import numpy as np
def q_average(Q, W=None):
if W is not None:
Q *= W[:, None]
eigvals, eigvecs = np.linalg.eig(Q.T@Q)
return eigvecs[:, eigvals.argmax()]
where Q is of size N-by-4. The resulting quaternion is already normalized.
In this case the weights are equal to 1 by default. Otherwise you can give a list of weights of size N (one per quaternion.)
That's it.
Check here for my solution to weighted averaging as well as Lp median of quaternions.
Here is my PyTorch implementation for computing the average quaternion
def mean(Q, weights=None):
if weights is None:
weights = torch.ones(len(Q), device=torch.device("cuda:0")) / len(Q)
A = torch.zeros((4, 4), device=torch.device("cuda:0"))
weight_sum = torch.sum(weights)
oriented_Q = ((Q[:, 0:1] > 0).float() - 0.5) * 2 * Q
A = torch.einsum("bi,bk->bik", (oriented_Q, oriented_Q))
A = torch.sum(torch.einsum("bij,b->bij", (A, weights)), 0)
A /= weight_sum
q_avg = torch.linalg.eigh(A)[1][:, -1]
if q_avg[0] < 0:
return -q_avg
return q_avg
I made use of the algorithm in http://tbirdal.blogspot.com/2019/10/i-allocate-this-post-to-providing.html which is "Averaging quaternions." by Markley, F. Landis, et al.
Here is a GitHub Repo with the implementation of this suggested algorithmn :) https://github.com/christophhagen/averaging-quaternions
Thanks and credits to christophhagen ofc ;)
There is another slerp-based-method which is probably what you actually want when averaging quaternions.
Let's first compare it to the eigen analysis based averaging:
Consider the averaging of two quaternions A and B corresponding to rotations around X-Axis by 0° and by 90° degree with weights w_A = 2 and w_B = 1.
The expected weighted average should correspond to a rotation around X-Axis by 30°.
Slerp-based weighted average will return the expected value.
Eigen analysis-based weighted average will return a rotation by 26.56°.
Eigen analysis-based method will return the quaternion which minimized Frobenius norm of the corresponding rotation matrices. The slerp based method will instead compute the average in 3D rotational space in terms of quaternions.
import math
import numpy as np
import quaternion # (pip install numpy-quaternion)
d2r = math.pi/180
r2d = 180/math.pi
def recover_angle(mat):
return np.arctan2(mat[1,0], mat[0,0])
# ground truth
angles = np.array([0,90])
weights = np.array([2,1])
mean_angle = np.sum(angles*(weights/weights.sum()))
quaternion_mean = quaternion.from_euler_angles(mean_angle*d2r,0,0)
# eigen analysis
Q = quaternion.as_float_array(
[
(weight**0.5) * quaternion.from_euler_angles(0,0,angle*d2r)
for angle,weight
in zip(angles,weights)
]
).T
QQT = Q @ Q.T
eigval,eigvec = np.linalg.eig(QQT)
quaternion_mean_eig = quaternion.from_float_array( eigvec[:,np.argmax(eigval)] )
# slerp based
def slerp_avg(quaternions, weights):
# welford's mean in terms of linear mix operations:
toqt = quaternion.from_float_array
mix = lambda a,b,k: quaternion.slerp(toqt(a),toqt(b),0,1,k)
wmean, wsum, num = quaternions[0], weights[0], len(weights)
for i in range(1,num):
wsum += weights[i]
k = (weights[i]/wsum)
# wmean := wmean*(1-k) + quaternions[i]*k
wmean = mix(wmean,quaternions[i],k)
return wmean
quaternion_mean_slerp = slerp_avg(quaternion.as_float_array(
[quaternion.from_euler_angles(0,0,angle*d2r) for angle in angles]
), weights)
mat_mean = quaternion.as_rotation_matrix(quaternion_mean)
mat_mean_eig = quaternion.as_rotation_matrix(quaternion_mean_eig)
mat_mean_slerp = quaternion.as_rotation_matrix(quaternion_mean_slerp)
print("expected", recover_angle(mat_mean)*r2d)
print("eigen", recover_angle(mat_mean_eig)*r2d)
print("slerp", recover_angle(mat_mean_slerp)*r2d)
Outputs:
expected 29.999999999999996
eigen 26.565051177077994
slerp 30.00000000000001
You can find a zero dependency C++ library for this at https://github.com/xaedes/average_affine_transform_mat/
