Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/spatial/explog-quaternion.hpp	Lines:	58	60	96.7 %
Date:	2023-08-09 08:43:58	Branches:	94	176	53.4 %


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

#ifndef __pinocchio_spatial_explog_quaternion_hpp__
#define __pinocchio_spatial_explog_quaternion_hpp__

#include "pinocchio/math/quaternion.hpp"
#include "pinocchio/spatial/explog.hpp"
#include "pinocchio/utils/static-if.hpp"

namespace pinocchio
{
  namespace quaternion
  {

    ///
    /// \brief Exp: so3 -> SO3 (quaternion)
    ///
    /// \returns the integral of the velocity vector as a queternion.
    ///
    /// \param[in] v The angular velocity vector.
    /// \param[out] qout The quanternion where the result is stored.
    ///
    template<typename Vector3Like, typename QuaternionLike>
    void exp3(const Eigen::MatrixBase<Vector3Like> & v,
              Eigen::QuaternionBase<QuaternionLike> & quat_out)
    {
      EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
      assert(v.size() == 3);

      typedef typename Vector3Like::Scalar Scalar;
      enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options };
      typedef Eigen::Quaternion<typename QuaternionLike::Scalar,Options> QuaternionPlain;

      const Scalar t2 = v.squaredNorm();
      const Scalar t = math::sqrt(t2);

      static const Scalar ts_prec = math::sqrt(Eigen::NumTraits<Scalar>::epsilon()); // Precision for the Taylor series expansion.

      Eigen::AngleAxis<Scalar> aa(t,v/t);
      QuaternionPlain quat_then(aa);

      QuaternionPlain quat_else;
      quat_else.vec() = (Scalar(1)/Scalar(2) - t2/48) * v;
      quat_else.w() = Scalar(1) - t2/8;

      using ::pinocchio::internal::if_then_else;
      for(Eigen::DenseIndex k = 0; k < 4; ++k)
      {
        quat_out.coeffs().coeffRef(k) = if_then_else(::pinocchio::internal::GT, t2, ts_prec,
                                                     quat_then.coeffs().coeffRef(k),
                                                     quat_else.coeffs().coeffRef(k));
      }

    }

    ///
    /// \brief Exp: so3 -> SO3 (quaternion)
    ///
    /// \returns the integral of the velocity vector as a queternion.
    ///
    /// \param[in] v The angular velocity vector.
    ///
    template<typename Vector3Like>
    Eigen::Quaternion<typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
    exp3(const Eigen::MatrixBase<Vector3Like> & v)
    {
      typedef Eigen::Quaternion<typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType;
      ReturnType res; exp3(v,res);
      return res;
    }

    ///
    /// \brief Same as \ref log3 but with a unit quaternion as input.
    ///
    /// \param[in] quat the unit quaternion.
    /// \param[out] theta the angle value (resuling from compurations).
    ///
    /// \return The angular velocity vector associated to the rotation matrix.
    ///
    template<typename QuaternionLike>
    Eigen::Matrix<typename QuaternionLike::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options>
    log3(const Eigen::QuaternionBase<QuaternionLike> & quat,
         typename QuaternionLike::Scalar & theta)
    {
      typedef typename QuaternionLike::Scalar Scalar;
      enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options };
      typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;

      Vector3 res;
      const Scalar norm_squared = quat.vec().squaredNorm();

      static const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
      static const Scalar ts_prec = TaylorSeriesExpansion<Scalar>::template precision<2>();
      const Scalar norm = math::sqrt(norm_squared + eps * eps);

      using ::pinocchio::internal::if_then_else;
      using ::pinocchio::internal::GE;
      using ::pinocchio::internal::LT;

      const Scalar pos_neg = if_then_else(GE, quat.w(), Scalar(0),
                                          Scalar(+1),
                                          Scalar(-1));

      Eigen::Quaternion<Scalar, Options> quat_pos;
      quat_pos.w() = pos_neg * quat.w();
      quat_pos.vec() = pos_neg * quat.vec();

      const Scalar theta_2 = math::atan2(norm,quat_pos.w());  // in [0,pi]
      const Scalar y_x = norm / quat_pos.w();  // nonnegative
      const Scalar y_x_sq = norm_squared / (quat_pos.w() * quat_pos.w());

      theta = if_then_else(LT, norm_squared, ts_prec,
                           Scalar(2.)*(Scalar(1) - y_x_sq / Scalar(3)) * y_x,
                           Scalar(2.)*theta_2);

      const Scalar th2_2 = theta * theta / Scalar(4);
      const Scalar inv_sinc = if_then_else(LT, norm_squared, ts_prec,
                                           Scalar(2) * (Scalar(1) + th2_2 / Scalar(6) + Scalar(7)/Scalar(360) * th2_2*th2_2),
                                           theta / math::sin(theta_2));

