YawPitchRoll.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012 by
 *   MetraLabs GmbH (MLAB), GERMANY
 * and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Contact: info@mira-project.org
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MLAB or NICR.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file. Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MLAB and NICR (or its successors, if any).
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_YAWPITCHROLL_H_
#define _MIRA_YAWPITCHROLL_H_

#ifndef Q_MOC_RUN
#include <boost/tuple/tuple.hpp>
#endif

#include <math/Eigen.h>

namespace mira
{


template<typename Derived>
inline Eigen::Matrix<typename Eigen::MatrixBase<Derived>::Scalar,3,1>
eulerAngles(const Eigen::MatrixBase<Derived>& mat,
            typename Eigen::MatrixBase<Derived>::Index a0,
            typename Eigen::MatrixBase<Derived>::Index a1,
            typename Eigen::MatrixBase<Derived>::Index a2)
{
    EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived,3,3)

    typedef typename Eigen::MatrixBase<Derived>::Index Index;
    typedef typename Eigen::MatrixBase<Derived>::Scalar Scalar;

    Eigen::Matrix<Scalar,3,1> res;
    typedef Eigen::Matrix<typename Derived::Scalar,2,1> Vector2;
    const Scalar epsilon = Eigen::NumTraits<Scalar>::dummy_precision();


    const Index odd = ((a0+1)%3 == a1) ? 0 : 1;
    const Index i = a0;
    const Index j = (a0 + 1 + odd)%3;
    const Index k = (a0 + 2 - odd)%3;

    if (a0==a2)
    {
        Scalar s = Vector2(mat.coeff(j,i) , mat.coeff(k,i)).norm();
        res[1] = std::atan2(s, mat.coeff(i,i));
        if (s > epsilon)
        {
            res[0] = std::atan2(mat.coeff(j,i), mat.coeff(k,i));
            res[2] = std::atan2(mat.coeff(i,j),-mat.coeff(i,k));
        }
        else
        {
            res[0] = Scalar(0);
            res[2] = (mat.coeff(i,i)>0?1:-1)*std::atan2(-mat.coeff(k,j), mat.coeff(j,j));
        }
    }
    else
    {
        Scalar c = Vector2(mat.coeff(i,i) , mat.coeff(i,j)).norm();
        res[1] = std::atan2(-mat.coeff(i,k), c);
        if (c > epsilon)
        {
            res[0] = std::atan2(mat.coeff(j,k), mat.coeff(k,k));
            res[2] = std::atan2(mat.coeff(i,j), mat.coeff(i,i));
        }
        else
        {
            res[0] = Scalar(0);
            res[2] = (mat.coeff(i,k)>0?1:-1)*std::atan2(-mat.coeff(k,j), mat.coeff(j,j));
        }
    }
    if (!odd)
        res = -res;
    return res;
}

template<typename T>
inline Eigen::Quaternion<T> quaternionFromYawPitchRoll(T yaw, T pitch, T roll)
{
    return Eigen::Quaternion<T>(
            Eigen::AngleAxis<T>(yaw,   Eigen::Matrix<T,3,1>::UnitZ()) *
            Eigen::AngleAxis<T>(pitch, Eigen::Matrix<T,3,1>::UnitY()) *
            Eigen::AngleAxis<T>(roll,  Eigen::Matrix<T,3,1>::UnitX())
        );
}

template<typename T>
inline Eigen::Quaternion<T> quaternionFromYawPitchRoll(const boost::tuples::tuple<T,T,T>& ypr)
{
    return quaternionFromYawPitchRoll(ypr.template get<0>(),
                                      ypr.template get<1>(),
                                      ypr.template get<2>());
}


template<typename T>
inline void quaternionCovFromYawPitchRollCov(
                                const Eigen::Matrix<T,6,6>& eulerCovariance,
                                float yaw, float pitch, float roll,
                                Eigen::Quaternion<T>& oOrientation,
                                Eigen::Matrix<T,7,7>& oCovariance)
{
    Eigen::Matrix<T,7,6> J = Eigen::Matrix<T,7,6>::Identity();

    T cy = (T)cos(yaw*(T)0.5);
    T sy = (T)sin(yaw*(T)0.5);
    T cp = (T)cos(pitch*(T)0.5);
    T sp = (T)sin(pitch*(T)0.5);
    T cr = (T)cos(roll*(T)0.5);
    T sr = (T)sin(roll*(T)0.5);

    T ccc = cr*cp*cy;
    T ccs = cr*cp*sy;
    T csc = cr*sp*cy;
    T css = cr*sp*sy;
    T scc = sr*cp*cy;
    T scs = sr*cp*sy;
    T ssc = sr*sp*cy;
    T sss = sr*sp*sy;

    J.template block<4,3>(3,3) <<
            (T)0.5*( ssc -ccs), (T)0.5*( scs -csc), (T)0.5*( css -scc),
            (T)0.5*(-csc -scs), (T)0.5*(-ssc -ccs), (T)0.5*( ccc +sss),
            (T)0.5*( scc -css), (T)0.5*( ccc -sss), (T)0.5*( ccs -ssc),
            (T)0.5*( ccc +sss), (T)0.5*(-css -scc), (T)0.5*(-csc -scs);

    oCovariance = J * eulerCovariance * J.transpose();
    oOrientation = Eigen::Quaternion<T>(ccc+sss, scc-css, csc+scs, ccs-ssc);
}

