| Line |
Branch |
Exec |
Source |
| 1 |
|
|
// |
| 2 |
|
|
// Copyright (c) 2019-2020 CNRS INRIA |
| 3 |
|
|
// |
| 4 |
|
|
|
| 5 |
|
|
#ifndef __pinocchio_math_rotation_hpp__ |
| 6 |
|
|
#define __pinocchio_math_rotation_hpp__ |
| 7 |
|
|
|
| 8 |
|
|
#include "pinocchio/fwd.hpp" |
| 9 |
|
|
#include "pinocchio/math/matrix.hpp" |
| 10 |
|
|
#include "pinocchio/math/sincos.hpp" |
| 11 |
|
|
|
| 12 |
|
|
#include <Eigen/Core> |
| 13 |
|
|
#include <Eigen/Geometry> |
| 14 |
|
|
#include <Eigen/SVD> |
| 15 |
|
|
|
| 16 |
|
|
namespace pinocchio |
| 17 |
|
|
{ |
| 18 |
|
|
|
| 19 |
|
|
/// |
| 20 |
|
|
/// \brief Computes a rotation matrix from a vector and values of sin and cos |
| 21 |
|
|
/// orientations values. |
| 22 |
|
|
/// |
| 23 |
|
|
/// \remarks This code is issue from Eigen::AxisAngle::toRotationMatrix |
| 24 |
|
|
/// |
| 25 |
|
|
template<typename Vector3, typename Scalar, typename Matrix3> |
| 26 |
|
✗ |
void toRotationMatrix( |
| 27 |
|
|
const Eigen::MatrixBase<Vector3> & axis, |
| 28 |
|
|
const Scalar & cos_value, |
| 29 |
|
|
const Scalar & sin_value, |
| 30 |
|
|
const Eigen::MatrixBase<Matrix3> & res) |
| 31 |
|
|
{ |
| 32 |
|
|
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3, 3); |
| 33 |
|
|
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3); |
| 34 |
|
|
|
| 35 |
|
✗ |
assert(isUnitary(axis) && "The axis is not unitary."); |
| 36 |
|
|
|
| 37 |
|
✗ |
Matrix3 & res_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, res); |
| 38 |
|
✗ |
Vector3 sin_axis = sin_value * axis; |
| 39 |
|
✗ |
Vector3 cos1_axis = (Scalar(1) - cos_value) * axis; |
| 40 |
|
|
|
| 41 |
|
|
Scalar tmp; |
| 42 |
|
✗ |
tmp = cos1_axis.x() * axis.y(); |
| 43 |
|
✗ |
res_.coeffRef(0, 1) = tmp - sin_axis.z(); |
| 44 |
|
✗ |
res_.coeffRef(1, 0) = tmp + sin_axis.z(); |
| 45 |
|
|
|
| 46 |
|
✗ |
tmp = cos1_axis.x() * axis.z(); |
| 47 |
|
✗ |
res_.coeffRef(0, 2) = tmp + sin_axis.y(); |
| 48 |
|
✗ |
res_.coeffRef(2, 0) = tmp - sin_axis.y(); |
| 49 |
|
|
|
| 50 |
|
✗ |
tmp = cos1_axis.y() * axis.z(); |
| 51 |
|
✗ |
res_.coeffRef(1, 2) = tmp - sin_axis.x(); |
| 52 |
|
✗ |
res_.coeffRef(2, 1) = tmp + sin_axis.x(); |
| 53 |
|
|
|
| 54 |
|
✗ |
res_.diagonal() = (cos1_axis.cwiseProduct(axis)).array() + cos_value; |
| 55 |
|
|
} |
| 56 |
|
|
|
| 57 |
|
|
/// |
| 58 |
|
|
/// \brief Computes a rotation matrix from a vector and the angular value |
| 59 |
|
|
/// orientations values. |
| 60 |
|
|
/// |
| 61 |
|
|
/// \remarks This code is issue from Eigen::AxisAngle::toRotationMatrix |
| 62 |
|
|
/// |
| 63 |
|
|
template<typename Vector3, typename Scalar, typename Matrix3> |
| 64 |
|
✗ |
void toRotationMatrix( |
| 65 |
|
|
const Eigen::MatrixBase<Vector3> & axis, |
| 66 |
|
|
const Scalar & angle, |
| 67 |
|
|
const Eigen::MatrixBase<Matrix3> & res) |
| 68 |
|
|
{ |
| 69 |
|
|
Scalar sa, ca; |
| 70 |
|
✗ |
SINCOS(angle, &sa, &ca); |
| 71 |
|
✗ |
toRotationMatrix(axis, ca, sa, PINOCCHIO_EIGEN_CONST_CAST(Matrix3, res)); |
| 72 |
|
|
} |
| 73 |
|
|
|
| 74 |
|
|
/// |
| 75 |
|
|
/// \brief Orthogonormalization procedure for a rotation matrix (closed enough to SO(3)). |
| 76 |
|
|
/// |
| 77 |
|
|
/// \param[in,out] rot A 3x3 matrix to orthonormalize |
| 78 |
|
|
/// |
| 79 |
|
|
template<typename Matrix3> |
| 80 |
|
|
void normalizeRotation(const Eigen::MatrixBase<Matrix3> & rot) |
| 81 |
|
|
{ |
| 82 |
|
|
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3); |
| 83 |
|
|
Matrix3 & rot_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3, rot); |
| 84 |
|
|
|
| 85 |
|
|
typedef typename Matrix3::Scalar Scalar; |
| 86 |
|
|
enum |
| 87 |
|
|
{ |
| 88 |
|
|
Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options |
| 89 |
|
|
}; |
| 90 |
|
|
typedef Eigen::Quaternion<Scalar, Options> Quaternion; |
| 91 |
|
|
Quaternion quat(rot); |
| 92 |
|
|
normalize(quat.coeffs()); |
| 93 |
|
|
rot_ = quat.toRotationMatrix(); |
| 94 |
|
|
} |
| 95 |
|
|
|
| 96 |
|
|
/// |
| 97 |
|
|
/// \brief Orthogonal projection of a matrix on the SO(3) manifold. |
| 98 |
|
|
/// |
| 99 |
|
|
/// \param[in] mat A 3x3 matrix to project on SO(3). |
| 100 |
|
|
/// |
| 101 |
|
|
/// \returns the orthogonal projection of mat on SO(3) |
| 102 |
|
|
/// |
| 103 |
|
|
template<typename Matrix3> |
| 104 |
|
|
typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3) |
| 105 |
|
✗ |
orthogonalProjection(const Eigen::MatrixBase<Matrix3> & mat) |
| 106 |
|
|
{ |
| 107 |
|
|
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3, 3, 3); |
| 108 |
|
|
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3) ReturnType; |
| 109 |
|
|
|
| 110 |
|
|
typedef Eigen::JacobiSVD<Matrix3> SVD; |
| 111 |
|
✗ |
const SVD svd(mat, Eigen::ComputeFullU | Eigen::ComputeFullV); |
| 112 |
|
|
|
| 113 |
|
✗ |
ReturnType res; |
| 114 |
|
✗ |
res.template leftCols<2>().noalias() = |
| 115 |
|
✗ |
svd.matrixU() * svd.matrixV().transpose().template leftCols<2>(); |
| 116 |
|
✗ |
res.col(2).noalias() = res.col(0).cross(res.col(1)); |
| 117 |
|
✗ |
return res; |
| 118 |
|
|
} |
| 119 |
|
|
} // namespace pinocchio |
| 120 |
|
|
|
| 121 |
|
|
#endif // #ifndef __pinocchio_math_rotation_hpp__ |
| 122 |
|
|
|