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 %

 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 12  struct log3_impl 13  { 14  template 15 21810  static void run(const Eigen::MatrixBase & R, 16  typename Matrix3Like::Scalar & theta, 17  const Eigen::MatrixBase & 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 22 23  typedef typename Matrix3Like::Scalar Scalar; 24  typedef Eigen::Matrix Vector3; 25 26 ✓✓✓✗ 21810  static const Scalar PI_value = PI(); 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::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 73  struct Jlog3_impl 74  { 75  template 76 992  static void run(const Scalar & theta, 77  const Eigen::MatrixBase & log, 78  const Eigen::MatrixBase & 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 83 84 992  Matrix3Like & Jlog_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like,Jlog); 85 86  Scalar alpha, diag_value; 87 ✓✗✓✓ 992  if(theta < TaylorSeriesExpansion::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)/(theta*theta) - 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 112  struct log6_impl 113  { 114  template 115 19397  static void run(const SE3Tpl & M, 116  MotionDense & mout) 117  { 118  typedef SE3Tpl 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::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 = t*st/(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 147  struct Jlog6_impl 148  { 149  template 150 315  static void run(const SE3Tpl & M, 151  const Eigen::MatrixBase & Jlog) 152  { 153 ✓✗✓✗✓✗✓✗ 315  PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like, Jlog, 6, 6); 154  // TODO: add static_assert 155 156  typedef SE3Tpl 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 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::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 - st*tinv*inv_2_2ct; 193 243  beta_dot_over_theta = -Scalar(2)*t2inv*t2inv + 194 243  (Scalar(1) + st*tinv) * t2inv * inv_2_2ct; 195  } 196 197 ✓✗ 315  Scalar wTp = w.dot(p); 198 199 ✓✗✓✗✓✗✓✗ 315  Vector3 v3_tmp((beta_dot_over_theta*wTp)*w - (t2*beta_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__

 Generated by: GCOVR (Version 4.2)