GCC Code Coverage Report
 Directory: ./ Exec Total Coverage File: include/pinocchio/spatial/explog-quaternion.hpp Lines: 58 60 96.7 % Date: 2024-04-26 13:14:21 Branches: 94 176 53.4 %

 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 26 12971  void exp3(const Eigen::MatrixBase & v, 27  Eigen::QuaternionBase & quat_out) 28  { 29  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like); 30 ✓✗✗✓ 12971  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 QuaternionPlain; 35 36 ✓✗ 12971  const Scalar t2 = v.squaredNorm(); 37 12971  const Scalar t = math::sqrt(t2); 38 39 ✓✓✓✗ 12971  static const Scalar ts_prec = math::sqrt(Eigen::NumTraits::epsilon()); // Precision for the Taylor series expansion. 40 41 ✓✗✓✗ 12971  Eigen::AngleAxis aa(t,v/t); 42 ✓✗ 12971  QuaternionPlain quat_then(aa); 43 44 ✓✗ 12971  QuaternionPlain quat_else; 45 ✓✗✓✗✓✗ 12971  quat_else.vec() = (Scalar(1)/Scalar(2) - t2/48) * v; 46 ✓✗ 12971  quat_else.w() = Scalar(1) - t2/8; 47 48  using ::pinocchio::internal::if_then_else; 49 ✓✓ 64855  for(Eigen::DenseIndex k = 0; k < 4; ++k) 50  { 51 ✓✗✓✓✓✗✓✗✓✗✓✗ 103768  quat_out.coeffs().coeffRef(k) = if_then_else(::pinocchio::internal::GT, t2, ts_prec, 52 51884  quat_then.coeffs().coeffRef(k), 53 51884  quat_else.coeffs().coeffRef(k)); 54  } 55 56 12971  } 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 66  Eigen::Quaternion 67  exp3(const Eigen::MatrixBase & v) 68  { 69  typedef Eigen::Quaternion 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 83  Eigen::Matrix 84 12438  log3(const Eigen::QuaternionBase & 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 Vector3; 90 91 ✓✗ 12438  Vector3 res; 92 ✓✗✓✗ 12438  const Scalar norm_squared = quat.vec().squaredNorm(); 93 94 ✓✓✓✗ 12438  static const Scalar eps = Eigen::NumTraits::epsilon(); 95 ✓✓✓✗✓✗✗✗ 12438  static const Scalar ts_prec = TaylorSeriesExpansion::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 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_sq*y_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 143  Eigen::Matrix 144 4996  log3(const Eigen::QuaternionBase & 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 157 64  void Jexp3CoeffWise(const Eigen::MatrixBase & v, 158  const Eigen::MatrixBase & 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::epsilon())) 172  { 173  Scalar c, s; 174 63  SINCOS(theta,&s,&c); 175 ✓✗✓✗✓✗✓✗✓✗✓✗ 63  Jout.template topRows<3>().noalias() = ((Scalar(0.5)/n2) * (c - 2*s/n)) * v * v.transpose(); 176 ✓✗✓✗✓✗✓✗ 63  Jout.template topRows<3>().diagonal().array() += s/n; 177 ✓✗✓✗✓✗✓✗✓✗ 63  Jout.template bottomRows<1>().noalias() = -s/(2*n) * 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 195 341  void Jlog3(const Eigen::QuaternionBase & quat, 196  const Eigen::MatrixBase & Jlog) 197  { 198  typedef typename QuaternionLike::Scalar Scalar; 199  typedef Eigen::Matrix 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__

 Generated by: GCOVR (Version 4.2)