/// @file /// Rotation matrix helper functions. #ifndef SOPHUS_ROTATION_MATRIX_HPP #define SOPHUS_ROTATION_MATRIX_HPP #include <Eigen/Dense> #include <Eigen/SVD> #include "types.hpp" namespace Sophus { /// Takes in arbitrary square matrix and returns true if it is /// orthogonal. template <class D> SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase<D> const& R) { using Scalar = typename D::Scalar; static int const N = D::RowsAtCompileTime; static int const M = D::ColsAtCompileTime; static_assert(N == M, "must be a square matrix"); static_assert(N >= 2, "must have compile time dimension >= 2"); return (R * R.transpose() - Matrix<Scalar, N, N>::Identity()).norm() < Constants<Scalar>::epsilon(); } /// Takes in arbitrary square matrix and returns true if it is /// "scaled-orthogonal" with positive determinant. /// template <class D> SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<D> const& sR) { using Scalar = typename D::Scalar; static int const N = D::RowsAtCompileTime; static int const M = D::ColsAtCompileTime; using std::pow; using std::sqrt; Scalar det = sR.determinant(); if (det <= Scalar(0)) { return false; } Scalar scale_sqr = pow(det, Scalar(2. / N)); static_assert(N == M, "must be a square matrix"); static_assert(N >= 2, "must have compile time dimension >= 2"); return (sR * sR.transpose() - scale_sqr * Matrix<Scalar, N, N>::Identity()) .template lpNorm<Eigen::Infinity>() < sqrt(Constants<Scalar>::epsilon()); } /// Takes in arbitrary square matrix (2x2 or larger) and returns closest /// orthogonal matrix with positive determinant. template <class D> SOPHUS_FUNC enable_if_t< std::is_floating_point<typename D::Scalar>::value, Matrix<typename D::Scalar, D::RowsAtCompileTime, D::RowsAtCompileTime>> makeRotationMatrix(Eigen::MatrixBase<D> const& R) { using Scalar = typename D::Scalar; static int const N = D::RowsAtCompileTime; static int const M = D::ColsAtCompileTime; static_assert(N == M, "must be a square matrix"); static_assert(N >= 2, "must have compile time dimension >= 2"); Eigen::JacobiSVD<Matrix<Scalar, N, N>> svd( R, Eigen::ComputeFullU | Eigen::ComputeFullV); // Determine determinant of orthogonal matrix U*V'. Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant(); // Starting from the identity matrix D, set the last entry to d (+1 or // -1), so that det(U*D*V') = 1. Matrix<Scalar, N, N> Diag = Matrix<Scalar, N, N>::Identity(); Diag(N - 1, N - 1) = d; return svd.matrixU() * Diag * svd.matrixV().transpose(); } } // namespace Sophus #endif // SOPHUS_ROTATION_MATRIX_HPP