CPose3D_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/poses/CPose2D.h>
#include <mrpt/poses/CPose3D.h>
#include <mrpt/poses/CPoint3D.h>
#include <mrpt/math/num_jacobian.h>
#include <CTraitsTest.h>
#include <gtest/gtest.h>

using namespace mrpt;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace std;

template class mrpt::CTraitsTest<CPose3D>;

class Pose3DTests : public ::testing::Test
{
   protected:
    virtual void SetUp() {}
    virtual void TearDown() {}
    void test_inverse(
        double x1, double y1, double z1, double yaw1, double pitch1,
        double roll1)
    {
        const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);

        const CMatrixDouble44 HM =
            p1.getHomogeneousMatrixVal<CMatrixDouble44>();
        CMatrixDouble44 HMi;
        p1.getInverseHomogeneousMatrix(HMi);

        CMatrixDouble44 I4;
        I4.unit(4, 1.0);

        EXPECT_NEAR((HM * HMi - I4).array().abs().sum(), 0, 1e-3)
            << "HM:\n"
            << HM << "inv(HM):\n"
            << HMi << "inv(HM)*HM:\n"
            << HM * HMi << endl;

        CPose3D p1_inv_inv = p1;

        p1_inv_inv.inverse();
        const CMatrixDouble44 HMi_from_p1_inv =
            p1_inv_inv.getHomogeneousMatrixVal<CMatrixDouble44>();

        p1_inv_inv.inverse();

        EXPECT_NEAR(
            (p1.getAsVectorVal() - p1_inv_inv.getAsVectorVal())
                .array()
                .abs()
                .sum(),
            0, 1e-3)
            << "p1: " << p1 << "p1_inv_inv: " << p1_inv_inv << endl;