template<typename T>
inline Eigen::Matrix<T,7,7> quaternionCovFromYawPitchRollCov(
                                const Eigen::Matrix<T,6,6>& eulerCovariance,
                                float yaw, float pitch, float roll)
{
    Eigen::Quaternion<T> q;
    Eigen::Matrix<T,7,7> cov;
    quaternionCovFromYawPitchRollCov(eulerCovariance,yaw,pitch,roll,q,cov);
    return cov;
}

template<typename T>
inline Eigen::Matrix<T,7,7> quaternionCovFromYawPitchRollCov(
                                const Eigen::Matrix<T,6,6>& eulerCovariance,
                                const Eigen::Quaternion<T>& rotation){
    T yaw, pitch, roll;
    boost::tie(yaw,pitch,roll) = quaternionToYawPitchRoll(rotation);
    return quaternionCovFromYawPitchRollCov(eulerCovariance,  yaw,pitch,roll);
}


template<typename T>
inline boost::tuples::tuple<T,T,T> quaternionToYawPitchRoll(
                                       const Eigen::Quaternion<T>& q)
{
    Eigen::Matrix<T,3,1> euler = eulerAngles(q.toRotationMatrix(), 2, 1, 0);
    return boost::tuples::make_tuple(euler(0,0), euler(1,0), euler(2,0));
}

