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 \( \delta = ||q||^2 - 1 \). Using the following limited development:

\[ \frac{1}{||q||} = (1 + \delta)^{-\frac{1}{2}} = 1 - \frac{\delta}{2} + \mathcal{O}(\delta^2) \]

The output is

\[ q_{out} = q \times \frac{3 - ||q_{in}||^2}{2} \]

The output quaternion is guaranted to statisfy the following:

\[ | ||q_{out}|| - 1 | \le \frac{M}{2} ||q_{in}|| ( ||q_{in}||^2 - 1 )^2 \]
where \( M = \frac{3}{4} (1 - \epsilon)^{-\frac{5}{2}} \) and \( \epsilon \) is the maximum tolerance of \( ||q_{in}||^2 - 1 \).

Note

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

Warning

\( ||q||^2 - 1 \) should already be close to zero.