标签： quaternion

euler-angle-quaternion

yuiwongmath

euler-angle-quaternion conversion

for euler angle and quaternion:
kdl conversion better than “yuiwongcppbase angle.hpp” conversion (maybe / not)
both ok. and sometimes maybe eigen conversion cannot work??
see “yuiwongtransformdemo” test and “yuiwongcppbase angle test”

#

by kdl

# ros package: `tf_conversions` when ros

# cmake
find_package(orocos_kdl REQUIRED)
find_package(kdl_conversions REQUIRED)

${orocos_kdl_LIBRARIES}
${kdl_conversions_LIBRARIES}

euler angle r. p. y. to quaternion

Quaternion Quaternion::fromRpy(
    double const roll, double const pitch, double const yaw)
{
    tf::Quaternion tfq;
    tfq.setRPY(roll, pitch, yaw);
    return Quaternion::fromTfQuaternion(tfq);
}

quaternion to r. p. y.

std::tuple<double, double, double> Quaternion::getRpy() const
{
    KDL::Rotation k;
    tf::quaternionMsgToKDL(*this, k);
    double roll, pitch, yaw;
    k.GetRPY(roll, pitch, yaw);
    return std::tuple<double, double, double>(roll, pitch, yaw);
}

conversion

euler angle r. p. y. to quaternion

Eigen::Quaterniond RpyToQuaternion(
    double const roll, double const pitch, double const yaw)
{
    /* Abbreviations for the various angular functions */
    double const cy = ::cos(yaw * 0.5);
    double const sy = ::sin(yaw * 0.5);
    double const cr = ::cos(roll * 0.5);
    double const sr = ::sin(roll * 0.5);
    double const cp = ::cos(pitch * 0.5);
    double const sp = ::sin(pitch * 0.5);
    Eigen::Quaterniond q;
    q.w() = cy * cr * cp + sy * sr * sp;
    q.x() = cy * sr * cp - sy * cr * sp;
    q.y() = cy * cr * sp + sy * sr * cp;
    q.z() = sy * cr * cp - cy * sr * sp;
    return q;
}

quaternion to r. p. y.

std::tuple<double, double, double> QuaternionToRpy(
    Eigen::Quaterniond const& q)
{
    /* roll (x-axis rotation) */
    double const sinr = +2.0 * (q.w() * q.x() + q.y() * q.z());
    double const cosr = +1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
    double const roll = ::atan2(sinr, cosr);
    /* pitch (y-axis rotation) */
    double const sinp = +2.0 * (q.w() * q.y() - q.z() * q.x());
    double pitch;
    if (DoubleCompare(::fabs(sinp), 1.0) > 0) {
        /* use 90 degrees if out of range */
        pitch = ::copysign(M_PI / 2.0, sinp);
    } else {
        pitch = ::asin(sinp);
    }
    /* yaw (z-axis rotation) */
    double const siny = +2.0 * (q.w() * q.z() + q.x() * q.y());
    double const cosy = +1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
    double const yaw = ::atan2(siny, cosy);
    return std::tuple<double, double, double>(roll, pitch, yaw);
}

by eigen

see
https://yuiwong.org/gitlab/cpp/cppcomm/blob/master/yuiwongcppbase/include/yuiwong/angle.hpp

see also

quaternion

yuiwongmath

quaternion

什么是四元数

In mathematics, the quaternions are a number system that extends
the complex numbers.
They were first described by Irish mathematician William Rowan Hamilton in
184312 and applied to mechanics in three-dimensional space.
A feature of quaternions is that multiplication of two quaternions is
noncommutative.
Hamilton defined a quaternion as the quotient of two directed lines
in a three-dimensional space3
or equivalently as the quotient of two vectors.4

Quaternions are generally represented in the form:

where a, b, c, and d are real numbers,
and i, j, and k are the fundamental quaternion units.

相比欧拉角(euler angles) 四元数 (quaternion) 则是一种紧凑、易于迭代、
又不会出现奇异值的表示方法.
它在程序中广为使用, 例如 ros 和几个著名的 slam 公开数据集、
g2o 等程序都使用四元数记录机器人的姿态.
因此, 理解四元数的含义与用法, 对学习 slam 来说是必须的.

四元数仅是 3d 姿态的一种表达方式,
我们用一个单位四元数表达原本用旋转矩阵表示的三维旋转.
这样做一个直接的好处是省空间.
一个旋转阵有 9 个分量, 但只有三个自由度.
那么, 能不能用三个数来描述呢? 可以是可以的,
但不可避免会出现奇异的情况, 欧拉角就是一个例子.
而四元数, 比三维向量多了一个分量, 从而可以无奇异地表示各种姿态.

四元数是 hamilton 找到的一种扩展的复数.
一个四元数拥有一个实部和三个虚部
(故事上说他原先找了很久带两个虚部的, 结果怎么也找不到,
最后豁然开朗找到了三虚部的四元数):


其中 i, j, k 为四元数的三个虚部. 这三个虚部满足关系式:


where i, j, and k are basis elements of H,
determine all the possible products of i, j, and k.

All the other possible products can be determined by similar methods,
resulting in

由于它的这种特殊表示形式, 有时人们也用一个标量和一个向量来表达四元数:

这里, 标量 r 称为四元数的实部, 而向量 v 称为它的虚部.
如果一个四元数虚部为 0, 称之为实四元数.
反之, 若它的实部为0, 称之为虚四元数. 该定义和复数是相似的.

四元数可以表示三维空间中任意一个旋转. 与旋转矩阵中类似,
我们仍假设某个旋转是绕单位向量:

进行了角度为 θ 的旋转, 那么这个旋转的四元数形式为:

\(
\begin{equation} \label{eq:ntheta2quaternion}
\mathbf{q} = \left[
\cos \frac{\theta}{2},
n_x \sin \frac{\theta}{2},
n_y \sin \frac{\theta}{2},
n_z \sin \frac{\theta}{2}\right]^T
\end{equation}
\)

事实上 这还是一个模长为 1 的四元数 称为单位四元数.
反之 我们亦可通过任意一个长度为 1 的四元数 计算对应旋转轴与夹角:

\(
\begin{equation} \begin{cases}
\theta = 2\arccos {q_0}\\
{\left[{{n_x},{n_y},{n_z}} \right]^T} =
{{{\left[ {{q_1},{q_2},{q_3}} \right]}^T}}
/ {\sin \frac{\theta }{2}} \end{cases} \end{equation}
\)

对式 θ 加上 2π 我们得到一个相同的旋转 但此时对应的四元数变成了 -1.
因此 在四元数中 任意的旋转都可以由两个互为相反数的四元数表示.
同理 取 θ 为 0 则得到一个没有任何旋转的四元数:

\(
\begin{equation} \mathbf{q}_0 = \left[ { \pm 1,0,0,0} \right]^T \end{equation}
\)

see also and reference