        EXPECT_NEAR((HMi_from_p1_inv - HMi).array().abs().sum(), 0, 1e-4)
            << "HMi_from_p1_inv:\n"
            << HMi_from_p1_inv << "HMi:\n"
            << HMi << endl;
    }

    void test_compose(
        double x1, double y1, double z1, double yaw1, double pitch1,
        double roll1, double x2, double y2, double z2, double yaw2,
        double pitch2, double roll2)
    {
        const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
        const CPose3D p2(x2, y2, z2, yaw2, pitch2, roll2);

        const CPose3D p1_c_p2 = p1 + p2;
        const CPose3D p1_i_p2 = p1 - p2;

        const CPose3D p1_c_p2_i_p2 = p1_c_p2 - p1;  // should be -> p2
        const CPose3D p2_c_p1_i_p2 = p2 + p1_i_p2;  // Should be -> p1

        EXPECT_NEAR(
            0,
            (p1_c_p2_i_p2.getAsVectorVal() - p2.getAsVectorVal())
                .array()
                .abs()
                .sum(),
            1e-5)
            << "p2          : " << p2 << endl
            << "p1_c_p2_i_p2: " << p1_c_p2_i_p2 << endl;

        EXPECT_NEAR(
            0,
            (p2_c_p1_i_p2.getAsVectorVal() - p1.getAsVectorVal())
                .array()
                .abs()
                .sum(),
            1e-5)
            << "p1          : " << p1 << endl
            << "p2          : " << p2 << endl
            << "p2 matrix   : " << endl
            << p2.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
            << "p1_i_p2     : " << p1_i_p2 << endl
            << "p1_i_p2 matrix: " << endl
            << p1_i_p2.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
            << "p2_c_p1_i_p2: " << p2_c_p1_i_p2 << endl;

        // Test + operator: trg new var
        {
            CPose3D C = p1;
            CPose3D A = C + p2;
            EXPECT_NEAR(
                0,
                (A.getAsVectorVal() - p1_c_p2.getAsVectorVal())
                    .array()
                    .abs()
                    .sum(),
                1e-6);
        }
        // Test + operator: trg same var
        {
            CPose3D A = p1;
            A = A + p2;
            EXPECT_NEAR(
                0,
                (A.getAsVectorVal() - p1_c_p2.getAsVectorVal())
                    .array()
                    .abs()
                    .sum(),
                1e-6);
        }
        // Test =+ operator
        {
            CPose3D A = p1;
            A += p2;
            EXPECT_NEAR(
                0,
                (A.getAsVectorVal() - p1_c_p2.getAsVectorVal())
                    .array()
                    .abs()
                    .sum(),
                1e-6);
        }
    }

    void test_to_from_2d(double x, double y, double phi)
    {
        const CPose2D p2d = CPose2D(x, y, phi);
        const CPose3D p3d = CPose3D(p2d);

        const CPose2D p2d_bis = CPose2D(p3d);

        EXPECT_FLOAT_EQ(p2d.x(), p2d_bis.x()) << "p2d: " << p2d << endl;
        EXPECT_FLOAT_EQ(p2d.y(), p2d_bis.y()) << "p2d: " << p2d << endl;
        EXPECT_FLOAT_EQ(p2d.phi(), p2d_bis.phi()) << "p2d: " << p2d << endl;

        EXPECT_FLOAT_EQ(p2d.phi(), p3d.yaw()) << "p2d: " << p2d << endl;
    }

    void test_composeFrom(
        double x1, double y1, double z1, double yaw1, double pitch1,
        double roll1, double x2, double y2, double z2, double yaw2,
        double pitch2, double roll2)
    {
        const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
        const CPose3D p2(x2, y2, z2, yaw2, pitch2, roll2);

        const CPose3D p1_plus_p2 = p1 + p2;

        {
            CPose3D p1_plus_p2bis;
            p1_plus_p2bis.composeFrom(p1, p2);

            EXPECT_NEAR(
                0,
                (p1_plus_p2bis.getAsVectorVal() - p1_plus_p2.getAsVectorVal())
                    .array()
                    .abs()
                    .sum(),
                1e-5)
                << "p2 : " << p2 << endl
                << "p1 : " << p1 << endl
                << "p1_plus_p2    : " << p1_plus_p2 << endl
                << "p1_plus_p2bis : " << p1_plus_p2bis << endl;
        }

        {
            CPose3D p1_plus_p2bis = p1;
            p1_plus_p2bis.composeFrom(p1_plus_p2bis, p2);

            EXPECT_NEAR(
                0,
                (p1_plus_p2bis.getAsVectorVal() - p1_plus_p2.getAsVectorVal())
                    .array()
                    .abs()
                    .sum(),
                1e-5)
                << "p2 : " << p2 << endl
                << "p1 : " << p1 << endl
                << "p1_plus_p2    : " << p1_plus_p2 << endl
                << "p1_plus_p2bis : " << p1_plus_p2bis << endl;
        }

        {
            CPose3D p1_plus_p2bis = p2;
            p1_plus_p2bis.composeFrom(p1, p1_plus_p2bis);

            EXPECT_NEAR(
                0,
                (p1_plus_p2bis.getAsVectorVal() - p1_plus_p2.getAsVectorVal())
                    .array()
                    .abs()
                    .sum(),
                1e-5)
                << "p2 : " << p2 << endl
                << "p1 : " << p1 << endl
                << "p1_plus_p2    : " << p1_plus_p2 << endl
                << "p1_plus_p2bis : " << p1_plus_p2bis << endl;
        }
    }

    void test_composePoint(
        double x1, double y1, double z1, double yaw1, double pitch1,
        double roll1, double x, double y, double z)
    {
        const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
        const CPoint3D p(x, y, z);
        CPoint3D p1_plus_p = p1 + p;

        CPoint3D p1_plus_p2;
        p1.composePoint(
            p.x(), p.y(), p.z(), p1_plus_p2.x(), p1_plus_p2.y(),
            p1_plus_p2.z());

        EXPECT_NEAR(
            0,
            (p1_plus_p2.getAsVectorVal() - p1_plus_p.getAsVectorVal())
                .array()
                .abs()
                .sum(),
            1e-5);

        // Repeat using same input/output variables:
        {
            double lx = p.x();
            double ly = p.y();
            double lz = p.z();

            p1.composePoint(lx, ly, lz, lx, ly, lz);
            EXPECT_NEAR(
                0,
                std::abs(lx - p1_plus_p.x()) + std::abs(ly - p1_plus_p.y()) +
                    std::abs(lz - p1_plus_p.z()),
                1e-5);
        }

        // Inverse:
        CPoint3D p_recov = p1_plus_p - p1;
        CPoint3D p_recov2;
        p1.inverseComposePoint(
            p1_plus_p.x(), p1_plus_p.y(), p1_plus_p.z(), p_recov2.x(),
            p_recov2.y(), p_recov2.z());

        EXPECT_NEAR(
            0,
            (p_recov2.getAsVectorVal() - p_recov.getAsVectorVal())
                .array()
                .abs()
                .sum(),
            1e-5);

        EXPECT_NEAR(
            0,
            (p.getAsVectorVal() - p_recov.getAsVectorVal()).array().abs().sum(),
            1e-5);
    }

    static void func_compose_point(
        const CArrayDouble<6 + 3>& x, const double& dummy, CArrayDouble<3>& Y)
    {
        MRPT_UNUSED_PARAM(dummy);
        CPose3D q(x[0], x[1], x[2], x[3], x[4], x[5]);
        const CPoint3D p(x[6 + 0], x[6 + 1], x[6 + 2]);
        const CPoint3D pp = q + p;
        for (int i = 0; i < 3; i++) Y[i] = pp[i];
    }

    static void func_inv_compose_point(
        const CArrayDouble<6 + 3>& x, const double& dummy, CArrayDouble<3>& Y)
    {
        MRPT_UNUSED_PARAM(dummy);
        CPose3D q(x[0], x[1], x[2], x[3], x[4], x[5]);
        const CPoint3D p(x[6 + 0], x[6 + 1], x[6 + 2]);
        const CPoint3D pp = p - q;
        Y[0] = pp.x();
        Y[1] = pp.y();
        Y[2] = pp.z();
    }

    void test_composePointJacob(
        double x1, double y1, double z1, double yaw1, double pitch1,
        double roll1, double x, double y, double z, bool use_aprox = false)
    {
        const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
        const CPoint3D p(x, y, z);

        CMatrixFixedNumeric<double, 3, 3> df_dpoint;
        CMatrixFixedNumeric<double, 3, 6> df_dpose;

        TPoint3D pp;
        p1.composePoint(
            x, y, z, pp.x, pp.y, pp.z, &df_dpoint, &df_dpose, nullptr,
            use_aprox);

        // Numerical approx:
        CMatrixFixedNumeric<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
        CMatrixFixedNumeric<double, 3, 6> num_df_dpose(UNINITIALIZED_MATRIX);
        {
            CArrayDouble<6 + 3> x_mean;
            for (int i = 0; i < 6; i++) x_mean[i] = p1[i];
            x_mean[6 + 0] = x;
            x_mean[6 + 1] = y;
            x_mean[6 + 2] = z;

            double DUMMY = 0;
            CArrayDouble<6 + 3> x_incrs;
            x_incrs.assign(1e-7);
            CMatrixDouble numJacobs;
            mrpt::math::estimateJacobian(
                x_mean,
                std::function<void(
                    const CArrayDouble<6 + 3>& x, const double& dummy,
                    CArrayDouble<3>& Y)>(&func_compose_point),
                x_incrs, DUMMY, numJacobs);

            numJacobs.extractMatrix(0, 0, num_df_dpose);
            numJacobs.extractMatrix(0, 6, num_df_dpoint);
        }

        const double max_eror = use_aprox ? 0.1 : 3e-3;

        EXPECT_NEAR(
            0, (df_dpoint - num_df_dpoint).array().abs().sum(), max_eror)
            << "p1: " << p1 << endl
            << "p:  " << p << endl
            << "Numeric approximation of df_dpoint: " << endl
            << num_df_dpoint << endl
            << "Implemented method: " << endl
            << df_dpoint << endl
            << "Error: " << endl
            << df_dpoint - num_df_dpoint << endl;

        EXPECT_NEAR(0, (df_dpose - num_df_dpose).array().abs().sum(), max_eror)
            << "p1: " << p1 << endl
            << "p:  " << p << endl
            << "Numeric approximation of df_dpose: " << endl
            << num_df_dpose << endl
            << "Implemented method: " << endl
            << df_dpose << endl
            << "Error: " << endl
            << df_dpose - num_df_dpose << endl;
    }

    void test_ExpLnEqual(
        double x1, double y1, double z1, double yaw1, double pitch1,
        double roll1)
    {
        const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);

        const CPose3D p2 = CPose3D::exp(p1.ln());
        EXPECT_NEAR(
            (p1.getAsVectorVal() - p2.getAsVectorVal()).array().abs().sum(), 0,
            1e-5)
            << "p1: " << p1 << endl;
    }

    void test_invComposePointJacob(
        double x1, double y1, double z1, double yaw1, double pitch1,
        double roll1, double x, double y, double z)
    {
        const CPose3D p1(x1, y1, z1, yaw1, pitch1, roll1);
        const CPoint3D p(x, y, z);

        CMatrixFixedNumeric<double, 3, 3> df_dpoint;
        CMatrixFixedNumeric<double, 3, 6> df_dpose;

        TPoint3D pp;
        p1.inverseComposePoint(
            x, y, z, pp.x, pp.y, pp.z, &df_dpoint, &df_dpose);

        // Numerical approx:
        CMatrixFixedNumeric<double, 3, 3> num_df_dpoint(UNINITIALIZED_MATRIX);
        CMatrixFixedNumeric<double, 3, 6> num_df_dpose(UNINITIALIZED_MATRIX);
        {
            CArrayDouble<6 + 3> x_mean;
            for (int i = 0; i < 6; i++) x_mean[i] = p1[i];
            x_mean[6 + 0] = x;
            x_mean[6 + 1] = y;
            x_mean[6 + 2] = z;

            double DUMMY = 0;
            CArrayDouble<6 + 3> x_incrs;
            x_incrs.assign(1e-7);
            CMatrixDouble numJacobs;
            mrpt::math::estimateJacobian(
                x_mean,
                std::function<void(
                    const CArrayDouble<6 + 3>& x, const double& dummy,
                    CArrayDouble<3>& Y)>(&func_inv_compose_point),
                x_incrs, DUMMY, numJacobs);

            numJacobs.extractMatrix(0, 0, num_df_dpose);
            numJacobs.extractMatrix(0, 6, num_df_dpoint);
        }

        EXPECT_NEAR(0, (df_dpoint - num_df_dpoint).array().abs().sum(), 3e-3)
            << "p1: " << p1 << endl
            << "p:  " << p << endl
            << "Numeric approximation of df_dpoint: " << endl
            << num_df_dpoint << endl
            << "Implemented method: " << endl
            << df_dpoint << endl
            << "Error: " << endl
            << df_dpoint - num_df_dpoint << endl;

        EXPECT_NEAR(0, (df_dpose - num_df_dpose).array().abs().sum(), 3e-3)
            << "p1: " << p1 << endl
            << "p:  " << p << endl
            << "Numeric approximation of df_dpose: " << endl
            << num_df_dpose << endl
            << "Implemented method: " << endl
            << df_dpose << endl
            << "Error: " << endl
            << df_dpose - num_df_dpose << endl;
    }

    void test_default_values(const CPose3D& p, const std::string& label)
    {
        EXPECT_EQ(p.x(), 0);
        EXPECT_EQ(p.y(), 0);
        EXPECT_EQ(p.z(), 0);
        EXPECT_EQ(p.yaw(), 0);
        EXPECT_EQ(p.pitch(), 0);
        EXPECT_EQ(p.roll(), 0);
        for (size_t i = 0; i < 4; i++)
            for (size_t j = 0; j < 4; j++)
                EXPECT_NEAR(
                    p.getHomogeneousMatrixVal<CMatrixDouble44>()(i, j),
                    i == j ? 1.0 : 0.0, 1e-8)
                    << "Failed for (i,j)=" << i << "," << j << endl
                    << "Matrix is: " << endl
                    << p.getHomogeneousMatrixVal<CMatrixDouble44>() << endl
                    << "case was: " << label << endl;
    }

    static void func_compose_point_se3(
        const CArrayDouble<6>& x, const CArrayDouble<3>& P, CArrayDouble<3>& Y)
    {
        CPose3D q = CPose3D::exp(x);
        const CPoint3D p(P[0], P[1], P[2]);
        const CPoint3D pp = q + p;
        for (int i = 0; i < 3; i++) Y[i] = pp[i];
    }

    static void func_invcompose_point_se3(
        const CArrayDouble<6>& x, const CArrayDouble<3>& P, CArrayDouble<3>& Y)
    {
        CPose3D q = CPose3D::exp(x);
        const CPoint3D p(P[0], P[1], P[2]);
        const CPoint3D pp = p - q;
        for (int i = 0; i < 3; i++) Y[i] = pp[i];
    }

    void test_composePointJacob_se3(const CPose3D& p, const TPoint3D x_l)
    {
        CMatrixFixedNumeric<double, 3, 6> df_dse3;

        TPoint3D pp;
        p.composePoint(
            x_l.x, x_l.y, x_l.z, pp.x, pp.y, pp.z, nullptr, nullptr, &df_dse3);

        // Numerical approx:
        CMatrixFixedNumeric<double, 3, 6> num_df_dse3(UNINITIALIZED_MATRIX);
        {
            CArrayDouble<6> x_mean;
            for (int i = 0; i < 6; i++) x_mean[i] = 0;

            CArrayDouble<3> P;
            for (int i = 0; i < 3; i++) P[i] = pp[i];

            CArrayDouble<6> x_incrs;
            x_incrs.assign(1e-9);
            mrpt::math::estimateJacobian(
                x_mean,
                std::function<void(
                    const CArrayDouble<6>& x, const CArrayDouble<3>& P,
                    CArrayDouble<3>& Y)>(&func_compose_point_se3),
                x_incrs, P, num_df_dse3);
        }

        EXPECT_NEAR(0, (df_dse3 - num_df_dse3).array().abs().sum(), 3e-3)
            << "p: " << p << endl
            << "x_l:  " << x_l << endl
            << "Numeric approximation of df_dse3: " << endl
            << num_df_dse3 << endl
            << "Implemented method: " << endl
            << df_dse3 << endl
            << "Error: " << endl
            << df_dse3 - num_df_dse3 << endl;
    }

    void test_invComposePointJacob_se3(const CPose3D& p, const TPoint3D x_g)
    {
        CMatrixFixedNumeric<double, 3, 6> df_dse3;

        TPoint3D pp;
        p.inverseComposePoint(
            x_g.x, x_g.y, x_g.z, pp.x, pp.y, pp.z, nullptr, nullptr, &df_dse3);

        // Numerical approx:
        CMatrixFixedNumeric<double, 3, 6> num_df_dse3(UNINITIALIZED_MATRIX);
        {
            CArrayDouble<6> x_mean;
            for (int i = 0; i < 6; i++) x_mean[i] = 0;

            CArrayDouble<3> P;
            for (int i = 0; i < 3; i++) P[i] = pp[i];

            CArrayDouble<6> x_incrs;
            x_incrs.assign(1e-9);
            mrpt::math::estimateJacobian(
                x_mean,
                std::function<void(
                    const CArrayDouble<6>& x, const CArrayDouble<3>& P,
                    CArrayDouble<3>& Y)>(&func_invcompose_point_se3),
                x_incrs, P, num_df_dse3);
        }

        EXPECT_NEAR(0, (df_dse3 - num_df_dse3).array().abs().sum(), 3e-3)
            << "p: " << p << endl
            << "x_g:  " << x_g << endl
            << "Numeric approximation of df_dse3: " << endl
            << num_df_dse3 << endl
            << "Implemented method: " << endl
            << df_dse3 << endl
            << "Error: " << endl
            << df_dse3 - num_df_dse3 << endl;
    }

    static void func_jacob_expe_e(
        const CArrayDouble<6>& x, const double& dummy, CArrayDouble<12>& Y)
    {
        MRPT_UNUSED_PARAM(dummy);
        const CPose3D p = CPose3D::exp(x);
        // const CMatrixDouble44 R =
        // p.getHomogeneousMatrixVal<CMatrixDouble44>();
        p.getAs12Vector(Y);
    }

    // Check dexp(e)_de
    void check_jacob_expe_e_at_0()
    {
        CArrayDouble<6> x_mean;
        for (int i = 0; i < 6; i++) x_mean[i] = 0;

        double dummy = 0.;
        CArrayDouble<6> x_incrs;
        x_incrs.assign(1e-9);
        CMatrixDouble numJacobs;
        mrpt::math::estimateJacobian(
            x_mean,
            std::function<void(
                const CArrayDouble<6>& x, const double& dummy,
                CArrayDouble<12>& Y)>(&func_jacob_expe_e),
            x_incrs, dummy, numJacobs);

        // Theoretical matrix:
        // [ 0   -[e1]_x ]
        // [ 0   -[e2]_x ]
        // [ 0   -[e3]_x ]
        // [ I_3    0    ]
        double vals[12 * 6] = {
            0, 0, 0, 0, 0, 0,  0, 0, 0, 0,  0, 1, 0, 0, 0, 0, -1, 0,

            0, 0, 0, 0, 0, -1, 0, 0, 0, 0,  0, 0, 0, 0, 0, 1, 0,  0,

            0, 0, 0, 0, 1, 0,  0, 0, 0, -1, 0, 0, 0, 0, 0, 0, 0,  0,

            1, 0, 0, 0, 0, 0,  0, 1, 0, 0,  0, 0, 0, 0, 1, 0, 0,  0};
        CMatrixFixedNumeric<double, 12, 6> M(vals);

        EXPECT_NEAR((numJacobs - M).array().abs().maxCoeff(), 0, 1e-5)
            << "M:\n"
            << M << "numJacobs:\n"
            << numJacobs << "\n";
    }

    static void func_jacob_LnT_T(
        const CArrayDouble<12>& x, const double& dummy, CArrayDouble<6>& Y)
    {
        MRPT_UNUSED_PARAM(dummy);
        CPose3D p;

        p.setFrom12Vector(x);
        // ensure 3x3 rot vector is orthonormal (Sophus complains otherwise):
        auto R = p.getRotationMatrix();
        const auto Rsvd =
            R.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
        R = Rsvd.matrixU() * Rsvd.matrixV().transpose();
        p.setRotationMatrix(R);

        Y = p.ln();
    }

    // Jacobian of Ln(T) wrt T
    void check_jacob_LnT_T(
        double x1, double y1, double z1, double yaw1, double pitch1,
        double roll1)
    {
        const CPose3D p(x1, y1, z1, yaw1, pitch1, roll1);

        CMatrixFixedNumeric<double, 6, 12> theor_jacob;
        p.ln_jacob(theor_jacob);

        CMatrixDouble numJacobs;
        {
            CArrayDouble<12> x_mean;
            p.getAs12Vector(x_mean);

            double dummy = 0.;
            CArrayDouble<12> x_incrs;
            x_incrs.assign(1e-6);
            mrpt::math::estimateJacobian(
                x_mean,
                std::function<void(
                    const CArrayDouble<12>& x, const double& dummy,
                    CArrayDouble<6>& Y)>(&func_jacob_LnT_T),
                x_incrs, dummy, numJacobs);
        }

        EXPECT_NEAR((numJacobs - theor_jacob).array().abs().sum(), 0, 1e-3)
            << "Pose: " << p << endl
            << "Pose matrix:\n"
            << p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
            << numJacobs << endl
            << "Theor. Jacob:\n"
            << theor_jacob << endl
            << "ERR:\n"
            << theor_jacob - numJacobs << endl;
    }

    // tech report:
    //
    static void func_jacob_expe_D(
        const CArrayDouble<6>& eps, const CPose3D& D, CArrayDouble<12>& Y)
    {
        CPose3D incr;
        CPose3D::exp(eps, incr);
        const CPose3D expe_D = incr + D;
        expe_D.getAs12Vector(Y);
    }

    // Test Jacobian: d exp(e)*D / d e
    // 10.3.3 in tech report
    void test_Jacob_dexpeD_de(
        double x1, double y1, double z1, double yaw1, double pitch1,
        double roll1)
    {
        const CPose3D p(x1, y1, z1, yaw1, pitch1, roll1);

        Eigen::Matrix<double, 12, 6> theor_jacob;
        CPose3D::jacob_dexpeD_de(p, theor_jacob);

        CMatrixDouble numJacobs;
        {
            CArrayDouble<6> x_mean;
            x_mean.setZero();

            CArrayDouble<6> x_incrs;
            x_incrs.assign(1e-6);
            mrpt::math::estimateJacobian(
                x_mean,
                std::function<void(
                    const CArrayDouble<6>& eps, const CPose3D& D,
                    CArrayDouble<12>& Y)>(&func_jacob_expe_D),
                x_incrs, p, numJacobs);
        }

        EXPECT_NEAR((numJacobs - theor_jacob).array().abs().maxCoeff(), 0, 1e-3)
            << "Pose: " << p << endl
            << "Pose matrix:\n"
            << p.getHomogeneousMatrixVal<CMatrixDouble44>() << "Num. Jacob:\n"
            << numJacobs << endl
            << "Theor. Jacob:\n"
            << theor_jacob << endl
            << "ERR:\n"
            << theor_jacob - numJacobs << endl;
    }

    struct TParams_func_jacob_Aexpe_D
    {
        CPose3D A, D;
    };

    static void func_jacob_Aexpe_D(
        const CArrayDouble<6>& eps, const TParams_func_jacob_Aexpe_D& params,
        CArrayDouble<12>& Y)
    {
        CPose3D incr;
        CPose3D::exp(eps, incr);
        const CPose3D res = params.A + incr + params.D;
        res.getAs12Vector(Y);
    }

    // Test Jacobian: d A*exp(e)*D / d e
    // 10.3.7 in tech report
    // http://ingmec.ual.es/~jlblanco/papers/jlblanco2010geometry3D_techrep.pdf
    void test_Jacob_dAexpeD_de(
        double x1, double y1, double z1, double yaw1, double pitch1,
        double roll1, double x2, double y2, double z2, double yaw2,
        double pitch2, double roll2)
    {
        const CPose3D A(x1, y1, z1, yaw1, pitch1, roll1);
        const CPose3D D(x2, y2, z2, yaw2, pitch2, roll2);

        Eigen::Matrix<double, 12, 6> theor_jacob;
        CPose3D::jacob_dAexpeD_de(A, D, theor_jacob);

        CMatrixDouble numJacobs;
        {
            CArrayDouble<6> x_mean;
            x_mean.setZero();

            TParams_func_jacob_Aexpe_D params;
            params.A = A;
            params.D = D;
            CArrayDouble<6> x_incrs;
            x_incrs.assign(1e-6);
            mrpt::math::estimateJacobian(
                x_mean,
                std::function<void(
                    const CArrayDouble<6>& eps,
                    const TParams_func_jacob_Aexpe_D& params,
                    CArrayDouble<12>& Y)>(&func_jacob_Aexpe_D),
                x_incrs, params, numJacobs);
        }

        EXPECT_NEAR((numJacobs - theor_jacob).array().abs().maxCoeff(), 0, 1e-3)
            << "Pose A: " << A << endl
            << "Pose D: " << D << endl
            << "Num. Jacob:\n"
            << numJacobs << endl
            << "Theor. Jacob:\n"
            << theor_jacob << endl
            << "ERR:\n"
            << theor_jacob - numJacobs << endl;
    }
};