      for(Eigen::DenseIndex k = 0; k < 3; ++k)
      {
        // res[k] = if_then_else(LT, norm_squared, ts_prec,
        //                       Scalar(2) * (Scalar(1) + y_x_sq / Scalar(6) - y_x_sq*y_x_sq / Scalar(9)) * pos_neg * quat.vec()[k],
        //                       inv_sinc * pos_neg * quat.vec()[k]);
        res[k] = inv_sinc * quat_pos.vec()[k];
      }
      return res;
    }

    ///
    /// \brief Log: SO3 -> so3.
    ///
    /// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$.
    ///
    /// \param[in] quat The unit quaternion representing a certain rotation.
    ///
    /// \return The angular velocity vector associated to the quaternion.
    ///
    template<typename QuaternionLike>
    Eigen::Matrix<typename QuaternionLike::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options>
    log3(const Eigen::QuaternionBase<QuaternionLike> & quat)
    {
      typename QuaternionLike::Scalar theta;
      return log3(quat.derived(),theta);
    }

    ///
    /// \brief Derivative of \f$ q = \exp{\mathbf{v} + \delta\mathbf{v}} \f$ where \f$ \delta\mathbf{v} \f$
    ///        is a small perturbation of \f$ \mathbf{v} \f$ at identity.
    ///
    /// \returns The Jacobian of the quaternion components variation.
    ///
    template<typename Vector3Like, typename Matrix43Like>
    void Jexp3CoeffWise(const Eigen::MatrixBase<Vector3Like> & v,
                        const Eigen::MatrixBase<Matrix43Like> & Jexp)
    {
//      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix43Like,4,3);
      assert(Jexp.rows() == 4 && Jexp.cols() == 3 && "Jexp does have the right size.");
      Matrix43Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix43Like,Jexp);

      typedef typename Vector3Like::Scalar Scalar;

      const Scalar n2 = v.squaredNorm();
      const Scalar n = math::sqrt(n2);
      const Scalar theta = Scalar(0.5) * n;
      const Scalar theta2 = Scalar(0.25) * n2;

      if(n2 > math::sqrt(Eigen::NumTraits<Scalar>::epsilon()))
      {
        Scalar c, s;
        SINCOS(theta,&s,&c);
        Jout.template topRows<3>().noalias() = ((Scalar(0.5)/n2) * (c - 2*s/n)) * v * v.transpose();
        Jout.template topRows<3>().diagonal().array() += s/n;
        Jout.template bottomRows<1>().noalias() = -s/(2*n) * v.transpose();
      }
      else
      {
        Jout.template topRows<3>().noalias() =  (-Scalar(1)/Scalar(12) + n2/Scalar(480)) * v * v.transpose();
        Jout.template topRows<3>().diagonal().array() += Scalar(0.5) * (1 - theta2/6);
        Jout.template bottomRows<1>().noalias() = (Scalar(-0.25) * (Scalar(1) - theta2/6)) * v.transpose();

      }
    }

    ///
    /// \brief Computes the Jacobian of log3 operator for a unit quaternion.
    ///
    /// \param[in] quat A unit quaternion representing the input rotation.
    /// \param[out] Jlog The resulting Jacobian of the log operator.
    ///
    template<typename QuaternionLike, typename Matrix3Like>
    void Jlog3(const Eigen::QuaternionBase<QuaternionLike> & quat,
               const Eigen::MatrixBase<Matrix3Like> & Jlog)
    {
      typedef typename QuaternionLike::Scalar Scalar;
      typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options> Vector3;

      Scalar t;
      Vector3 w(log3(quat,t));
      pinocchio::Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog));
    }
  } // namespace quaternion
}

