Среднее значение нескольких кватернионов?

К сожалению, это не очень просто сделать, но возможно. Вот технический документ, объясняющий математику, стоящую за этим: http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20070017872_2007014421.pdf

Посетите вики-страницу Unity3D (нижеприведенный пример кода взят из той же статьи): 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;
    }
}

Также этот пост: http://forum.unity3d.com/threads/86898-Average-quaternions

Вот реализация функции MATLAB, которую я использую для усреднения кватернионов для оценки ориентации. MATLAB легко преобразовать в любой другой язык, за исключением того, что этот конкретный метод (Markley 2007) требует вычисления собственных векторов и собственных значений. Есть много библиотек (включая Eigen C++), которые могут сделать это за вас.

Вы можете прочитать описание/заголовок файла, чтобы увидеть математику из оригинальной статьи.

файл matlab, взятый с 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

Я попытался использовать кватернионы, как было предложено здесь, но это не помогло. работаю над тем, что я пытаюсь сделать (модель была искажена), поэтому я просто преобразовал векторы по каждому кватерниону, а затем сделал среднее (пока не найду лучшее решение).

Вы не можете добавлять кватернионы. Что вы можете сделать, так это найти кватернион, который непрерывно вращается между двумя углами, в том числе наполовину. Интерполяция кватернионов известна как «slerp» и имеет страницу в Википедии. Это очень полезный трюк для анимации. В некотором отношении slerp является основной причиной использования кватернионов в компьютерной графике.

Это моя реализация алгоритма Толги Бердала на питоне:

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]

Существует технический отчет за 2001 год, в котором говорится, что среднее на самом деле является довольно хорошим приближением, при условии, что кватернионы лежат близко друг к другу. (для случая -q=q можно просто перевернуть те, которые указывают в другом направлении, предварительно умножив их на -1, чтобы все кватернионы включали жизнь в одну и ту же полусферу.

В этой статье 2007 года описан еще лучший подход, в котором используется СВД. Это тот же документ, на который ссылался Натан. Я хотел бы добавить, что существует не только C++, но и реализация Matlab< /а>. Из выполнения тестового сценария, который поставляется с кодом Matlab, я могу сказать, что он дает неплохие результаты для небольших возмущений (0,004 * равномерный шум) задействованных кватернионов:

qinit=rand(4,1);
Q=repmat(qinit,1,10);

% apply small perturbation to the quaternions 
perturb=0.004;
Q2=Q+rand(size(Q))*perturb;

С кватернионами можно сделать то же самое, но с небольшой поправкой: 1. Отменить кватернион перед усреднением, если его скалярное произведение с предыдущей суммой отрицательно. 2. Нормировать средний кватернион, конец усреднения, если ваша библиотека работает с единичными кватернионами.

Средний кватернион будет представлять примерно среднее вращение (максимальная ошибка около 5 градусов).

ВНИМАНИЕ: Средняя матрица разных ориентаций может быть нарушена, если повороты слишком разные.

Кватернионы не являются идеальным набором степеней свободы для вращения при вычислении неограниченного среднего.

Вот чем я чаще всего пользуюсь(

[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;

    }`

Поскольку здесь есть разные подходы, я написал скрипт Matlab для их сравнения. Эти результаты, кажется, предполагают, что простого усреднения и нормализации кватернионов (подход из вики Unity, называемый здесь simple_average) может быть достаточно для случаев, когда кватернионы достаточно похожи и небольшие отклонения допустимы.

Вот результат:

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

И вот код:

%% 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']);

Проверьте здесь мое решение взвешенного усреднения, а также медиану Lp кватернионов.

Самая простая реализация (с Numpy в Python›=3.6) решения Маркли:

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()]

где Q имеет размер N на 4. Полученный кватернион уже нормализован.

В этом случае веса по умолчанию равны 1. В противном случае вы можете дать список весов размера N (по одному на кватернион).

Вот и все.

Вот репозиторий GitHub с реализацией предложенного алгоритма :) https://github.com/christophhagen/averaging-quaternions

Спасибо и кредиты christophhagen ofc ;)