// Elemental tests:
TEST_F(Pose3DTests, DefaultValues)
{
    {
        CPose3D p;
        test_default_values(p, "Default");
    }
    {
        CPose3D p2;
        CPose3D p = p2;
        test_default_values(p, "p=p2");
    }
    {
        CPose3D p1, p2;
        test_default_values(p1 + p2, "p1+p2");
        CPose3D p = p1 + p2;
        test_default_values(p, "p=p1+p2");
    }
    {
        CPose3D p1, p2;
        CPose3D p = p1 - p2;
        test_default_values(p, "p1-p2");
    }
}

TEST_F(Pose3DTests, Initialization)
{
    CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
    EXPECT_NEAR(p.x(), 1, 1e-7);
    EXPECT_NEAR(p.y(), 2, 1e-7);
    EXPECT_NEAR(p.z(), 3, 1e-7);
    EXPECT_NEAR(p.yaw(), 0.2, 1e-7);
    EXPECT_NEAR(p.pitch(), 0.3, 1e-7);
    EXPECT_NEAR(p.roll(), 0.4, 1e-7);
}

TEST_F(Pose3DTests, OperatorBracket)
{
    CPose3D p(1, 2, 3, 0.2, 0.3, 0.4);
    EXPECT_NEAR(p[0], 1, 1e-7);
    EXPECT_NEAR(p[1], 2, 1e-7);
    EXPECT_NEAR(p[2], 3, 1e-7);
    EXPECT_NEAR(p[3], 0.2, 1e-7);
    EXPECT_NEAR(p[4], 0.3, 1e-7);
    EXPECT_NEAR(p[5], 0.4, 1e-7);
}