template<typename T>
inline Eigen::Matrix<T,6,6> quaternionCovToYawPitchRollCov(
                                const Eigen::Matrix<T,7,7>& covariance,
                                const Eigen::Quaternion<T>& q)
{
    Eigen::Matrix<T,6,7> J = Eigen::Matrix<T,6,7>::Identity();

    // Generally there exist 3 cases for calculation of yaw, pitch and roll
    // from a quaternion:
    // 1. regular case
    // 2. gimbal lock upwards
    // 3. gimbal lock downwards
    // For these three cases the Jacobian is calculated

    // There exist different methods to calculate the yaw, pitch, roll angle
    // from a quaternion:
    //
    // 1. The current implementation of the Jacobian is built on the calculations
    // of yaw, pitch and roll from Eigen
    //
    // 2. A different calculation is described in the technical report
    // @TECHREPORT{blanco2010se3,
    //    author = {Blanco, Jos{\'{e}}-Luis},
    //    month = sep,
    //    title = {A tutorial on SE(3) transformation parameterizations and on-manifold optimization},
    //    year = {2010},
    //    institution = {University of Malaga}
    //}
    // http://mapir.isa.uma.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
    //
    // This calculation is simpler and faster. But since we were not sure, whether
    // it is compatible to the eigen calculation of the yaw, pitch and roll
    // it is uncommented until yet

    // the jacobian of the eigen conversion from quaternion to yaw, pitch, roll
    T a = (T)1.0 - (T)2.0 * (q.x() * q.x() + q.y() * q.y());
    T b = (T)2.0 * (q.y() * q.z() + q.w() * q.x());
    T c = sqrt(a*a + b*b);

    if(c > 1e-3)
    {
        // from eigen quaternionToYawPitchRoll
        // pitch = atan2(-2.0*(q.x()*q.z()-q.w()*q.y()), c)/M_PI*180.0
        // roll  = atan2(2.0*(q.z()*q.y()+q.w()*q.x()), (1.0 - 2.0*(q.x()*q.x()+q.y() * q.y())))/M_PI*180.0
        // yaw   = atan2(2.0 * (q.y() * q.x() + q.z() * q.w()) ,  (1.0 - 2.0*(q.y() * q.y() + q.z() * q.z())))/M_PI*180.0

        T t1 = (T) ((T) q.y() * (T) q.y());
        T t2 = 2 * t1;
        T t3 = (T) q.z() * (T) q.z();
        T t5 = 1 - t2 - 2 * t3;
        T t7 = t5 * t5;
        T t10 = (T) ((T) q.w() * (T) q.z() + (T) q.x() * (T) q.y());
        T t13 = 1 / (t7 + 4 * t10 * t10);
        T t31 = (T) q.x() * (T) q.x();
        T t34 = 1 - 2 * t31 - 2 * t1;
        T t35 = t34 * t34;
        T t38 = (T) q.y() * (T) q.z() + (T) q.w() * (T) q.x();
        T t39 = (T) (t38 * t38);
        T t40 = 4 * t39;
        T t42 = sqrt(t35 + t40);
        T t47 = -(T) q.x() * (T) q.z() + (T) q.w() * (T) q.y();
        T t48 = 2 * t47 * t38;
        T t49 = 1 / t42;
        T t56 = 1 / (t35 + t40 + 4 * t47 * t47);
        T t90 = 1 - 2 * t31 - t2;
        T t92 = t90 * t90;
        T t95 = 1 / (t92 + 4 * t38 * t38);

        J.template block<3,4>(3,3) <<
                2 * q.z() * t5 * t13,
                2 * q.y() * t5 * t13,
                (2 * q.x() * t5 + 8 * t10 * q.y()) * t13,
                (2 * q.w() * t5 + 8 * t10 * q.z()) * t13,

                (2 * (T) q.y() * t42 - 4 * (T) t48 * (T) q.x() * t49) * t56,
                (-2 * (T) q.z() * t42 - (T) t47 * (-8 * t34 * (T) q.x() + 8 * (T) t38 * (T) q.w()) * t49) * t56,
                (2 * (T) q.w() * t42 - (T) t47 * (-8 * t34 * (T) q.y() + 8 * (T) t38 * (T) q.z()) * t49) * t56,
                (-2 * (T) q.x() * t42 - 4 * (T) t48 * (T) q.y() * t49) * t56,

                2 * (T) q.x() * t90 * t95,
                (2 * (T) q.w() * t90 + 8 * t38 * (T) q.x()) * t95,
                (2 * (T) q.z() * t90 + 8 * t38 * (T) q.y()) * t95,
                2 * (T) q.y() * t90 * t95;


        // from technical report
        // pitch = asin(2.0*(q.w()*q.y()-q.x()*q.z())); (differs from eigen)
        // roll  = atan2(2.0*(q.w()*q.x()+q.y()*q.z()), (1.0 - 2.0*(q.x()*q.x()+q.y()*q.y()))); (like eigen)
        // yaw   = atan2(2.0*(q.w()*q.z()+q.x()*q.y()), (1.0 - 2.0*(q.y()*q.y()+q.z()*q.z()))); (like eigen)

        //T t1 = (T) ((T) q.y() * (T) q.y());
        //T t2 = 2 * t1;
        //T t3 = (T) ((T) q.z() * (T) q.z());
        //T t5 = 1 - t2 - 2 * t3;
        //T t7 = t5 * t5;
        //T t10 = (T) ((T) q.w() * (T) q.z() + (T) q.x() * (T) q.y());
        //T t13 = 1 / (t7 + 4 * t10 * t10);
        //T t31 = (T) ((T) q.w() * (T) q.w());
        //T t38 = (T) ((T) q.x() * (T) q.x());
        //T t42 = sqrt((T) (1 - 4 * t31 * t1 + 8 * q.w() * q.y() * q.x() * q.z() - 4 * t38 * t3));
        //T t43 = 0.1e1 / t42;
        //T t53 = 1 - 2 * t38 - t2;
        //T t55 = t53 * t53;
        //T t58 = q.w() * q.x() + q.y() * q.z();
        //T t61 = 1 / (t55 + 4 * t58 * t58);
        //
        //J.template block<3,4>(3,3) <<
        //  2 * q.z() * t5 * t13,
        //  2 * q.y() * t5 * t13,
        //  (2 * q.x() * t5 + 8 * t10 * q.y()) * t13,
        //  (2 * q.w() * t5 + 8 * t10 * q.z()) * t13,
        //  (T) (0.2e1 * (T) q.y() * t43),
        //  -(T) (0.2e1 * (T) q.z() * t43),
        //  (T) (0.2e1 * (T) q.w() * t43),
        //  -(T) (0.2e1 * (T) q.x() * t43),
        //  2 * q.x() * t53 * t61,
        //  (2 * q.w() * t53 + 8 * t58 * q.x()) * t61,
        //  (2 * q.z() * t53 + 8 * t58 * q.y()) * t61,
        //  2 * q.y() * t53 * t61;
    }
    else{

        // for eigen
        T t1 = q.x() * q.x();
        T t3 = q.y() * q.y();
        T t5 = (T)0.10e1 - (T)0.20e1 * t1 - (T)0.20e1 * t3;
        T t6 = t5 * t5;
        T t9 = q.y() * q.z() + q.w() * q.x();
        T t10 = t9 * t9;
        T t11 = (T)0.400e1 * t10;
        T t13 = sqrt(t6 + t11);
        T t16 = q.x() * q.z();
        T t18 = q.w() * q.y();
        T t20 = (T)-0.20e1 * t16 + (T)0.20e1 * t18;
        T t22 = (T)0.1e1 / t13;
        T t27 = t20 * t20;
        T t33 = -t16 + t18;
        T t45 = (T)0.1e1 / (t6 + t11 + (T)0.4e1 * t33 * t33);
        T t67 = (T)0.2e1 * t1;
        T t68 = (T) (q.z() * q.z());
        T t70 = (T)0.1e1 - t67 - (T) (2 * t68);
        T t72 = t70 * t70;
        T t77 = (T)-0.20e1 * q.x() * q.y() + (T)0.20e1 * q.w() * q.z();
        T t78 = t77 * t77;
        T t80 = (T)0.1e1 / (t72 + t78);
        T t96 = (T)0.1e1 - t67 - (T)0.2e1 * t3;
        T t98 = t96 * t96;

        // for technical report
        //T t1 = (T) q.w() * (T) q.w();
        //T t2 = (T) q.x() * (T) q.x();
        //T t4 = 1 / (T) (t1 + t2);

        if(q.x()*q.z() - q.w() * q.y() > 0){
            // from eigen quaternionToYawPitchRoll
            //pitch = atan2(-2.0*(q.x()*q.z()-q.w()*q.y()), c)/M_PI*180.0
            //roll = +atan2(-2.0 * (q.x()*q.y()-q.w() * q.z()), (1.0 - 2.0*(q.x() * q.x() + q.z() * q.z())))/M_PI * 180.0
            //yaw = 0

            J.template block<3,4>(3,3) <<
                    0,0,0,0,

                    (T) ((0.20e1 * q.y() * t13 - 0.4000000000e1 * t20 * t9 * q.x() * t22) / (t6 + t11 + t27)),
                    (T) ((-0.2e1 * q.z() * t13 - t33 * (-0.80e1 * q.x() * t5 + 0.800e1 * t9 * q.w()) * t22) * t45),
                    (T) ((0.2e1 * q.w() * t13 - t33 * (-0.80e1 * t5 * q.y() + 0.800e1 * t9 * q.z()) * t22) * t45),
                    (T) ((-0.2e1 * q.x() * t13 - 0.8000000000e1 * t33 * t9 * q.y() * t22) * t45),

                    (T) (0.20e1 * q.z() * t70 * t80),
                    (T) ((-0.20e1 * q.y() * t70 + 0.4e1 * t77 * q.x()) * t80),
                    -(T) (0.20e1 * q.x() * t70 / (t72 * t70 + t78)),
                    (T) (0.2e1 * q.y() * t96 / (t98 + 0.4e1 * t9 * t9));

            // from technical report
            // pitch = -M_PI/2.0; (differs from eigen)
            // roll  = 0; (differs from eigen)
            // yaw   = 2.0 * atan2(q.x(), q.w()); (differs from eigen)

            //J.template block<3,4>(3,3) <<
            //      -2 * q.x() * t4, 2 * q.w() * t4, 0, 0,
            //      0,0,0,0,
            //      0,0,0,0;
        }
        else{
            // from eigen quaternionToYawPitchRoll
            //pitch = atan2(-2.0*(q.x()*q.z()-q.w()*q.y()), c)/M_PI*180.0
            //roll = -atan2(-2.0 * (q.x()*q.y()-q.w() * q.z()), (1.0 - 2.0*(q.x() * q.x() + q.z() * q.z())))/M_PI * 180.0
            //yaw = 0

            J.template block<3,4>(3,3) <<
                    0,0,0,0,

                    (T) ((0.20e1 * q.y() * t13 - 0.4000000000e1 * t20 * t9 * q.x() * t22) / (t6 + t11 + t27)),
                    (T) ((-0.2e1 * q.z() * t13 - t33 * (-0.80e1 * q.x() * t5 + 0.800e1 * t9 * q.w()) * t22) * t45),
                    (T) ((0.2e1 * q.w() * t13 - t33 * (-0.80e1 * t5 * q.y() + 0.800e1 * t9 * q.z()) * t22) * t45),
                    (T) ((-0.2e1 * q.x() * t13 - 0.8000000000e1 * t33 * t9 * q.y() * t22) * t45),

                    -(T) (0.20e1 * q.z() * t70 * t80),
                    -(T) ((-0.20e1 * q.y() * t70 + 0.4e1 * t77 * q.x()) * t80),
                    (T) (0.20e1 * q.x() * t70 / (t72 * t70 + t78)),
                    -(T) (0.2e1 * q.y() * t96 / (t98 + 0.4e1 * t9 * t9));

            // from technical report
            // pitch = M_PI/2.0; (differs from eigen)
            // roll  = 0; (differs from eigen)
            // yaw   = -2.0 * atan2(q.x(), q.w()); (differs from eigen)

            //J.template block<3,4>(3,3) <<
            //      2 * q.x() * t4, -2 * q.w() * t4, 0, 0,
            //      0,0,0,0,
            //      0,0,0,0;

        }
    }

    return J * covariance * J.transpose();
}


