CQuaternion_unittest.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include <mrpt/math/CQuaternion.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPose3DQuat.h>
#include <gtest/gtest.h>
#include <CTraitsTest.h>

using namespace mrpt;
using namespace mrpt::poses;

using namespace mrpt::math;
using namespace std;

template class mrpt::CTraitsTest<mrpt::poses::CPose3DQuat>;
template class mrpt::CTraitsTest<mrpt::poses::CPose3D>;
template class mrpt::CTraitsTest<mrpt::math::CQuaternionDouble>;

class QuaternionTests : public ::testing::Test
{
   protected:
    virtual void SetUp() {}
    virtual void TearDown() {}
    void test_gimbalLock(double YAW, double PITCH, double ROLL)
    {
        CQuaternionDouble q1, q1r;
        CPose3D p1(0, 0, 0, YAW, PITCH, ROLL);
        p1.getAsQuaternion(q1);
        CPose3D(q1, 0, 0, 0).getAsQuaternion(q1r);

        EXPECT_NEAR(
            0, std::abs(
                   (CPose3D(q1, 0, 0, 0).getAsVectorVal() -
                    CPose3D(q1r, 0, 0, 0).getAsVectorVal())
                       .sum()),
            1e-6);
    }

    void test_toYPRAndBack(double YAW, double PITCH, double ROLL)
    {
        CPose3D p1(0, 0, 0, YAW, PITCH, ROLL);
        CPose3DQuat q1(p1);
        CPose3D p2 = CPose3D(q1);

        EXPECT_NEAR(
            0, (p1.getRotationMatrix() - p2.getRotationMatrix())
                   .array()
                   .abs()
                   .sum(),
            1e-4)
            << "ypr->quat->ypr failed with:" << endl
            << "  p1:" << p1 << endl
            << "  q1:" << q1 << endl
            << "  p2:" << p2 << endl;

        CPose3D p3(q1.quat(), q1[0], q1[1], q1[2]);
        EXPECT_NEAR(
            0, (p1.getRotationMatrix() - p3.getRotationMatrix())
                   .array()
                   .abs()
                   .sum(),
            1e-4)
            << "pose constructor from quat failed with:" << endl
            << "  p1:" << p1 << endl
            << "  q1:" << q1 << endl
            << "  p3:" << p3 << endl;
    }

    void test_lnAndExpMatches(double yaw1, double pitch1, double roll1)
    {
        const CPose3D pp(0, 0, 0, yaw1, pitch1, roll1);
        CQuaternionDouble q1;
        pp.getAsQuaternion(q1);

        mrpt::math::CVectorDouble q1_ln = q1.ln<mrpt::math::CVectorDouble>();
        const CQuaternionDouble q2 = CQuaternionDouble::exp(q1_ln);

        // q2 should be == q1
        EXPECT_NEAR(0, (q1 - q2).array().abs().sum(), 1e-10) << "q1:\n"
                                                             << q1 << endl
                                                             << "q2:\n"
                                                             << q2 << endl
                                                             << "Error:\n"
                                                             << (q1 - q2)
                                                             << endl;
    }

    void test_ExpAndLnMatches(double v0, double v1, double v2)
    {
        CArrayDouble<3> v;
        v[0] = v0;
        v[1] = v1;
        v[2] = v2;

        const CQuaternionDouble q1 = CQuaternionDouble::exp(v);
        CArrayDouble<3> q1_ln = q1.ln<CArrayDouble<3>>();

        // q1_ln should be == v
        EXPECT_NEAR(0, (q1_ln - v).array().abs().sum(), 1e-10) << "v:\n"
                                                               << v << endl
                                                               << "q1_ln:\n"
                                                               << q1_ln << endl
                                                               << "Error:\n"
                                                               << (q1_ln - v)
                                                               << endl;
    }
};