// List of "random" poses to test with (x,y,z,yaw,pitch,roll) (angles in
// degrees)
const double ptc[][6] = {  // ptc = poses_test_cases
    {.0, .0, .0, .0, .0, .0},
    {1.0, 2.0, 3.0, .0, .0, .0},
    {1.0, 2.0, 3.0, 10.0, .0, .0},
    {1.0, 2.0, 3.0, .0, 1.0, .0},
    {1.0, 2.0, 3.0, .0, .0, 1.0},
    {1.0, 2.0, 3.0, 80.0, 5.0, 5.0},
    {1.0, 2.0, 3.0, -20.0, -30.0, -40.0},
    {1.0, 2.0, 3.0, -45.0, 10.0, 70.0},
    {1.0, 2.0, 3.0, 40.0, -5.0, 25.0},
    {1.0, 2.0, 3.0, 40.0, 20.0, -15.0},
    {-6.0, 2.0, 3.0, 40.0, 20.0, 15.0},
    {6.0, -5.0, 3.0, 40.0, 20.0, 15.0},
    {6.0, 2.0, -9.0, 40.0, 20.0, 15.0},
    {0.0, 8.0, 5.0, -45.0, 10.0, 70.0},
    {1.0, 0.0, 5.0, -45.0, 10.0, 70.0},
    {1.0, 8.0, 0.0, -45.0, 10.0, 70.0}};
