Beispiel #1
0
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;
}
Beispiel #2
0
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;
}