template<typename T>
inline Eigen::Matrix<T,3,3> matrixFromYawPitchRoll(T yaw, T pitch, T roll)
{
    typedef Eigen::Matrix<T,3,3> Mat3;
    Mat3 Rx, Ry, Rz;

    float s1 = sin(roll);
    float c1 = cos(roll);
    Rx <<   1,  0,  0,
            0, c1,-s1,
            0, s1, c1;

    float s2 = sin(pitch);
    float c2 = cos(pitch);
    Ry <<  c2,  0, s2,
            0,  1,  0,
          -s2,  0, c2;

    float s3 = sin(yaw);
    float c3 = cos(yaw);
    Rz <<  c3,-s3,  0,
           s3, c3,  0,
            0,  0,  1;

    return Rx*Ry*Rz; // operation order is from right to left
}

template<typename T>
inline Eigen::Matrix<T,3,3> matrixFromYawPitchRoll(const boost::tuples::tuple<T,T,T>& ypr)
{
    return matrixFromYawPitchRoll(ypr.template get<0>(),
                                  ypr.template get<1>(),
                                  ypr.template get<2>());
}

template<typename T>
inline boost::tuples::tuple<T,T,T> matrixToYawPitchRoll(const Eigen::Matrix<T,3,3>& r)
{
    Eigen::Matrix<T,3,1> euler = eulerAngles(r, 2, 1, 0);
    return boost::tuples::make_tuple(euler(0,0), euler(1,0), euler(2,0));
}