const size_t num_ptc = sizeof(ptc) / sizeof(ptc[0]);

// More complex tests:
TEST_F(Pose3DTests, InverseHM)
{
    for (size_t i = 0; i < num_ptc; i++)
        test_inverse(
            ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
            DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]));
}

TEST_F(Pose3DTests, Compose)
{
    for (size_t i = 0; i < num_ptc; i++)
        for (size_t j = 0; j < num_ptc; j++)
            test_compose(
                ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
                DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), ptc[j][0], ptc[j][1],
                ptc[j][2], DEG2RAD(ptc[j][3]), DEG2RAD(ptc[j][4]),
                DEG2RAD(ptc[j][5]));
}
TEST_F(Pose3DTests, composeFrom)
{
    for (size_t i = 0; i < num_ptc; i++)
        for (size_t j = 0; j < num_ptc; j++)
            test_composeFrom(
                ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
                DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), ptc[j][0], ptc[j][1],
                ptc[j][2], DEG2RAD(ptc[j][3]), DEG2RAD(ptc[j][4]),
                DEG2RAD(ptc[j][5]));
}

TEST_F(Pose3DTests, ToFromCPose2D)
{
    for (size_t i = 0; i < num_ptc; i++)
        test_to_from_2d(ptc[i][0], ptc[i][1], DEG2RAD(ptc[i][3]));
}