TEST_F(QuaternionTests, crossProduct)
{
    CQuaternionDouble q1, q2, q3;

    // q1 = CQuaternionDouble(1,2,3,4); q1.normalize();
    CPose3D p1(0, 0, 0, DEG2RAD(10), DEG2RAD(30), DEG2RAD(-20));
    p1.getAsQuaternion(q1);

    CPose3D p2(0, 0, 0, DEG2RAD(30), DEG2RAD(-20), DEG2RAD(10));
    p2.getAsQuaternion(q2);

    // q3 = q1 x q2
    q3.crossProduct(q1, q2);

    const CPose3D p3 = p1 + p2;

    EXPECT_NEAR(
        0, std::abs(
               (p3.getAsVectorVal() - CPose3D(q3, 0, 0, 0).getAsVectorVal())
                   .sum()),
        1e-6)
        << "q1 = " << q1 << endl
        << "q1 as CPose3D = " << CPose3D(q1, 0, 0, 0) << endl
        << "q2 = " << q2 << endl
        << "q2 as CPose3D = " << CPose3D(q2, 0, 0, 0) << endl
        << "q3 = q1 * q2 = " << q3 << endl
        << "q3 as CPose3D = " << CPose3D(q3, 0, 0, 0) << endl
        << "Should be equal to p3 = p1 (+) p2 = " << p3 << endl;
}

// Use special cases: gimbal lock:
TEST_F(QuaternionTests, gimbalLock)
{
    test_gimbalLock(DEG2RAD(20), DEG2RAD(90), DEG2RAD(0));
    test_gimbalLock(DEG2RAD(20), DEG2RAD(-90), DEG2RAD(0));
}

TEST_F(QuaternionTests, ToYPRAndBack)
{
    test_toYPRAndBack(DEG2RAD(20), DEG2RAD(30), DEG2RAD(40));
    test_toYPRAndBack(DEG2RAD(20), DEG2RAD(30), DEG2RAD(40));
    test_toYPRAndBack(DEG2RAD(30), DEG2RAD(90), DEG2RAD(0));
    test_toYPRAndBack(DEG2RAD(-30), DEG2RAD(90), DEG2RAD(0));
    test_toYPRAndBack(DEG2RAD(-30), DEG2RAD(88), DEG2RAD(60));
    test_toYPRAndBack(DEG2RAD(-30), DEG2RAD(10), DEG2RAD(60));
}

TEST_F(QuaternionTests, LnAndExpMatches)
{
    const double list_test_YPR_angles_degrees[][3] = {
        {0, 0, 0},   {-1, 0, 0},    {1, 0, 0},      {0, -1, 0},
        {0, 1, 0},   {0, 0, -1},    {0, 0, 1},      {40, 0, 0},
        {0, 40, 0},  {0, 0, 40},    {-40, 0, 0},    {0, -40, 0},
        {0, 0, -40}, {-30, 20, 50}, {-30, 20, -50}, {30, -20, -50}};

    const size_t N = sizeof(list_test_YPR_angles_degrees) /
                     sizeof(list_test_YPR_angles_degrees[0]);
    for (size_t i = 0; i < N; i++)
        test_lnAndExpMatches(
            DEG2RAD(list_test_YPR_angles_degrees[i][0]),
            DEG2RAD(list_test_YPR_angles_degrees[i][1]),
            DEG2RAD(list_test_YPR_angles_degrees[i][2]));
}

TEST_F(QuaternionTests, ExpAndLnMatches)
{
    const double list_test_XYZ[][3] = {
        {0, 0, 0},          {-1, 0, 0},        {1, 0, 0},
        {0, -1, 0},         {0, 1, 0},         {0, 0, -1},
        {0, 0, 1},          {1e-3, 0, 0},     {0, 1e-3, 0},
        {0, 0, 1e-3},       {-1e-3, 0, 0},   {0, -1e-3, 0},
        {0, 0, -1e-3},      {-0.1, 0.2, 0.3},  {-0.1, -0.2, 0.3},
        {-0.1, -0.2, -0.3}, {-0.1, 0.2, -0.3}, {0.1, 0.2, -0.3},
        {0.1, 0.2, 0.3}};

    const size_t N = sizeof(list_test_XYZ) / sizeof(list_test_XYZ[0]);
    for (size_t i = 0; i < N; i++)
        test_ExpAndLnMatches(
            list_test_XYZ[i][0], list_test_XYZ[i][1], list_test_XYZ[i][2]);
}