template<typename T>
inline void derivMatricesFromYawPitchRoll(T yaw, T pitch, T roll,
                                          Eigen::Matrix<T,3,3>& oR_dyaw,
                                          Eigen::Matrix<T,3,3>& oR_dpitch,
                                          Eigen::Matrix<T,3,3>& oR_droll)
{
    typedef Eigen::Matrix<T,3,3> Mat3;
    Mat3 Rx, Ry, Rz, dRx_droll, dRy_dpitch, dRz_dyaw;

    float s1 = sin(roll);
    float c1 = cos(roll);
    Rx <<   1,  0,  0,
            0, c1,-s1,
            0, s1, c1;

    dRx_droll <<  0,  0,  0,
                  0,-s1,-c1,
                  0, c1,-s1;

    float s2 = sin(pitch);
    float c2 = cos(pitch);
    Ry <<  c2,  0, s2,
            0,  1,  0,
          -s2,  0, c2;

    dRy_dpitch <<  -s2,  0, c2,
                     0,  0,  0,
                   -c2,  0,-s2;

    float s3 = sin(yaw);
    float c3 = cos(yaw);
    Rz <<  c3,-s3,  0,
           s3, c3,  0,
            0,  0,  1;

    dRz_dyaw << -s3,-c3,  0,
                 c3,-s3,  0,
                  0,  0,  0;


    oR_dyaw   = Rx*Ry*dRz_dyaw;
    oR_dpitch = Rx*dRy_dpitch*Rz;
    oR_droll  = dRx_droll*Ry*Rz;
}


} // namespace mira


// injecting the methods to Eigen namespace resolves the the
// "unqualified lookup" problem which arises on compilers that implement
// the C++ standard very closely. By injecting the methods into the Eigen
// namespace the compiler can do argument-dependent lookup (ADL).
namespace Eigen {

using mira::quaternionFromYawPitchRoll;
using mira::quaternionCovFromYawPitchRollCov;
using mira::quaternionToYawPitchRoll;
using mira::quaternionCovToYawPitchRollCov;
using mira::matrixFromYawPitchRoll;
using mira::matrixToYawPitchRoll;
using mira::derivMatricesFromYawPitchRoll;

}


#endif