quaternion exponential logarithm
Submitted by Michael Norel
Assigned to Nobody
Link to original bugzilla bug (#1244)
Version: 3.3 (current stable)
Description
For me, it can be useful to have conversions to/from rodrigues parameters from/to quaternions.
Usualy it is called " exponential/logarithm" (from lie algebra). As it can be seen in "Sophus" library built on top of Eigen.
https://github.com/strasdat/Sophus/blob/master/sophus/so3.hpp
My versions of this operations looks like (the SO3 class represent 3 rodrigues params in member m_coeffs ):
SO3(const QuaternionT& q_)
{
// forcing positive "w" to work from 0 to PI
const QuaternionT& q = q_.w() >= Scalar(0) ? q_ : QuaternionT(-q_.coeffs());
const Vector3& qv = q.vec();
Scalar sinha = qv.norm();
if(sinha > Scalar(0))
{
Scalar angle = Scalar(2) * atan2(sinha, q.w()); //NOTE: signed
m_coeffs = qv * (angle/sinha);
}else{
// if l is too small, its norm can be equal 0 but norm_inf greater 0
// probably w is much bigger that vec, use it as length
m_coeffs = qv * (Scalar(2)/q.w()); ////NOTE: signed
}
} NOTE: it is important to keep rodrigues params inside (0 ...PI) interval for precision. Also all handmade taylor expansions of SINC is not useful and have no better precision than direct computation.
QuaternionT getQuaternion() const
{
QuaternionT q;
const Vector3 ha = m_coeffs/Scalar(2); // vector of half angle
Scalar l = ha.norm();
if( l > Scalar(0) )
{
Scalar ss = sin(l)/l;
q = QuaternionT(cos(l), ha.x()*ss, ha.y()*ss, ha.z()*ss);
}else{
// if l is too small, its norm can be equal 0 but norm_inf greater 0
q = QuaternionT(Scalar(1), ha.x(), ha.y(), ha.z());
}
return q;
}