85 lines
2.6 KiB
C++
85 lines
2.6 KiB
C++
/// @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
|