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 = [a_1*q_1 a_2*q_2 ... a_n*q_n]
Where a_i are the weight of the ith quaternion, and q_i are the ith quaternion being averaged, as a column vector. Q is therefore a 4xN matrix.
The normalized eigenvector corresponding to the largest eigenvalue of Q*Q^T is the weighted average. Since Q*Q^T 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 (with code): http://wiki.unity3d.com/index.php/Averaging_Quaternions_and_Vectors
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).
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.
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]
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.
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']);
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;
}`
来源：https://stackoverflow.com/questions/12374087/average-of-multiple-quaternions