Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/spatial/se3-tpl.hpp	Lines:	112	113	99.1 %
Date:	2024-04-26 13:14:21	Branches:	122	238	51.3 %


//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_se3_tpl_hpp__
#define __pinocchio_se3_tpl_hpp__

#include "pinocchio/spatial/fwd.hpp"
#include "pinocchio/spatial/se3-base.hpp"

#include "pinocchio/math/quaternion.hpp"
#include "pinocchio/math/rotation.hpp"
#include "pinocchio/spatial/cartesian-axis.hpp"

#include <Eigen/Geometry>

namespace pinocchio
{
  template<typename _Scalar, int _Options>
  struct traits< SE3Tpl<_Scalar,_Options> >
  {
    enum {
      Options = _Options,
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef _Scalar Scalar;
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,4,1,Options> Vector4;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef Matrix3 AngularType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Matrix3) AngularRef;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix3) ConstAngularRef;
    typedef Vector3 LinearType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector3) LinearRef;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector3) ConstLinearRef;
    typedef Matrix6 ActionMatrixType;
    typedef Matrix4 HomogeneousMatrixType;
    typedef SE3Tpl<Scalar,Options> PlainType;
  }; // traits SE3Tpl

  template<typename _Scalar, int _Options>
  struct SE3Tpl : public SE3Base< SE3Tpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PINOCCHIO_SE3_TYPEDEF_TPL(SE3Tpl);
    typedef SE3Base< SE3Tpl<_Scalar,_Options> > Base;
    typedef Eigen::Quaternion<Scalar,Options> Quaternion;
    typedef typename traits<SE3Tpl>::Vector3 Vector3;
    typedef typename traits<SE3Tpl>::Matrix3 Matrix3;
    typedef typename traits<SE3Tpl>::Matrix4 Matrix4;
    typedef typename traits<SE3Tpl>::Vector4 Vector4;
    typedef typename traits<SE3Tpl>::Matrix6 Matrix6;

    using Base::rotation;
    using Base::translation;

    SE3Tpl(): rot(), trans() {};

    template<typename QuaternionLike,typename Vector3Like>
    SE3Tpl(const Eigen::QuaternionBase<QuaternionLike> & quat,
           const Eigen::MatrixBase<Vector3Like> & trans)
    : rot(quat.matrix()), trans(trans)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
    }

    template<typename Matrix3Like,typename Vector3Like>
    SE3Tpl(const Eigen::MatrixBase<Matrix3Like> & R,
           const Eigen::MatrixBase<Vector3Like> & trans)
    : rot(R), trans(trans)
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3)
    }

    template<typename Matrix4Like>
    explicit SE3Tpl(const Eigen::MatrixBase<Matrix4Like> & m)
    : rot(m.template block<3,3>(LINEAR,LINEAR))
    , trans(m.template block<3,1>(LINEAR,ANGULAR))
    {
      EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like,4,4);
    }

    SE3Tpl(int)
    : rot(AngularType::Identity())
    , trans(LinearType::Zero())
    {}

    template<int O2>
    SE3Tpl(const SE3Tpl<Scalar,O2> & clone)
    : rot(clone.rotation()),trans(clone.translation()) {}

    template<int O2>
    SE3Tpl & operator=(const SE3Tpl<Scalar,O2> & other)
    {
      rot = other.rotation();
      trans = other.translation();
      return *this;
    }

    static SE3Tpl Identity()
    {
      return SE3Tpl(1);
    }

    SE3Tpl & setIdentity()
    { rot.setIdentity (); trans.setZero (); return *this;}

    /// aXb = bXa.inverse()
    SE3Tpl inverse() const
    {
      return SE3Tpl(rot.transpose(), -rot.transpose()*trans);
    }

    static SE3Tpl Random()
    {
      return SE3Tpl().setRandom();
    }

    SE3Tpl & setRandom()
    {
      Quaternion q; quaternion::uniformRandom(q);
      rot = q.matrix();
      trans.setRandom();

      return *this;
    }

    HomogeneousMatrixType toHomogeneousMatrix_impl() const
    {
      HomogeneousMatrixType M;
      M.template block<3,3>(LINEAR,LINEAR) = rot;
      M.template block<3,1>(LINEAR,ANGULAR) = trans;
      M.template block<1,3>(ANGULAR,LINEAR).setZero();
      M(3,3) = 1;
      return M;
    }

    /// Vb.toVector() = bXa.toMatrix() * Va.toVector()
    ActionMatrixType toActionMatrix_impl() const
    {
      typedef Eigen::Block<ActionMatrixType,3,3> Block3;
      ActionMatrixType M;
      M.template block<3,3>(ANGULAR,ANGULAR)
      = M.template block<3,3>(LINEAR,LINEAR) = rot;
      M.template block<3,3>(ANGULAR,LINEAR).setZero();
      Block3 B = M.template block<3,3>(LINEAR,ANGULAR);

      B.col(0) = trans.cross(rot.col(0));
      B.col(1) = trans.cross(rot.col(1));
      B.col(2) = trans.cross(rot.col(2));
      return M;
    }

    ActionMatrixType toActionMatrixInverse_impl() const
    {
      typedef Eigen::Block<ActionMatrixType,3,3> Block3;
      ActionMatrixType M;
      M.template block<3,3>(ANGULAR,ANGULAR)
      = M.template block<3,3>(LINEAR,LINEAR) = rot.transpose();
      Block3 C = M.template block<3,3>(ANGULAR,LINEAR); // used as temporary
      Block3 B = M.template block<3,3>(LINEAR,ANGULAR);

#define PINOCCHIO_INTERNAL_COMPUTATION(axis_id,v3_in,v3_out,R,res) \
  CartesianAxis<axis_id>::cross(v3_in,v3_out); \
  res.col(axis_id).noalias() = R.transpose() * v3_out;

      PINOCCHIO_INTERNAL_COMPUTATION(0,trans,C.col(0),rot,B);
      PINOCCHIO_INTERNAL_COMPUTATION(1,trans,C.col(0),rot,B);
      PINOCCHIO_INTERNAL_COMPUTATION(2,trans,C.col(0),rot,B);

#undef PINOCCHIO_INTERNAL_COMPUTATION

      C.setZero();
      return M;
    }

    ActionMatrixType toDualActionMatrix_impl() const
    {
      typedef Eigen::Block<ActionMatrixType,3,3> Block3;
      ActionMatrixType M;
      M.template block<3,3>(ANGULAR,ANGULAR)
      = M.template block<3,3>(LINEAR,LINEAR) = rot;
      M.template block<3,3>(LINEAR,ANGULAR).setZero();
      Block3 B = M.template block<3,3>(ANGULAR,LINEAR);

      B.col(0) = trans.cross(rot.col(0));
      B.col(1) = trans.cross(rot.col(1));
      B.col(2) = trans.cross(rot.col(2));
      return M;
    }

    void disp_impl(std::ostream & os) const
    {
      os
      << "  R =\n" << rot << std::endl
      << "  p = " << trans.transpose() << std::endl;
    }

    /// --- GROUP ACTIONS ON M6, F6 and I6 ---

    /// ay = aXb.act(by)
    template<typename D>
    typename SE3GroupAction<D>::ReturnType
    act_impl(const D & d) const
    {
      return d.se3Action(*this);
    }

    /// by = aXb.actInv(ay)
    template<typename D> typename SE3GroupAction<D>::ReturnType
    actInv_impl(const D & d) const
    {
      return d.se3ActionInverse(*this);
    }

    template<typename EigenDerived>
    typename EigenDerived::PlainObject
    actOnEigenObject(const Eigen::MatrixBase<EigenDerived> & p) const
    { return (rotation()*p+translation()).eval(); }

    template<typename MapDerived>
    Vector3 actOnEigenObject(const Eigen::MapBase<MapDerived> & p) const
    { return Vector3(rotation()*p+translation()); }

    template<typename EigenDerived>
    typename EigenDerived::PlainObject
    actInvOnEigenObject(const Eigen::MatrixBase<EigenDerived> & p) const
    { return (rotation().transpose()*(p-translation())).eval(); }

    template<typename MapDerived>
    Vector3 actInvOnEigenObject(const Eigen::MapBase<MapDerived> & p) const
    { return Vector3(rotation().transpose()*(p-translation())); }

    Vector3 act_impl(const Vector3 & p) const
    { return Vector3(rotation()*p+translation()); }

    Vector3 actInv_impl(const Vector3 & p) const
    { return Vector3(rotation().transpose()*(p-translation())); }

    template<int O2>
    SE3Tpl act_impl(const SE3Tpl<Scalar,O2> & m2) const
    { return SE3Tpl(rot*m2.rotation()
                    ,translation()+rotation()*m2.translation());}

    template<int O2>
    SE3Tpl actInv_impl(const SE3Tpl<Scalar,O2> & m2) const
    { return SE3Tpl(rot.transpose()*m2.rotation(),
                    rot.transpose()*(m2.translation()-translation()));}

    template<int O2>
    SE3Tpl __mult__(const SE3Tpl<Scalar,O2> & m2) const
    { return this->act_impl(m2);}

    template<int O2>
    bool isEqual(const SE3Tpl<Scalar,O2> & m2) const
    {
      return (rotation() == m2.rotation() && translation() == m2.translation());
    }

    template<int O2>
    bool isApprox_impl(const SE3Tpl<Scalar,O2> & m2,
                       const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      return rotation().isApprox(m2.rotation(), prec)
      && translation().isApprox(m2.translation(), prec);
    }

    bool isIdentity(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      return rotation().isIdentity(prec) && translation().isZero(prec);
    }

    ConstAngularRef rotation_impl() const { return rot; }
    AngularRef rotation_impl() { return rot; }
    void rotation_impl(const AngularType & R) { rot = R; }
    ConstLinearRef translation_impl() const { return trans;}
    LinearRef translation_impl() { return trans;}
    void translation_impl(const LinearType & p) { trans = p; }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    SE3Tpl<NewScalar,Options> cast() const
    {
      typedef SE3Tpl<NewScalar,Options> ReturnType;
      ReturnType res(rot.template cast<NewScalar>(),
                     trans.template cast<NewScalar>());

      // During the cast, it may appear that the matrix is not normalized correctly.
      // Force the normalization of the rotation part of the matrix.
      internal::cast_call_normalize_method<SE3Tpl,NewScalar,Scalar>::run(res);
      return res;
    }

    bool isNormalized(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
    {
      return isUnitary(rot,prec);
    }

    void normalize()
    {
      rot = orthogonalProjection(rot);
    }

    PlainType normalized() const
    {
      PlainType res(*this); res.normalize();
      return res;
    }

    ///
    /// \brief Linear interpolation on the SE3 manifold.
    ///
    /// \param[in] A Initial transformation.
    /// \param[in] B Target transformation.
    /// \param[in] alpha Interpolation factor in [0 ... 1].
    ///
    /// \returns An interpolated transformation between A and B.
    ///
    /// \note This is similar to the SLERP operation which acts initially for rotation but applied here to rigid transformation.
    ///
    template<typename OtherScalar>
    static SE3Tpl Interpolate(const SE3Tpl & A, const SE3Tpl & B, const OtherScalar & alpha);

  protected:
    AngularType rot;
    LinearType trans;

  }; // class SE3Tpl

  namespace internal
  {
    template<typename Scalar, int Options>
    struct cast_call_normalize_method<SE3Tpl<Scalar,Options>,Scalar,Scalar>
    {
      template<typename T>
      static void run(T &) {}
    };

    template<typename Scalar, int Options, typename NewScalar>
    struct cast_call_normalize_method<SE3Tpl<Scalar,Options>,NewScalar,Scalar>
    {
      template<typename T>
      static void run(T & self)
      {
        if(pinocchio::cast<NewScalar>(Eigen::NumTraits<Scalar>::epsilon()) > Eigen::NumTraits<NewScalar>::epsilon())
          self.normalize();
      }
    };

  }

} // namespace pinocchio

