Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/math/rotation.hpp	Lines:	22	22	100.0 %
Date:	2024-04-26 13:14:21	Branches:	46	92	50.0 %


//
// Copyright (c) 2019-2020 CNRS INRIA
//

#ifndef __pinocchio_math_rotation_hpp__
#define __pinocchio_math_rotation_hpp__

#include "pinocchio/fwd.hpp"
#include "pinocchio/math/matrix.hpp"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>

namespace pinocchio
{
  ///
  /// \brief Computes a rotation matrix from a vector and values of sin and cos
  ///        orientations values.
  ///
  /// \remarks This code is issue from Eigen::AxisAngle::toRotationMatrix
  ///
  template<typename Vector3, typename Scalar, typename Matrix3>
  void toRotationMatrix(const Eigen::MatrixBase<Vector3> & axis,
                        const Scalar & cos_value, const Scalar & sin_value,
                        const Eigen::MatrixBase<Matrix3> & res)
  {
    EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);

    assert(isUnitary(axis) && "The axis is not unitary.");

    Matrix3 & res_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,res);
    Vector3 sin_axis  = sin_value * axis;
    Vector3 cos1_axis = (Scalar(1)-cos_value) * axis;

    Scalar tmp;
    tmp = cos1_axis.x() * axis.y();
    res_.coeffRef(0,1) = tmp - sin_axis.z();
    res_.coeffRef(1,0) = tmp + sin_axis.z();

    tmp = cos1_axis.x() * axis.z();
    res_.coeffRef(0,2) = tmp + sin_axis.y();
    res_.coeffRef(2,0) = tmp - sin_axis.y();

    tmp = cos1_axis.y() * axis.z();
    res_.coeffRef(1,2) = tmp - sin_axis.x();
    res_.coeffRef(2,1) = tmp + sin_axis.x();

    res_.diagonal() = (cos1_axis.cwiseProduct(axis)).array() + cos_value;
  }

  ///
  /// \brief Orthogonormalization procedure for a rotation matrix (closed enough to SO(3)).
  ///
  /// \param[in,out] rot A 3x3 matrix to orthonormalize
  ///
  template<typename Matrix3>
  void normalizeRotation(const Eigen::MatrixBase<Matrix3> & rot)
  {
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
    Matrix3 & rot_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,rot);

    typedef typename Matrix3::Scalar Scalar;
    enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options };
    typedef Eigen::Quaternion<Scalar,Options> Quaternion;
    Quaternion quat(rot); quat.normalize();
    rot_ = quat.toRotationMatrix();
  }

  ///
  /// \brief Orthogonal projection of a matrix on the SO(3) manifold.
  ///
  /// \param[in] mat A 3x3 matrix to project on SO(3).
  ///
  /// \returns the orthogonal projection of mat on SO(3)
  ///
  template<typename Matrix3>
  typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)
  orthogonalProjection(const Eigen::MatrixBase<Matrix3> & mat)
  {
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
    typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3) ReturnType;

    typedef Eigen::JacobiSVD<Matrix3> SVD;
    const SVD svd(mat,Eigen::ComputeFullU | Eigen::ComputeFullV);

    ReturnType res;
    res.template leftCols<2>().noalias() = svd.matrixU() * svd.matrixV().transpose().template leftCols<2>();
    res.col(2).noalias() = res.col(0).cross(res.col(1));
    return res;
  }
}

#endif //#ifndef __pinocchio_math_rotation_hpp__



Generated by: GCOVR (Version 4.2)

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
11			#include <Eigen/Core>
12			#include <Eigen/Geometry>
13			#include <Eigen/SVD>
14
15			namespace pinocchio
16			{
17			///
18			/// \brief Computes a rotation matrix from a vector and values of sin and cos
19			/// orientations values.
20			///
21			/// \remarks This code is issue from Eigen::AxisAngle::toRotationMatrix
22			///
23			template<typename Vector3, typename Scalar, typename Matrix3>
24		100016	void toRotationMatrix(const Eigen::MatrixBase<Vector3> & axis,
25			const Scalar & cos_value, const Scalar & sin_value,
26			const Eigen::MatrixBase<Matrix3> & res)
27			{
28			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3,3);
29			EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
30
31	✓✗✓✗	100016	assert(isUnitary(axis) && "The axis is not unitary.");
32
33		100016	Matrix3 & res_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,res);
34	✓✗✓✗	100016	Vector3 sin_axis = sin_value * axis;
35	✓✗✓✗	100016	Vector3 cos1_axis = (Scalar(1)-cos_value) * axis;
36
37			Scalar tmp;
38	✓✗✓✗	100016	tmp = cos1_axis.x() * axis.y();
39	✓✗✓✗	100016	res_.coeffRef(0,1) = tmp - sin_axis.z();
40	✓✗✓✗	100016	res_.coeffRef(1,0) = tmp + sin_axis.z();
41
42	✓✗✓✗	100016	tmp = cos1_axis.x() * axis.z();
43	✓✗✓✗	100016	res_.coeffRef(0,2) = tmp + sin_axis.y();
44	✓✗✓✗	100016	res_.coeffRef(2,0) = tmp - sin_axis.y();
45
46	✓✗✓✗	100016	tmp = cos1_axis.y() * axis.z();
47	✓✗✓✗	100016	res_.coeffRef(1,2) = tmp - sin_axis.x();
48	✓✗✓✗	100016	res_.coeffRef(2,1) = tmp + sin_axis.x();
49
50	✓✗✓✗ ✓✗✓✗ ✓✗	100016	res_.diagonal() = (cos1_axis.cwiseProduct(axis)).array() + cos_value;
51		100016	}
52
53			///
54			/// \brief Orthogonormalization procedure for a rotation matrix (closed enough to SO(3)).
55			///
56			/// \param[in,out] rot A 3x3 matrix to orthonormalize
57			///
58			template<typename Matrix3>
59			void normalizeRotation(const Eigen::MatrixBase<Matrix3> & rot)
60			{
61			EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
62			Matrix3 & rot_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3,rot);
63
64			typedef typename Matrix3::Scalar Scalar;
65			enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)::Options };
66			typedef Eigen::Quaternion<Scalar,Options> Quaternion;
67			Quaternion quat(rot); quat.normalize();
68			rot_ = quat.toRotationMatrix();
69			}
70
71			///
72			/// \brief Orthogonal projection of a matrix on the SO(3) manifold.
73			///
74			/// \param[in] mat A 3x3 matrix to project on SO(3).
75			///
76			/// \returns the orthogonal projection of mat on SO(3)
77			///
78			template<typename Matrix3>
79			typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3)
80		200172	orthogonalProjection(const Eigen::MatrixBase<Matrix3> & mat)
81			{
82			EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3,3,3);
83			typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3) ReturnType;
84
85			typedef Eigen::JacobiSVD<Matrix3> SVD;
86	✓✗✓✗	200172	const SVD svd(mat,Eigen::ComputeFullU \| Eigen::ComputeFullV);
87
88	✓✗	200172	ReturnType res;
89	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	200172	res.template leftCols<2>().noalias() = svd.matrixU() * svd.matrixV().transpose().template leftCols<2>();
90	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	200172	res.col(2).noalias() = res.col(0).cross(res.col(1));
91		400344	return res;
92			}
93			}
94
95			#endif //#ifndef __pinocchio_math_rotation_hpp__
96