TEST_F(Pose3DTests, ComposeAndInvComposeWithPoint)
{
    for (size_t i = 0; i < num_ptc; i++)
    {
        test_composePoint(
            ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
            DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), 10, 11, 12);
        test_composePoint(
            ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
            DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), -5, 1, 2);
        test_composePoint(
            ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
            DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), 5, -1, 2);
        test_composePoint(
            ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
            DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), 5, 1, -2);
    }
}

TEST_F(Pose3DTests, ComposePointJacob)
{
    for (size_t i = 0; i < num_ptc; i++)
    {
        test_composePointJacob(
            ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
            DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), 10, 11, 12);
        test_composePointJacob(
            ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
            DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), -5, 1, 2);
    }
}

TEST_F(Pose3DTests, ComposePointJacobApprox)
{  // Test approximated Jacobians for very small rotations
    test_composePointJacob(
        1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0), 10, 11, 12, true);
    test_composePointJacob(
        1.0, 2.0, 3.0, DEG2RAD(0.1), DEG2RAD(0), DEG2RAD(0), 10, 11, 12, true);
    test_composePointJacob(
        1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0.1), DEG2RAD(0), 10, 11, 12, true);
    test_composePointJacob(
        1.0, 2.0, 3.0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0.1), 10, 11, 12, true);
}

TEST_F(Pose3DTests, InvComposePointJacob)
{
    for (size_t i = 0; i < num_ptc; i++)
    {
        test_invComposePointJacob(
            ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
            DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), 10, 11, 12);
        test_invComposePointJacob(
            ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
            DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), -5, 1, 2);
        test_invComposePointJacob(
            ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
            DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), 5, -1, 2);
        test_invComposePointJacob(
            ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
            DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), 5, 1, -2);
    }
}