#endif // ifndef __pinocchio_spatial_explog_quaternion_hpp__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2018-2020 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_spatial_explog_quaternion_hpp__
6			#define __pinocchio_spatial_explog_quaternion_hpp__
7
8			#include "pinocchio/math/quaternion.hpp"
9			#include "pinocchio/spatial/explog.hpp"
10			#include "pinocchio/utils/static-if.hpp"
11
12			namespace pinocchio
13			{
14			namespace quaternion
15			{
16
17			///
18			/// \brief Exp: so3 -> SO3 (quaternion)
19			///
20			/// \returns the integral of the velocity vector as a queternion.
21			///
22			/// \param[in] v The angular velocity vector.
23			/// \param[out] qout The quanternion where the result is stored.
24			///
25			template<typename Vector3Like, typename QuaternionLike>
26		12969	void exp3(const Eigen::MatrixBase<Vector3Like> & v,
27			Eigen::QuaternionBase<QuaternionLike> & quat_out)
28			{
29			EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
30	✓✗✗✓	12969	assert(v.size() == 3);
31
32			typedef typename Vector3Like::Scalar Scalar;
33			enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options };
34			typedef Eigen::Quaternion<typename QuaternionLike::Scalar,Options> QuaternionPlain;
35
36	✓✗	12969	const Scalar t2 = v.squaredNorm();
37		12969	const Scalar t = math::sqrt(t2);
38
39	✓✓✓✗	12969	static const Scalar ts_prec = math::sqrt(Eigen::NumTraits<Scalar>::epsilon()); // Precision for the Taylor series expansion.
40
41	✓✗✓✗	12969	Eigen::AngleAxis<Scalar> aa(t,v/t);
42	✓✗	12969	QuaternionPlain quat_then(aa);
43
44	✓✗	12969	QuaternionPlain quat_else;
45	✓✗✓✗ ✓✗	12969	quat_else.vec() = (Scalar(1)/Scalar(2) - t2/48) * v;
46	✓✗	12969	quat_else.w() = Scalar(1) - t2/8;
47
48			using ::pinocchio::internal::if_then_else;
49	✓✓	64845	for(Eigen::DenseIndex k = 0; k < 4; ++k)
50			{
51	✓✗✓✓ ✓✗✓✗ ✓✗✓✗	103752	quat_out.coeffs().coeffRef(k) = if_then_else(::pinocchio::internal::GT, t2, ts_prec,
52		51876	quat_then.coeffs().coeffRef(k),
53		51876	quat_else.coeffs().coeffRef(k));
54			}
55
56		12969	}
57
58			///
59			/// \brief Exp: so3 -> SO3 (quaternion)
60			///
61			/// \returns the integral of the velocity vector as a queternion.
62			///
63			/// \param[in] v The angular velocity vector.
64			///
65			template<typename Vector3Like>
66			Eigen::Quaternion<typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
67			exp3(const Eigen::MatrixBase<Vector3Like> & v)
68			{
69			typedef Eigen::Quaternion<typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> ReturnType;
70			ReturnType res; exp3(v,res);
71			return res;
72			}
73
74			///
75			/// \brief Same as \ref log3 but with a unit quaternion as input.
76			///
77			/// \param[in] quat the unit quaternion.
78			/// \param[out] theta the angle value (resuling from compurations).
79			///
80			/// \return The angular velocity vector associated to the rotation matrix.
81			///
82			template<typename QuaternionLike>
83			Eigen::Matrix<typename QuaternionLike::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options>
84		12438	log3(const Eigen::QuaternionBase<QuaternionLike> & quat,
85			typename QuaternionLike::Scalar & theta)
86			{
87			typedef typename QuaternionLike::Scalar Scalar;
88			enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options };
89			typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
90
91	✓✗	12438	Vector3 res;
92	✓✗✓✗	12438	const Scalar norm_squared = quat.vec().squaredNorm();
93
94	✓✓✓✗	12438	static const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
95	✓✓✓✗ ✓✗✗✗	12438	static const Scalar ts_prec = TaylorSeriesExpansion<Scalar>::template precision<2>();
96		12438	const Scalar norm = math::sqrt(norm_squared + eps * eps);
97
98			using ::pinocchio::internal::if_then_else;
99			using ::pinocchio::internal::GE;
100			using ::pinocchio::internal::LT;
101
102	✓✗✓✗	12438	const Scalar pos_neg = if_then_else(GE, quat.w(), Scalar(0),
103			Scalar(+1),
104			Scalar(-1));
105
106	✓✗	12438	Eigen::Quaternion<Scalar, Options> quat_pos;
107	✓✗✓✗	12438	quat_pos.w() = pos_neg * quat.w();
108	✓✗✓✗ ✓✗✓✗	12438	quat_pos.vec() = pos_neg * quat.vec();
109
110	✓✗✓✗	12438	const Scalar theta_2 = math::atan2(norm,quat_pos.w()); // in [0,pi]
111	✓✗	12438	const Scalar y_x = norm / quat_pos.w(); // nonnegative
112	✓✗✓✗	12438	const Scalar y_x_sq = norm_squared / (quat_pos.w() * quat_pos.w());
113
114		24876	theta = if_then_else(LT, norm_squared, ts_prec,
115			Scalar(2.)(Scalar(1) - y_x_sq / Scalar(3)) y_x,
116	✓✗	12438	Scalar(2.)*theta_2);
117
118		12438	const Scalar th2_2 = theta * theta / Scalar(4);
119		24876	const Scalar inv_sinc = if_then_else(LT, norm_squared, ts_prec,
120			Scalar(2) * (Scalar(1) + th2_2 / Scalar(6) + Scalar(7)/Scalar(360) * th2_2*th2_2),
121	✓✗	12438	theta / math::sin(theta_2));
122
123	✓✓	49752	for(Eigen::DenseIndex k = 0; k < 3; ++k)
124			{
125			// res[k] = if_then_else(LT, norm_squared, ts_prec,
126			// Scalar(2) * (Scalar(1) + y_x_sq / Scalar(6) - y_x_sqy_x_sq / Scalar(9)) pos_neg * quat.vec()[k],
127			// inv_sinc * pos_neg * quat.vec()[k]);
128	✓✗✓✗ ✓✗	37314	res[k] = inv_sinc * quat_pos.vec()[k];
129			}
130		24876	return res;
131			}
132
133			///
134			/// \brief Log: SO3 -> so3.
135			///
136			/// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, \|\|v\|\| \le pi } \f$.
137			///
138			/// \param[in] quat The unit quaternion representing a certain rotation.
139			///
140			/// \return The angular velocity vector associated to the quaternion.
141			///
142			template<typename QuaternionLike>
143			Eigen::Matrix<typename QuaternionLike::Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options>
144		4996	log3(const Eigen::QuaternionBase<QuaternionLike> & quat)
145			{
146			typename QuaternionLike::Scalar theta;
147	✓✗	9992	return log3(quat.derived(),theta);
148			}
149
150			///
151			/// \brief Derivative of \f$ q = \exp{\mathbf{v} + \delta\mathbf{v}} \f$ where \f$ \delta\mathbf{v} \f$
152			/// is a small perturbation of \f$ \mathbf{v} \f$ at identity.
153			///
154			/// \returns The Jacobian of the quaternion components variation.
155			///
156			template<typename Vector3Like, typename Matrix43Like>
157		64	void Jexp3CoeffWise(const Eigen::MatrixBase<Vector3Like> & v,
158			const Eigen::MatrixBase<Matrix43Like> & Jexp)
159			{
160			// EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix43Like,4,3);
161	✓✗✓✗ ✓✗✓✗	64	assert(Jexp.rows() == 4 && Jexp.cols() == 3 && "Jexp does have the right size.");
162		64	Matrix43Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix43Like,Jexp);
163
164			typedef typename Vector3Like::Scalar Scalar;
165
166	✓✗	64	const Scalar n2 = v.squaredNorm();
167		64	const Scalar n = math::sqrt(n2);
168		64	const Scalar theta = Scalar(0.5) * n;
169		64	const Scalar theta2 = Scalar(0.25) * n2;
170
171	✓✓	64	if(n2 > math::sqrt(Eigen::NumTraits<Scalar>::epsilon()))
172			{
173			Scalar c, s;
174		63	SINCOS(theta,&s,&c);
175	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	63	Jout.template topRows<3>().noalias() = ((Scalar(0.5)/n2) * (c - 2s/n)) v * v.transpose();
176	✓✗✓✗ ✓✗✓✗	63	Jout.template topRows<3>().diagonal().array() += s/n;
177	✓✗✓✗ ✓✗✓✗ ✓✗	63	Jout.template bottomRows<1>().noalias() = -s/(2n) v.transpose();
178			}
179			else
180			{
181	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	1	Jout.template topRows<3>().noalias() = (-Scalar(1)/Scalar(12) + n2/Scalar(480)) * v * v.transpose();
182	✓✗✓✗ ✓✗✓✗	1	Jout.template topRows<3>().diagonal().array() += Scalar(0.5) * (1 - theta2/6);
183	✓✗✓✗ ✓✗✓✗ ✓✗	1	Jout.template bottomRows<1>().noalias() = (Scalar(-0.25) * (Scalar(1) - theta2/6)) * v.transpose();
184
185			}
186		64	}
187
188			///
189			/// \brief Computes the Jacobian of log3 operator for a unit quaternion.
190			///
191			/// \param[in] quat A unit quaternion representing the input rotation.
192			/// \param[out] Jlog The resulting Jacobian of the log operator.
193			///
194			template<typename QuaternionLike, typename Matrix3Like>
195		341	void Jlog3(const Eigen::QuaternionBase<QuaternionLike> & quat,
196			const Eigen::MatrixBase<Matrix3Like> & Jlog)
197			{
198			typedef typename QuaternionLike::Scalar Scalar;
199			typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options> Vector3;
200
201			Scalar t;
202	✓✗	341	Vector3 w(log3(quat,t));
203	✓✗	341	pinocchio::Jlog3(t,w,PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog));
204		341	}
205			} // namespace quaternion
206			}
207
208			#endif // ifndef __pinocchio_spatial_explog_quaternion_hpp__