Last active
March 25, 2024 13:57
-
-
Save PeteBlackerThe3rd/f73e9d569e29f23e8bd828d7886636a0 to your computer and use it in GitHub Desktop.
Rotation quaternion average calculation function using Eigen
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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,
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
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
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.