/*
* @brief Convert a rotation matrix to a quaternion.
* @note Pay attention to the convention used. The function follows the
* conversion in "Indirect Kalman Filter for 3D Attitude Estimation:
* A Tutorial for Quaternion Algebra", Equation (78).
*
* The input quaternion should be in the form
* [q1, q2, q3, q4(scalar)]^T
*/
inline Eigen::Vector4d rotationToQuaternion(
const Eigen::Matrix3d& R) {
Eigen::Vector4d score;
score(0) = R(0, 0);
score(1) = R(1, 1);
score(2) = R(2, 2);
score(3) = R.trace();
int max_row = 0, max_col = 0;
score.maxCoeff(&max_row, &max_col);
Eigen::Vector4d q = Eigen::Vector4d::Zero();
if (max_row == 0) {
q(0) = std::sqrt(1+2*R(0, 0)-R.trace()) / 2.0;
q(1) = (R(0, 1)+R(1, 0)) / (4*q(0));
q(2) = (R(0, 2)+R(2, 0)) / (4*q(0));
q(3) = (R(1, 2)-R(2, 1)) / (4*q(0));
} else if (max_row == 1) {
q(1) = std::sqrt(1+2*R(1, 1)-R.trace()) / 2.0;
q(0) = (R(0, 1)+R(1, 0)) / (4*q(1));
q(2) = (R(1, 2)+R(2, 1)) / (4*q(1));
q(3) = (R(2, 0)-R(0, 2)) / (4*q(1));
} else if (max_row == 2) {
q(2) = std::sqrt(1+2*R(2, 2)-R.trace()) / 2.0;
q(0) = (R(0, 2)+R(2, 0)) / (4*q(2));
q(1) = (R(1, 2)+R(2, 1)) / (4*q(2));
q(3) = (R(0, 1)-R(1, 0)) / (4*q(2));
} else {
q(3) = std::sqrt(1+R.trace()) / 2.0;
q(0) = (R(1, 2)-R(2, 1)) / (4*q(3));
q(1) = (R(2, 0)-R(0, 2)) / (4*q(3));
q(2) = (R(0, 1)-R(1, 0)) / (4*q(3));
}
if (q(3) < 0) q = -q;
quaternionNormalize(q);
return q;
}
