Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/spatial/log.hxx	Lines:	86	88	97.7 %
Date:	2024-04-26 13:14:21	Branches:	120	225	53.3 %


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

#ifndef __pinocchio_spatial_log_hxx__
#define __pinocchio_spatial_log_hxx__

namespace pinocchio
{
  /// \brief Generic evaluation of log3 function
  template<typename _Scalar>
  struct log3_impl
  {
    template<typename Matrix3Like, typename Vector3Out>
    static void run(const Eigen::MatrixBase<Matrix3Like> & R,
                    typename Matrix3Like::Scalar & theta,
                    const Eigen::MatrixBase<Vector3Out> & res)
    {
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Out, res, 3, 1);
      // TODO: add static_assert<is_same_type<_Scalar,Scalar>

      typedef typename Matrix3Like::Scalar Scalar;
      typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;

      static const Scalar PI_value = PI<Scalar>();

      Scalar tr = R.trace();
      if(tr >= Scalar(3))
      {
        tr = Scalar(3); // clip value
        theta = Scalar(0); // acos((3-1)/2)
      }
      else if(tr <= Scalar(-1))
      {
        tr = Scalar(-1); // clip value
        theta = PI_value; // acos((-1-1)/2)
      }
      else
        theta = math::acos((tr - Scalar(1))/Scalar(2));
      assert(theta == theta && "theta contains some NaN"); // theta != NaN

      Vector3Out & res_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3Out,res);

      // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
      if(theta >= PI_value - 1e-2)
      {
        // 1e-2: A low value is not required since the computation is
        // using explicit formula. However, the precision of this method
        // is the square root of the precision with the antisymmetric
        // method (Nominal case).
        const Scalar cphi = -(tr - Scalar(1))/Scalar(2);
        const Scalar beta = theta*theta / (Scalar(1) + cphi);
        const Vector3 tmp((R.diagonal().array() + cphi) * beta);
        res_(0) = (R (2, 1) > R (1, 2) ? Scalar(1) : Scalar(-1)) * (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0));
        res_(1) = (R (0, 2) > R (2, 0) ? Scalar(1) : Scalar(-1)) * (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0));
        res_(2) = (R (1, 0) > R (0, 1) ? Scalar(1) : Scalar(-1)) * (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0));
      }
      else
      {
        const Scalar t = ((theta > TaylorSeriesExpansion<Scalar>::template precision<3>())
                          ? theta / sin(theta)
                          : Scalar(1)) / Scalar(2);
        res_(0) = t * (R (2, 1) - R (1, 2));
        res_(1) = t * (R (0, 2) - R (2, 0));
        res_(2) = t * (R (1, 0) - R (0, 1));
      }
    }
  };

  /// \brief Generic evaluation of Jlog3 function
  template<typename _Scalar>
  struct Jlog3_impl
  {
    template<typename Scalar, typename Vector3Like, typename Matrix3Like>
    static void run(const Scalar & theta,
                    const Eigen::MatrixBase<Vector3Like> & log,
                    const Eigen::MatrixBase<Matrix3Like> & Jlog)
    {
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like,  log, 3, 1);
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, Jlog, 3, 3);
      // TODO: add static_assert<is_same_type<_Scalar,Scalar>

      Matrix3Like & Jlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog);

      Scalar alpha, diag_value;
      if(theta < TaylorSeriesExpansion<Scalar>::template precision<3>())
      {
        alpha = Scalar(1)/Scalar(12) + theta*theta / Scalar(720);
        diag_value = Scalar(0.5) * (2 - theta*theta / Scalar(6));
      }
      else
      {
        // Jlog = alpha I
        Scalar ct,st; SINCOS(theta,&st,&ct);
        const Scalar st_1mct = st/(Scalar(1)-ct);

        alpha = Scalar(1)/(theta*theta) - st_1mct/(Scalar(2)*theta);
        diag_value = Scalar(0.5) * (theta * st_1mct);
      }

      Jlog_.noalias() = alpha * log * log.transpose();
      Jlog_.diagonal().array() += diag_value;

      // Jlog += r_{\times}/2
      addSkew(Scalar(0.5) * log, Jlog_);
    }
  };

  /// \brief Generic evaluation of log6 function
  template<typename _Scalar>
  struct log6_impl
  {
    template<typename Scalar, int Options, typename MotionDerived>
    static void run(const SE3Tpl<Scalar,Options> & M,
                    MotionDense<MotionDerived> & mout)
    {
      typedef SE3Tpl<Scalar,Options> SE3;
      typedef typename SE3::Vector3 Vector3;

      typename SE3::ConstAngularRef R = M.rotation();
      typename SE3::ConstLinearRef p = M.translation();

      Scalar t;
      Vector3 w(log3(R,t)); // t in [0,π]
      const Scalar t2 = t*t;
      Scalar alpha, beta;

      if (t < TaylorSeriesExpansion<Scalar>::template precision<3>())
      {
        alpha = Scalar(1) - t2/Scalar(12) - t2*t2/Scalar(720);
        beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
      }
      else
      {
        Scalar st,ct; SINCOS(t,&st,&ct);
        alpha = t*st/(Scalar(2)*(Scalar(1)-ct));
        beta = Scalar(1)/t2 - st/(Scalar(2)*t*(Scalar(1)-ct));
      }

      mout.linear().noalias() = alpha * p - Scalar(0.5) * w.cross(p) + (beta * w.dot(p)) * w;
      mout.angular() = w;
    }
  };

  template<typename _Scalar>
  struct Jlog6_impl
  {
    template<typename Scalar, int Options, typename Matrix6Like>
    static void run(const SE3Tpl<Scalar, Options> & M,
                    const Eigen::MatrixBase<Matrix6Like> & Jlog)
    {
      PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like, Jlog, 6, 6);
      // TODO: add static_assert<is_same_type<_Scalar,Scalar>

      typedef SE3Tpl<Scalar,Options> SE3;
      typedef typename SE3::Vector3 Vector3;
      Matrix6Like & value = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jlog);

      typename SE3::ConstAngularRef R = M.rotation();
      typename SE3::ConstLinearRef p = M.translation();

      Scalar t;
      Vector3 w(log3(R,t));

      // value is decomposed as following:
      // value = [ A, B;
      //           C, D ]
      typedef Eigen::Block<Matrix6Like,3,3> Block33;
      Block33 A = value.template topLeftCorner<3,3>();
      Block33 B = value.template topRightCorner<3,3>();
      Block33 C = value.template bottomLeftCorner<3,3>();
      Block33 D = value.template bottomRightCorner<3,3>();

      Jlog3(t, w, A);
      D = A;

      const Scalar t2 = t*t;
      Scalar beta, beta_dot_over_theta;
      if(t < TaylorSeriesExpansion<Scalar>::template precision<3>())
      {
        beta                = Scalar(1)/Scalar(12) + t2/Scalar(720);
        beta_dot_over_theta = Scalar(1)/Scalar(360);
      }
      else
      {
        const Scalar tinv = Scalar(1)/t,
                     t2inv = tinv*tinv;
        Scalar st,ct; SINCOS (t, &st, &ct);
        const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));

        beta = t2inv - st*tinv*inv_2_2ct;
        beta_dot_over_theta = -Scalar(2)*t2inv*t2inv +
          (Scalar(1) + st*tinv) * t2inv * inv_2_2ct;
      }

      Scalar wTp = w.dot(p);

      Vector3 v3_tmp((beta_dot_over_theta*wTp)*w - (t2*beta_dot_over_theta+Scalar(2)*beta)*p);
      // C can be treated as a temporary variable
      C.noalias() = v3_tmp * w.transpose();
      C.noalias() += beta * w * p.transpose();
      C.diagonal().array() += wTp * beta;
      addSkew(Scalar(.5)*p,C);

      B.noalias() = C * A;
      C.setZero();
    }
  };

} // namespace pinocchio

