rotation_matrix.hpp
Go to the documentation of this file.
1
4 #ifndef SOPHUS_ROTATION_MATRIX_HPP
5 #define SOPHUS_ROTATION_MATRIX_HPP
6
7 #include <Eigen/Dense>
8 #include <Eigen/SVD>
9
10 #include "types.hpp"
11
12 namespace Sophus {
13
16 template <class D>
17 SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase<D> const& R) {
18  using Scalar = typename D::Scalar;
19  static int const N = D::RowsAtCompileTime;
20  static int const M = D::ColsAtCompileTime;
21
22  static_assert(N == M, "must be a square matrix");
23  static_assert(N >= 2, "must have compile time dimension >= 2");
24
25  return (R * R.transpose() - Matrix<Scalar, N, N>::Identity()).norm() <
27 }
28
32 template <class D>
33 SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<D> const& sR) {
34  using Scalar = typename D::Scalar;
35  static int const N = D::RowsAtCompileTime;
36  static int const M = D::ColsAtCompileTime;
37  using std::pow;
38  using std::sqrt;
39
40  Scalar det = sR.determinant();
41
42  if (det <= Scalar(0)) {
43  return false;
44  }
45
46  Scalar scale_sqr = pow(det, Scalar(2. / N));
47
48  static_assert(N == M, "must be a square matrix");
49  static_assert(N >= 2, "must have compile time dimension >= 2");
50
51  return (sR * sR.transpose() - scale_sqr * Matrix<Scalar, N, N>::Identity())
52  .template lpNorm<Eigen::Infinity>() <
54 }
55
58 template <class D>
60  std::is_floating_point<typename D::Scalar>::value,
61  Matrix<typename D::Scalar, D::RowsAtCompileTime, D::RowsAtCompileTime>>
62 makeRotationMatrix(Eigen::MatrixBase<D> const& R) {
63  using Scalar = typename D::Scalar;
64  static int const N = D::RowsAtCompileTime;
65  static int const M = D::ColsAtCompileTime;
66
67  static_assert(N == M, "must be a square matrix");
68  static_assert(N >= 2, "must have compile time dimension >= 2");
69
70  Eigen::JacobiSVD<Matrix<Scalar, N, N>> svd(
71  R, Eigen::ComputeFullU | Eigen::ComputeFullV);
72
73  // Determine determinant of orthogonal matrix U*V'.
74  Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant();
75  // Starting from the identity matrix D, set the last entry to d (+1 or
76  // -1), so that det(U*D*V') = 1.
78  Diag(N - 1, N - 1) = d;
79  return svd.matrixU() * Diag * svd.matrixV().transpose();
80 }
81
82 } // namespace Sophus
83
84 #endif // SOPHUS_ROTATION_MATRIX_HPP
types.hpp
Sophus::enable_if_t
typename std::enable_if< B, T >::type enable_if_t
Definition: common.hpp:221
Sophus::Constants::epsilon
static SOPHUS_FUNC Scalar epsilon()
Definition: common.hpp:147
Sophus::Matrix
Eigen::Matrix< Scalar, M, N > Matrix
Definition: types.hpp:41
Sophus
Definition: average.hpp:17
SOPHUS_FUNC
#define SOPHUS_FUNC
Definition: common.hpp:37
Sophus::isOrthogonal
SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase< D > const &R)
Definition: rotation_matrix.hpp:17
Sophus::makeRotationMatrix
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)
Definition: rotation_matrix.hpp:62
Sophus::Constants
Definition: common.hpp:146
Sophus::isScaledOrthogonalAndPositive
SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase< D > const &sR)
Definition: rotation_matrix.hpp:33

sophus
Author(s): Hauke Strasdat
autogenerated on Wed Mar 2 2022 01:01:47