TEST_F(Pose3DTests, ComposePointJacob_se3)
{
    for (size_t i = 0; i < num_ptc; i++)
    {
        test_composePointJacob_se3(
            CPose3D(
                ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
                DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5])),
            TPoint3D(0, 0, 0));
        test_composePointJacob_se3(
            CPose3D(
                ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
                DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5])),
            TPoint3D(10, 11, 12));
        test_composePointJacob_se3(
            CPose3D(
                ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
                DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5])),
            TPoint3D(-5.0, -15.0, 8.0));
    }
}
TEST_F(Pose3DTests, InvComposePointJacob_se3)
{
    for (size_t i = 0; i < num_ptc; i++)
    {
        test_invComposePointJacob_se3(
            CPose3D(
                ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
                DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5])),
            TPoint3D(0, 0, 0));
        test_invComposePointJacob_se3(
            CPose3D(
                ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
                DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5])),
            TPoint3D(10, 11, 12));
        test_invComposePointJacob_se3(
            CPose3D(
                ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
                DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5])),
            TPoint3D(-5.0, -15.0, 8.0));
    }
}

TEST_F(Pose3DTests, ExpLnEqual)
{
    for (size_t i = 0; i < num_ptc; i++)
        test_ExpLnEqual(
            ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
            DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]));
}

TEST_F(Pose3DTests, Jacob_dExpe_de_at_0) { check_jacob_expe_e_at_0(); }
TEST_F(Pose3DTests, Jacob_dLnT_dT)
{
    check_jacob_LnT_T(0, 0, 0, DEG2RAD(0), DEG2RAD(0), DEG2RAD(0));
    // JL NOTE:
    //  This function cannot be properly tested numerically, since the logm()
    //  implementation
    //  is not generic and does NOT depends on all matrix entries, thus the
    //  numerical Jacobian
    //  contains entire columns of zeros, even if the theorethical doesn't.
    //  check_jacob_LnT_T(1.0,0,0, DEG2RAD(0),DEG2RAD(0),DEG2RAD(0) ); ...
}

TEST_F(Pose3DTests, Jacob_dexpeD_de)
{
    for (size_t i = 0; i < num_ptc; i++)
        test_Jacob_dexpeD_de(
            ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
            DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]));
}

TEST_F(Pose3DTests, Jacob_dAexpeD_de)
{
    for (size_t i = 0; i < num_ptc; i++)
        for (size_t j = 0; j < num_ptc; j++)
            test_Jacob_dAexpeD_de(
                ptc[i][0], ptc[i][1], ptc[i][2], DEG2RAD(ptc[i][3]),
                DEG2RAD(ptc[i][4]), DEG2RAD(ptc[i][5]), ptc[j][0], ptc[j][1],
                ptc[j][2], DEG2RAD(ptc[j][3]), DEG2RAD(ptc[j][4]),
                DEG2RAD(ptc[j][5]));
}