コード例 #1
0
 void ExpMapQuaternion::exponential(OutputType& q, const ConstRefVec& v)
 {
   mnf_assert(v.size() == 3 && "Increment for expMap must be of size 3");
   double n2 = v.squaredNorm(); // (theta)^2 (in Grassia)
   mnf_assert(sqrt(n2) < M_PI && "Increment for expMap must be of norm at most pi");
   double s; // sin(theta/2)/theta in Grassia
   if (n2 < prec)
   {
     toQuat(q.data()).w() = 1 + (-1 + n2 / 48)*(n2/8);// cos(theta/2) in Grassia
     s = (1+(-1+0.0125*n2)*n2/24)/2;
   }
   else
   {
     double t = sqrt(n2); // theta (in Grassia)
     toQuat(q.data()).w() = cos(0.5*t);
     s = sin(0.5*t) / t;
   }
   toQuat(q.data()).vec() = s*v;
 }
コード例 #2
0
  void ExpMapQuaternion::logarithm(RefVec out, const OutputType& v)
  {
    const toConstQuat vQ(v.data());
    double n2 = vQ.vec().squaredNorm();
    double n = sqrt(n2);

    if (n < prec && vQ.w()!=0 )
      out = (2/vQ.w())*vQ.vec();
    else
      out = atan2(2 * n * vQ.w(), vQ.w() * vQ.w() - n2) / n * vQ.vec(); 
  }
コード例 #3
0
 void ExpMapQuaternion::retractation_(RefVec out, const ConstRefVec& x, const ConstRefVec& v)
 {
   OutputType q;
   exponential(q,v);
   toQuat(out.data()) = (toConstQuat(x.data()))*(toConstQuat(q.data())); //out = x*exp(v)
 }