Eigen::Matrix<typename Derived::Scalar, 4, 1> rotmat2quat(const Eigen::MatrixBase<Derived>& M)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<Derived>, 3, 3);
using namespace std;
Matrix<typename Derived::Scalar, 4, 3> A;
A.row(0) << 1.0, 1.0, 1.0;
A.row(1.0) << 1.0, -1.0, -1.0;
A.row(2) << -1.0, 1.0, -1.0;
A.row(3) << -1.0, -1.0, 1.0;
Matrix<typename Derived::Scalar, 4, 1> B = A * M.diagonal();
typename Matrix<typename Derived::Scalar, 4, 1>::Index ind, max_col;
typename Derived::Scalar val = B.maxCoeff(&ind, &max_col);
typename Derived::Scalar w, x, y, z;
switch (ind) {
case 0: {
// val = trace(M)
w = sqrt(1.0 + val) / 2.0;
typename Derived::Scalar w4 = w * 4.0;
x = (M(2, 1) - M(1, 2)) / w4;
y = (M(0, 2) - M(2, 0)) / w4;
z = (M(1, 0) - M(0, 1)) / w4;
break;
}
case 1: {
// val = M(1,1) - M(2,2) - M(3,3)
double s = 2.0 * sqrt(1.0 + val);
w = (M(2, 1) - M(1, 2)) / s;
x = 0.25 * s;
y = (M(0, 1) + M(1, 0)) / s;
z = (M(0, 2) + M(2, 0)) / s;
break;
}
case 2: {
// % val = M(2,2) - M(1,1) - M(3,3)
double s = 2.0 * (sqrt(1.0 + val));
w = (M(0, 2) - M(2, 0)) / s;
x = (M(0, 1) + M(1, 0)) / s;
y = 0.25 * s;
z = (M(1, 2) + M(2, 1)) / s;
break;
}
default: {
// val = M(3,3) - M(2,2) - M(1,1)
double s = 2.0 * (sqrt(1.0 + val));
w = (M(1, 0) - M(0, 1)) / s;
x = (M(0, 2) + M(2, 0)) / s;
y = (M(1, 2) + M(2, 1)) / s;
z = 0.25 * s;
break;
}
}
Eigen::Matrix<typename Derived::Scalar, 4, 1> q;
q << w, x, y, z;
return q;
}
typename Gradient<Eigen::Matrix<typename DerivedR::Scalar, QUAT_SIZE, 1>, DerivedDR::ColsAtCompileTime>::type drotmat2quat(
const Eigen::MatrixBase<DerivedR>& R,
const Eigen::MatrixBase<DerivedDR>& dR)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Eigen::MatrixBase<DerivedR>, SPACE_DIMENSION, SPACE_DIMENSION);
EIGEN_STATIC_ASSERT(Eigen::MatrixBase<DerivedDR>::RowsAtCompileTime == RotmatSize, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
typedef typename DerivedR::Scalar Scalar;
typedef typename Gradient<Eigen::Matrix<Scalar, QUAT_SIZE, 1>, DerivedDR::ColsAtCompileTime>::type ReturnType;
const int nq = dR.cols();
auto dR11_dq = getSubMatrixGradient(dR, 0, 0, R.rows());
auto dR12_dq = getSubMatrixGradient(dR, 0, 1, R.rows());
auto dR13_dq = getSubMatrixGradient(dR, 0, 2, R.rows());
auto dR21_dq = getSubMatrixGradient(dR, 1, 0, R.rows());
auto dR22_dq = getSubMatrixGradient(dR, 1, 1, R.rows());
auto dR23_dq = getSubMatrixGradient(dR, 1, 2, R.rows());
auto dR31_dq = getSubMatrixGradient(dR, 2, 0, R.rows());
auto dR32_dq = getSubMatrixGradient(dR, 2, 1, R.rows());
auto dR33_dq = getSubMatrixGradient(dR, 2, 2, R.rows());
Matrix<Scalar, 4, 3> A;
A.row(0) << 1.0, 1.0, 1.0;
A.row(1.0) << 1.0, -1.0, -1.0;
A.row(2) << -1.0, 1.0, -1.0;
A.row(3) << -1.0, -1.0, 1.0;
Matrix<Scalar, 4, 1> B = A * R.diagonal();
typename Matrix<Scalar, 4, 1>::Index ind, max_col;
Scalar val = B.maxCoeff(&ind, &max_col);
ReturnType dq(QUAT_SIZE, nq);
using namespace std;
switch (ind) {
case 0: {
// val = trace(M)
auto dvaldq = dR11_dq + dR22_dq + dR33_dq;
auto dwdq = dvaldq / (4.0 * sqrt(1.0 + val));
auto w = sqrt(1.0 + val) / 2.0;
auto wsquare4 = 4.0 * w * w;
dq.row(0) = dwdq;
dq.row(1) = ((dR32_dq - dR23_dq) * w - (R(2, 1) - R(1, 2)) * dwdq) / wsquare4;
dq.row(2) = ((dR13_dq - dR31_dq) * w - (R(0, 2) - R(2, 0)) * dwdq) / wsquare4;
dq.row(3) = ((dR21_dq - dR12_dq) * w - (R(1, 0) - R(0, 1)) * dwdq) / wsquare4;
break;
}
case 1: {
// val = M(1,1) - M(2,2) - M(3,3)
auto dvaldq = dR11_dq - dR22_dq - dR33_dq;
auto s = 2.0 * sqrt(1.0 + val);
auto ssquare = s * s;
auto dsdq = dvaldq / sqrt(1.0 + val);
dq.row(0) = ((dR32_dq - dR23_dq) * s - (R(2, 1) - R(1, 2)) * dsdq) / ssquare;
dq.row(1) = .25 * dsdq;
dq.row(2) = ((dR12_dq + dR21_dq) * s - (R(0, 1) + R(1, 0)) * dsdq) / ssquare;
dq.row(3) = ((dR13_dq + dR31_dq) * s - (R(0, 2) + R(2, 0)) * dsdq) / ssquare;
break;
}
case 2: {
// val = M(2,2) - M(1,1) - M(3,3)
auto dvaldq = -dR11_dq + dR22_dq - dR33_dq;
auto s = 2.0 * (sqrt(1.0 + val));
auto ssquare = s * s;
auto dsdq = dvaldq / sqrt(1.0 + val);
dq.row(0) = ((dR13_dq - dR31_dq) * s - (R(0, 2) - R(2, 0)) * dsdq) / ssquare;
dq.row(1) = ((dR12_dq + dR21_dq) * s - (R(0, 1) + R(1, 0)) * dsdq) / ssquare;
dq.row(2) = .25 * dsdq;
dq.row(3) = ((dR23_dq + dR32_dq) * s - (R(1, 2) + R(2, 1)) * dsdq) / ssquare;
break;
}
default: {
// val = M(3,3) - M(2,2) - M(1,1)
auto dvaldq = -dR11_dq - dR22_dq + dR33_dq;
auto s = 2.0 * (sqrt(1.0 + val));
auto ssquare = s * s;
auto dsdq = dvaldq / sqrt(1.0 + val);
dq.row(0) = ((dR21_dq - dR12_dq) * s - (R(1, 0) - R(0, 1)) * dsdq) / ssquare;
dq.row(1) = ((dR13_dq + dR31_dq) * s - (R(0, 2) + R(2, 0)) * dsdq) / ssquare;
dq.row(2) = ((dR23_dq + dR32_dq) * s - (R(1, 2) + R(2, 1)) * dsdq) / ssquare;
dq.row(3) = .25 * dsdq;
break;
}
}
return dq;
}