Split out some quaternion utility functions
This allows them to be used from elsewhere.
Change-Id: I5f691c5e78811916411a19e37dd666cf916b78fd
diff --git a/aos/controls/quaternion_utils.h b/aos/controls/quaternion_utils.h
new file mode 100644
index 0000000..1455821
--- /dev/null
+++ b/aos/controls/quaternion_utils.h
@@ -0,0 +1,71 @@
+#ifndef AOS_CONTROLS_QUATERNION_UTILS_H_
+#define AOS_CONTROLS_QUATERNION_UTILS_H_
+
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+#include "glog/logging.h"
+
+namespace aos {
+namespace controls {
+
+// Function to compute the quaternion average of a bunch of quaternions. Each
+// column in the input matrix is a quaternion (optionally scaled by it's
+// weight).
+template <int SM>
+inline Eigen::Matrix<double, 4, 1> QuaternionMean(
+ Eigen::Matrix<double, 4, SM> input) {
+ // Algorithm to compute the average of a bunch of quaternions:
+ // http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf
+
+ Eigen::Matrix<double, 4, 4> m = input * input.transpose();
+
+ Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>> solver;
+ solver.compute(m);
+
+ Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvectorsType
+ eigenvectors = solver.eigenvectors();
+ Eigen::EigenSolver<Eigen::Matrix<double, 4, 4>>::EigenvalueType eigenvalues =
+ solver.eigenvalues();
+
+ int max_index = 0;
+ double max_eigenvalue = 0.0;
+ for (int i = 0; i < 4; ++i) {
+ const double eigenvalue = std::abs(eigenvalues(i, 0));
+ if (eigenvalue > max_eigenvalue) {
+ max_eigenvalue = eigenvalue;
+ max_index = i;
+ }
+ }
+
+ // Assume that there shouldn't be any imaginary components to the eigenvector.
+ // I can't prove this is true, but everyone else seems to assume it...
+ // TODO(james): Handle this more rigorously.
+ for (int i = 0; i < 4; ++i) {
+ CHECK_LT(eigenvectors(i, max_index).imag(), 1e-4)
+ << eigenvectors(i, max_index);
+ }
+ return eigenvectors.col(max_index).real().normalized();
+}
+
+// Converts from a quaternion to a rotation vector, where the rotation vector's
+// direction represents the axis to rotate around and its magnitude represents
+// the number of radians to rotate.
+Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
+ const Eigen::Matrix<double, 4, 1> &X);
+
+inline Eigen::Matrix<double, 3, 1> ToRotationVectorFromQuaternion(
+ const Eigen::Quaternion<double> &X) {
+ return ToRotationVectorFromQuaternion(X.coeffs());
+}
+
+// Converts from a rotation vector to a quaternion. If you supply max_angle_cap,
+// then the rotation vector's magnitude will be clipped to be no more than
+// max_angle_cap before being converted to a quaternion.
+Eigen::Matrix<double, 4, 1> ToQuaternionFromRotationVector(
+ const Eigen::Matrix<double, 3, 1> &X,
+ const double max_angle_cap = std::numeric_limits<double>::infinity());
+
+} // namespace controls
+} // namespace aos
+
+#endif // AOS_CONTROLS_QUATERNION_UTILS_H_