Rotation quaternion average calculation function using Eigen

quaternion_average.h

#include <Eigen/SVD>

/// Method to find the average of a set of rotation quaternions using Singular Value Decomposition
/*
 * The algorithm used is described here:
 * https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20070017872.pdf
 */
Eigen::Vector4f quaternionAverage(std::vector<Eigen::Vector4f> quaternions)
{
	if (quaternions.size() == 0)
	{
		std::cerr << "Error trying to calculate the average quaternion of an empty set!\n";
		return Eigen::Vector4f::Zero();
	}

	// first build a 4x4 matrix which is the elementwise sum of the product of each quaternion with itself
	Eigen::Matrix4f A = Eigen::Matrix4f::Zero();

	for (int q=0; q<quaternions.size(); ++q)
		A += quaternions[q] * quaternions[q].transpose();

	// normalise with the number of quaternions
	A /= quaternions.size();

	// Compute the SVD of this 4x4 matrix
	Eigen::JacobiSVD<Eigen::MatrixXf> svd(A, Eigen::ComputeThinU | Eigen::ComputeThinV);

	Eigen::VectorXf singularValues = svd.singularValues();
	Eigen::MatrixXf U = svd.matrixU();

	// find the eigen vector corresponding to the largest eigen value
	int largestEigenValueIndex;
	float largestEigenValue;
	bool first = true;

	for (int i=0; i<singularValues.rows(); ++i)
	{
		if (first)
		{
			largestEigenValue = singularValues(i);
			largestEigenValueIndex = i;
			first = false;
		}
		else if (singularValues(i) > largestEigenValue)
		{
			largestEigenValue = singularValues(i);
			largestEigenValueIndex = i;
		}
	}

	Eigen::Vector4f average;
	average(0) = U(0, largestEigenValueIndex);
	average(1) = U(1, largestEigenValueIndex);
	average(2) = U(2, largestEigenValueIndex);
	average(3) = U(3, largestEigenValueIndex);

	return average;
}

Hi, can I ask what the assumed order of quaternion elements is? (i.e. w,x,y,z or x,y,z,w)
Also, how come you used Eigen::Vector4 instead of Eigen::Quaternion?
Thanks

Hi EceChaik,

I believe this algorithm is agnostic to the position of the w element. You should be able to test this to be sure.

I wrote this code when I was still very new to Eigen, so I was unaware that there was an Eigen::Quaternion type at the time! There are a lot of improvements that could be made to this code the final average could be extracted with a single block call. I'm fairly sure that the output of the SVD operation is already ordered by eigen value size, so you can simply take the first eigen vector.

Hope this helps.

Hi,

Thanks for the reply, I'm not that worried about code optimization, although I agree that it can be made simpler.
I was just looking for a quick way to average some rotations, so I'll try it and hopefully it'll work

Hi,
I tried to use Eigen::Quaterniond, but for doing the instruction at line 20 I must use an Eigen::Vector4d. Could you share with me another way to write the instruction mentioned above? @EceChaik