#endif // ifndef __pinocchio_se3_tpl_hpp__



Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2020 CNRS INRIA
3			// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4			//
5
6			#ifndef __pinocchio_se3_tpl_hpp__
7			#define __pinocchio_se3_tpl_hpp__
8
9			#include "pinocchio/spatial/fwd.hpp"
10			#include "pinocchio/spatial/se3-base.hpp"
11
12			#include "pinocchio/math/quaternion.hpp"
13			#include "pinocchio/math/rotation.hpp"
14			#include "pinocchio/spatial/cartesian-axis.hpp"
15
16			#include <Eigen/Geometry>
17
18			namespace pinocchio
19			{
20			template<typename _Scalar, int _Options>
21			struct traits< SE3Tpl<_Scalar,_Options> >
22			{
23			enum {
24			Options = _Options,
25			LINEAR = 0,
26			ANGULAR = 3
27			};
28			typedef _Scalar Scalar;
29			typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
30			typedef Eigen::Matrix<Scalar,4,1,Options> Vector4;
31			typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
32			typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
33			typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4;
34			typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
35			typedef Matrix3 AngularType;
36			typedef typename PINOCCHIO_EIGEN_REF_TYPE(Matrix3) AngularRef;
37			typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix3) ConstAngularRef;
38			typedef Vector3 LinearType;
39			typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector3) LinearRef;
40			typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector3) ConstLinearRef;
41			typedef Matrix6 ActionMatrixType;
42			typedef Matrix4 HomogeneousMatrixType;
43			typedef SE3Tpl<Scalar,Options> PlainType;
44			}; // traits SE3Tpl
45
46			template<typename _Scalar, int _Options>
47			struct SE3Tpl : public SE3Base< SE3Tpl<_Scalar,_Options> >
48			{
49			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
50			PINOCCHIO_SE3_TYPEDEF_TPL(SE3Tpl);
51			typedef SE3Base< SE3Tpl<_Scalar,_Options> > Base;
52			typedef Eigen::Quaternion<Scalar,Options> Quaternion;
53			typedef typename traits<SE3Tpl>::Vector3 Vector3;
54			typedef typename traits<SE3Tpl>::Matrix3 Matrix3;
55			typedef typename traits<SE3Tpl>::Matrix4 Matrix4;
56			typedef typename traits<SE3Tpl>::Vector4 Vector4;
57			typedef typename traits<SE3Tpl>::Matrix6 Matrix6;
58
59			using Base::rotation;
60			using Base::translation;
61
62		198947	SE3Tpl(): rot(), trans() {};
63
64			template<typename QuaternionLike,typename Vector3Like>
65		61	SE3Tpl(const Eigen::QuaternionBase<QuaternionLike> & quat,
66			const Eigen::MatrixBase<Vector3Like> & trans)
67		61	: rot(quat.matrix()), trans(trans)
68			{
69			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
70		61	}
71
72			template<typename Matrix3Like,typename Vector3Like>
73		2909970	SE3Tpl(const Eigen::MatrixBase<Matrix3Like> & R,
74			const Eigen::MatrixBase<Vector3Like> & trans)
75		2909970	: rot(R), trans(trans)
76			{
77			EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3)
78			EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like,3,3)
79		2909970	}
80
81			template<typename Matrix4Like>
82		1	explicit SE3Tpl(const Eigen::MatrixBase<Matrix4Like> & m)
83			: rot(m.template block<3,3>(LINEAR,LINEAR))
84	✓✗✓✗	1	, trans(m.template block<3,1>(LINEAR,ANGULAR))
85			{
86			EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix4Like,4,4);
87		1	}
88
89		542224	SE3Tpl(int)
90			: rot(AngularType::Identity())
91	✓✗✓✗	542224	, trans(LinearType::Zero())
92		542224	{}
93
94			template<int O2>
95			SE3Tpl(const SE3Tpl<Scalar,O2> & clone)
96			: rot(clone.rotation()),trans(clone.translation()) {}
97
98			template<int O2>
99			SE3Tpl & operator=(const SE3Tpl<Scalar,O2> & other)
100			{
101			rot = other.rotation();
102			trans = other.translation();
103			return *this;
104			}
105
106		542216	static SE3Tpl Identity()
107			{
108		542216	return SE3Tpl(1);
109			}
110
111		183	SE3Tpl & setIdentity()
112		183	{ rot.setIdentity (); trans.setZero (); return *this;}
113
114			/// aXb = bXa.inverse()
115		6253	SE3Tpl inverse() const
116			{
117	✓✗✓✗ ✓✗✓✗	12506	return SE3Tpl(rot.transpose(), -rot.transpose()*trans);
118			}
119
120		105620	static SE3Tpl Random()
121			{
122	✓✗✓✗	105620	return SE3Tpl().setRandom();
123			}
124
125		105633	SE3Tpl & setRandom()
126			{
127	✓✗✓✗	105633	Quaternion q; quaternion::uniformRandom(q);
128	✓✗	105633	rot = q.matrix();
129	✓✗	105633	trans.setRandom();
130
131		105633	return *this;
132			}
133
134		5	HomogeneousMatrixType toHomogeneousMatrix_impl() const
135			{
136		5	HomogeneousMatrixType M;
137	✓✗	5	M.template block<3,3>(LINEAR,LINEAR) = rot;
138	✓✗	5	M.template block<3,1>(LINEAR,ANGULAR) = trans;
139	✓✗	5	M.template block<1,3>(ANGULAR,LINEAR).setZero();
140		5	M(3,3) = 1;
141		5	return M;
142			}
143
144			/// Vb.toVector() = bXa.toMatrix() * Va.toVector()
145		3853	ActionMatrixType toActionMatrix_impl() const
146			{
147			typedef Eigen::Block<ActionMatrixType,3,3> Block3;
148	✓✗	3853	ActionMatrixType M;
149			M.template block<3,3>(ANGULAR,ANGULAR)
150	✓✗✓✗ ✓✗✓✗	3853	= M.template block<3,3>(LINEAR,LINEAR) = rot;
151	✓✗✓✗	3853	M.template block<3,3>(ANGULAR,LINEAR).setZero();
152	✓✗	3853	Block3 B = M.template block<3,3>(LINEAR,ANGULAR);
153
154	✓✗✓✗ ✓✗✓✗	3853	B.col(0) = trans.cross(rot.col(0));
155	✓✗✓✗ ✓✗✓✗	3853	B.col(1) = trans.cross(rot.col(1));
156	✓✗✓✗ ✓✗✓✗	3853	B.col(2) = trans.cross(rot.col(2));
157		7706	return M;
158			}
159
160		15	ActionMatrixType toActionMatrixInverse_impl() const
161			{
162			typedef Eigen::Block<ActionMatrixType,3,3> Block3;
163	✓✗	15	ActionMatrixType M;
164			M.template block<3,3>(ANGULAR,ANGULAR)
165	✓✗✓✗ ✓✗✓✗ ✓✗	15	= M.template block<3,3>(LINEAR,LINEAR) = rot.transpose();
166	✓✗	15	Block3 C = M.template block<3,3>(ANGULAR,LINEAR); // used as temporary
167	✓✗	15	Block3 B = M.template block<3,3>(LINEAR,ANGULAR);
168
169			#define PINOCCHIO_INTERNAL_COMPUTATION(axis_id,v3_in,v3_out,R,res) \
170			CartesianAxis<axis_id>::cross(v3_in,v3_out); \
171			res.col(axis_id).noalias() = R.transpose() * v3_out;
172
173	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	15	PINOCCHIO_INTERNAL_COMPUTATION(0,trans,C.col(0),rot,B);
174	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	15	PINOCCHIO_INTERNAL_COMPUTATION(1,trans,C.col(0),rot,B);
175	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	15	PINOCCHIO_INTERNAL_COMPUTATION(2,trans,C.col(0),rot,B);
176
177			#undef PINOCCHIO_INTERNAL_COMPUTATION
178
179	✓✗	15	C.setZero();
180		30	return M;
181			}
182
183		88	ActionMatrixType toDualActionMatrix_impl() const
184			{
185			typedef Eigen::Block<ActionMatrixType,3,3> Block3;
186	✓✗	88	ActionMatrixType M;
187			M.template block<3,3>(ANGULAR,ANGULAR)
188	✓✗✓✗ ✓✗✓✗	88	= M.template block<3,3>(LINEAR,LINEAR) = rot;
189	✓✗✓✗	88	M.template block<3,3>(LINEAR,ANGULAR).setZero();
190	✓✗	88	Block3 B = M.template block<3,3>(ANGULAR,LINEAR);
191
192	✓✗✓✗ ✓✗✓✗	88	B.col(0) = trans.cross(rot.col(0));
193	✓✗✓✗ ✓✗✓✗	88	B.col(1) = trans.cross(rot.col(1));
194	✓✗✓✗ ✓✗✓✗	88	B.col(2) = trans.cross(rot.col(2));
195		176	return M;
196			}
197
198		4	void disp_impl(std::ostream & os) const
199			{
200			os
201		4	<< " R =\n" << rot << std::endl
202	✓✗✓✗	4	<< " p = " << trans.transpose() << std::endl;
203		4	}
204
205			/// --- GROUP ACTIONS ON M6, F6 and I6 ---
206
207			/// ay = aXb.act(by)
208			template<typename D>
209			typename SE3GroupAction<D>::ReturnType
210		283825	act_impl(const D & d) const
211			{
212		283825	return d.se3Action(*this);
213			}
214
215			/// by = aXb.actInv(ay)
216			template<typename D> typename SE3GroupAction<D>::ReturnType
217		188908	actInv_impl(const D & d) const
218			{
219		188908	return d.se3ActionInverse(*this);
220			}
221
222			template<typename EigenDerived>
223			typename EigenDerived::PlainObject
224			actOnEigenObject(const Eigen::MatrixBase<EigenDerived> & p) const
225			{ return (rotation()*p+translation()).eval(); }
226
227			template<typename MapDerived>
228			Vector3 actOnEigenObject(const Eigen::MapBase<MapDerived> & p) const
229			{ return Vector3(rotation()*p+translation()); }
230
231			template<typename EigenDerived>
232			typename EigenDerived::PlainObject
233			actInvOnEigenObject(const Eigen::MatrixBase<EigenDerived> & p) const
234			{ return (rotation().transpose()*(p-translation())).eval(); }
235
236			template<typename MapDerived>
237			Vector3 actInvOnEigenObject(const Eigen::MapBase<MapDerived> & p) const
238			{ return Vector3(rotation().transpose()*(p-translation())); }
239
240		1355	Vector3 act_impl(const Vector3 & p) const
241	✓✗✓✗	1355	{ return Vector3(rotation()*p+translation()); }
242
243		5513	Vector3 actInv_impl(const Vector3 & p) const
244	✓✗✓✗ ✓✗✓✗	5513	{ return Vector3(rotation().transpose()*(p-translation())); }
245
246			template<int O2>
247		1413723	SE3Tpl act_impl(const SE3Tpl<Scalar,O2> & m2) const
248	✓✗✓✗	1413723	{ return SE3Tpl(rot*m2.rotation()
249	✓✗✓✗ ✓✗	4241169	,translation()+rotation()*m2.translation());}
250
251			template<int O2>
252		14495	SE3Tpl actInv_impl(const SE3Tpl<Scalar,O2> & m2) const
253	✓✗✓✗ ✓✗	14495	{ return SE3Tpl(rot.transpose()*m2.rotation(),
254	✓✗✓✗ ✓✗	43485	rot.transpose()*(m2.translation()-translation()));}
255
256			template<int O2>
257		1413723	SE3Tpl __mult__(const SE3Tpl<Scalar,O2> & m2) const
258		1413723	{ return this->act_impl(m2);}
259
260			template<int O2>
261		9764	bool isEqual(const SE3Tpl<Scalar,O2> & m2) const
262			{
263	✓✓✓✓	9764	return (rotation() == m2.rotation() && translation() == m2.translation());
264			}
265
266			template<int O2>
267		55781	bool isApprox_impl(const SE3Tpl<Scalar,O2> & m2,
268			const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
269			{
270		55781	return rotation().isApprox(m2.rotation(), prec)
271	✓✗✓✗	55781	&& translation().isApprox(m2.translation(), prec);
272			}
273
274		2	bool isIdentity(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
275			{
276	✓✗✓✗	2	return rotation().isIdentity(prec) && translation().isZero(prec);
277			}
278
279		3770010	ConstAngularRef rotation_impl() const { return rot; }
280		568481	AngularRef rotation_impl() { return rot; }
281		29023	void rotation_impl(const AngularType & R) { rot = R; }
282		3324689	ConstLinearRef translation_impl() const { return trans;}
283		97551	LinearRef translation_impl() { return trans;}
284		22556	void translation_impl(const LinearType & p) { trans = p; }
285
286			/// \returns An expression of *this with the Scalar type casted to NewScalar.
287			template<typename NewScalar>
288		1616	SE3Tpl<NewScalar,Options> cast() const
289			{
290			typedef SE3Tpl<NewScalar,Options> ReturnType;
291	✓✗✓✗	1616	ReturnType res(rot.template cast<NewScalar>(),
292		1616	trans.template cast<NewScalar>());
293
294			// During the cast, it may appear that the matrix is not normalized correctly.
295			// Force the normalization of the rotation part of the matrix.
296		1616	internal::cast_call_normalize_method<SE3Tpl,NewScalar,Scalar>::run(res);
297		1616	return res;
298			}
299
300		3	bool isNormalized(const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision()) const
301			{
302		3	return isUnitary(rot,prec);
303			}
304
305		176	void normalize()
306			{
307		176	rot = orthogonalProjection(rot);
308		176	}
309
310		1	PlainType normalized() const
311			{
312		1	PlainType res(*this); res.normalize();
313		1	return res;
314			}
315
316			///
317			/// \brief Linear interpolation on the SE3 manifold.
318			///
319			/// \param[in] A Initial transformation.
320			/// \param[in] B Target transformation.
321			/// \param[in] alpha Interpolation factor in [0 ... 1].
322			///
323			/// \returns An interpolated transformation between A and B.
324			///
325			/// \note This is similar to the SLERP operation which acts initially for rotation but applied here to rigid transformation.
326			///
327			template<typename OtherScalar>
328			static SE3Tpl Interpolate(const SE3Tpl & A, const SE3Tpl & B, const OtherScalar & alpha);
329
330			protected:
331			AngularType rot;
332			LinearType trans;
333
334			}; // class SE3Tpl
335
336			namespace internal
337			{
338			template<typename Scalar, int Options>
339			struct cast_call_normalize_method<SE3Tpl<Scalar,Options>,Scalar,Scalar>
340			{
341			template<typename T>
342		1015	static void run(T &) {}
343			};
344
345			template<typename Scalar, int Options, typename NewScalar>
346			struct cast_call_normalize_method<SE3Tpl<Scalar,Options>,NewScalar,Scalar>
347			{
348			template<typename T>
349		178	static void run(T & self)
350			{
351	✓✗✓✓	178	if(pinocchio::cast<NewScalar>(Eigen::NumTraits<Scalar>::epsilon()) > Eigen::NumTraits<NewScalar>::epsilon())
352		172	self.normalize();
353		178	}
354			};
355
356			}
357
358			} // namespace pinocchio
359
360			#endif // ifndef __pinocchio_se3_tpl_hpp__
361