#endif // ifndef __pinocchio_spatial_log_hxx__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2020 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_spatial_log_hxx__
6			#define __pinocchio_spatial_log_hxx__
7
8			namespace pinocchio
9			{
10			/// \brief Generic evaluation of log3 function
11			template<typename _Scalar>
12			struct log3_impl
13			{
14			template<typename Matrix3Like, typename Vector3Out>
15		21810	static void run(const Eigen::MatrixBase<Matrix3Like> & R,
16			typename Matrix3Like::Scalar & theta,
17			const Eigen::MatrixBase<Vector3Out> & res)
18			{
19	✓✗✓✗	21810	PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, R, 3, 3);
20	✓✗✓✗	21810	PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Out, res, 3, 1);
21			// TODO: add static_assert<is_same_type<_Scalar,Scalar>
22
23			typedef typename Matrix3Like::Scalar Scalar;
24			typedef Eigen::Matrix<Scalar,3,1,PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3Like)::Options> Vector3;
25
26	✓✓✓✗	21810	static const Scalar PI_value = PI<Scalar>();
27
28		21810	Scalar tr = R.trace();
29	✓✓	21810	if(tr >= Scalar(3))
30			{
31		12557	tr = Scalar(3); // clip value
32		12557	theta = Scalar(0); // acos((3-1)/2)
33			}
34	✗✓	9253	else if(tr <= Scalar(-1))
35			{
36			tr = Scalar(-1); // clip value
37			theta = PI_value; // acos((-1-1)/2)
38			}
39			else
40		9253	theta = math::acos((tr - Scalar(1))/Scalar(2));
41	✗✓	21810	assert(theta == theta && "theta contains some NaN"); // theta != NaN
42
43		21810	Vector3Out & res_ = PINOCCHIO_EIGEN_CONST_CAST(Vector3Out,res);
44
45			// From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
46	✓✓	21810	if(theta >= PI_value - 1e-2)
47			{
48			// 1e-2: A low value is not required since the computation is
49			// using explicit formula. However, the precision of this method
50			// is the square root of the precision with the antisymmetric
51			// method (Nominal case).
52		3	const Scalar cphi = -(tr - Scalar(1))/Scalar(2);
53		3	const Scalar beta = theta*theta / (Scalar(1) + cphi);
54	✓✗✓✗ ✓✗✓✗ ✓✗	3	const Vector3 tmp((R.diagonal().array() + cphi) * beta);
55	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	3	res_(0) = (R (2, 1) > R (1, 2) ? Scalar(1) : Scalar(-1)) * (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0));
56	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	3	res_(1) = (R (0, 2) > R (2, 0) ? Scalar(1) : Scalar(-1)) * (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0));
57	✓✗✓✗ ✗✓✓✗ ✓✗✓✗ ✓✗	3	res_(2) = (R (1, 0) > R (0, 1) ? Scalar(1) : Scalar(-1)) * (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0));
58			}
59			else
60			{
61		21807	const Scalar t = ((theta > TaylorSeriesExpansion<Scalar>::template precision<3>())
62	✓✓	21807	? theta / sin(theta)
63			: Scalar(1)) / Scalar(2);
64		21807	res_(0) = t * (R (2, 1) - R (1, 2));
65		21807	res_(1) = t * (R (0, 2) - R (2, 0));
66		21807	res_(2) = t * (R (1, 0) - R (0, 1));
67			}
68		21810	}
69			};
70
71			/// \brief Generic evaluation of Jlog3 function
72			template<typename _Scalar>
73			struct Jlog3_impl
74			{
75			template<typename Scalar, typename Vector3Like, typename Matrix3Like>
76		992	static void run(const Scalar & theta,
77			const Eigen::MatrixBase<Vector3Like> & log,
78			const Eigen::MatrixBase<Matrix3Like> & Jlog)
79			{
80	✓✗✓✗ ✓✗✓✗	992	PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector3Like, log, 3, 1);
81	✓✗✓✓ ✗✓✗✓ ✗	992	PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, Jlog, 3, 3);
82			// TODO: add static_assert<is_same_type<_Scalar,Scalar>
83
84		992	Matrix3Like & Jlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog);
85
86			Scalar alpha, diag_value;
87	✓✗✓✓	992	if(theta < TaylorSeriesExpansion<Scalar>::template precision<3>())
88			{
89		110	alpha = Scalar(1)/Scalar(12) + theta*theta / Scalar(720);
90		110	diag_value = Scalar(0.5) * (2 - theta*theta / Scalar(6));
91			}
92			else
93			{
94			// Jlog = alpha I
95		882	Scalar ct,st; SINCOS(theta,&st,&ct);
96		882	const Scalar st_1mct = st/(Scalar(1)-ct);
97
98		882	alpha = Scalar(1)/(thetatheta) - st_1mct/(Scalar(2)theta);
99		882	diag_value = Scalar(0.5) * (theta * st_1mct);
100			}
101
102	✓✗✓✗ ✓✗✓✗ ✓✗	992	Jlog_.noalias() = alpha * log * log.transpose();
103	✓✗✓✗ ✓✗	992	Jlog_.diagonal().array() += diag_value;
104
105			// Jlog += r_{\times}/2
106	✓✗✓✗	992	addSkew(Scalar(0.5) * log, Jlog_);
107		992	}
108			};
109
110			/// \brief Generic evaluation of log6 function
111			template<typename _Scalar>
112			struct log6_impl
113			{
114			template<typename Scalar, int Options, typename MotionDerived>
115		19397	static void run(const SE3Tpl<Scalar,Options> & M,
116			MotionDense<MotionDerived> & mout)
117			{
118			typedef SE3Tpl<Scalar,Options> SE3;
119			typedef typename SE3::Vector3 Vector3;
120
121	✓✗	19397	typename SE3::ConstAngularRef R = M.rotation();
122	✓✗	19397	typename SE3::ConstLinearRef p = M.translation();
123
124			Scalar t;
125	✓✗	19397	Vector3 w(log3(R,t)); // t in [0,π]
126		19397	const Scalar t2 = t*t;
127			Scalar alpha, beta;
128
129	✓✗✓✓	19397	if (t < TaylorSeriesExpansion<Scalar>::template precision<3>())
130			{
131		15811	alpha = Scalar(1) - t2/Scalar(12) - t2*t2/Scalar(720);
132		15811	beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
133			}
134			else
135			{
136		3586	Scalar st,ct; SINCOS(t,&st,&ct);
137		3586	alpha = tst/(Scalar(2)(Scalar(1)-ct));
138		3586	beta = Scalar(1)/t2 - st/(Scalar(2)t(Scalar(1)-ct));
139			}
140
141	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	19397	mout.linear().noalias() = alpha * p - Scalar(0.5) * w.cross(p) + (beta * w.dot(p)) * w;
142	✓✗✓✗	19397	mout.angular() = w;
143		19397	}
144			};
145
146			template<typename _Scalar>
147			struct Jlog6_impl
148			{
149			template<typename Scalar, int Options, typename Matrix6Like>
150		315	static void run(const SE3Tpl<Scalar, Options> & M,
151			const Eigen::MatrixBase<Matrix6Like> & Jlog)
152			{
153	✓✗✓✗ ✓✗✓✗	315	PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like, Jlog, 6, 6);
154			// TODO: add static_assert<is_same_type<_Scalar,Scalar>
155
156			typedef SE3Tpl<Scalar,Options> SE3;
157			typedef typename SE3::Vector3 Vector3;
158		315	Matrix6Like & value = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,Jlog);
159
160	✓✗	315	typename SE3::ConstAngularRef R = M.rotation();
161	✓✗	315	typename SE3::ConstLinearRef p = M.translation();
162
163			Scalar t;
164	✓✗	315	Vector3 w(log3(R,t));
165
166			// value is decomposed as following:
167			// value = [ A, B;
168			// C, D ]
169			typedef Eigen::Block<Matrix6Like,3,3> Block33;
170	✓✗	315	Block33 A = value.template topLeftCorner<3,3>();
171	✓✗	315	Block33 B = value.template topRightCorner<3,3>();
172	✓✗	315	Block33 C = value.template bottomLeftCorner<3,3>();
173	✓✗	315	Block33 D = value.template bottomRightCorner<3,3>();
174
175	✓✗	315	Jlog3(t, w, A);
176	✓✗	315	D = A;
177
178		315	const Scalar t2 = t*t;
179			Scalar beta, beta_dot_over_theta;
180	✓✗✓✓	315	if(t < TaylorSeriesExpansion<Scalar>::template precision<3>())
181			{
182		72	beta = Scalar(1)/Scalar(12) + t2/Scalar(720);
183		72	beta_dot_over_theta = Scalar(1)/Scalar(360);
184			}
185			else
186			{
187		243	const Scalar tinv = Scalar(1)/t,
188		243	t2inv = tinv*tinv;
189		243	Scalar st,ct; SINCOS (t, &st, &ct);
190		243	const Scalar inv_2_2ct = Scalar(1)/(Scalar(2)*(Scalar(1)-ct));
191
192		243	beta = t2inv - sttinvinv_2_2ct;
193		243	beta_dot_over_theta = -Scalar(2)t2invt2inv +
194		243	(Scalar(1) + sttinv) t2inv * inv_2_2ct;
195			}
196
197	✓✗	315	Scalar wTp = w.dot(p);
198
199	✓✗✓✗ ✓✗✓✗	315	Vector3 v3_tmp((beta_dot_over_thetawTp)w - (t2beta_dot_over_theta+Scalar(2)beta)*p);
200			// C can be treated as a temporary variable
201	✓✗✓✗ ✓✗✓✗	315	C.noalias() = v3_tmp * w.transpose();
202	✓✗✓✗ ✓✗✓✗ ✓✗	315	C.noalias() += beta * w * p.transpose();
203	✓✗✓✗ ✓✗	315	C.diagonal().array() += wTp * beta;
204	✓✗✓✗	315	addSkew(Scalar(.5)*p,C);
205
206	✓✗✓✗ ✓✗	315	B.noalias() = C * A;
207	✓✗	315	C.setZero();
208		315	}
209			};
210
211			} // namespace pinocchio
212
213			#endif // ifndef __pinocchio_spatial_log_hxx__