Template Function pinocchio::quaternion::firstOrderNormalize

Function Documentation

template<typename D>
void pinocchio::quaternion::firstOrderNormalize(const Eigen::QuaternionBase<D> &q)

Approximately normalize by applying the first order limited development of the normalization function.

Only additions and multiplications are required. Neither square root nor division are used (except a division by 2). Let δ=||q||21. Using the following limited development:

1||q||=(1+δ)12=1δ2+O(δ2)

The output is

qout=q×3||qin||22

The output quaternion is guaranted to statisfy the following:

|||qout||1|M2||qin||(||qin||21)2
where M=34(1ϵ)52 and ϵ is the maximum tolerance of ||qin||21.

Note

See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#title3 to know the reason why the argument is const.

Warning

||q||21 should already be close to zero.