matrix_ops1_unittest.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, 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 |
   +------------------------------------------------------------------------+ */

// Note: Matrices unit tests have been split in different files since
// building them with eigen3 eats a lot of RAM and may be a problem while
// compiling in small systems.

#include <mrpt/math/CMatrixFixedNumeric.h>
#include <mrpt/math/CMatrixD.h>
#include <mrpt/math/matrix_serialization.h>  // serialization of matrices
#include <mrpt/math/ops_matrices.h>
#include <mrpt/utils/metaprogramming.h>
#include <mrpt/utils/CMemoryStream.h>
#include <mrpt/random.h>
#include <gtest/gtest.h>

using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::math;
using namespace mrpt::random;
using namespace mrpt::utils::metaprogramming;
using namespace std;

const double dat_A[] = {4, 5, 8, -2, 1, 3};
const double dat_B[] = {2, 6, 9, 8};
const double dat_Cok[] = {53, 64, -2, 32, 29, 30};

#define CHECK_AND_RET_ERROR(_COND_, _MSG_) EXPECT_FALSE(_COND_) << _MSG_;

TEST(Matrices, A_times_B_dyn)
{
        // Dyn. size, double.
        CMatrixDouble A(3, 2, dat_A);
        CMatrixDouble B(2, 2, dat_B);
        CMatrixDouble C = A * B;
        CMatrixDouble C_ok(3, 2, dat_Cok);
        CMatrixDouble err = C - C_ok;
        EXPECT_NEAR(0, fabs(err.sum()), 1e-5) << "A:   " << A << "B:   " << B
                                                                                  << "A*B: " << C << endl;
}

TEST(Matrices, A_times_B_fix)
{
        // Fix. size, double.
        CMatrixFixedNumeric<double, 3, 2> A(dat_A);
        CMatrixFixedNumeric<double, 2, 2> B(dat_B);
        CMatrixFixedNumeric<double, 3, 2> C, C_ok(dat_Cok), Err;

        C = A * B;
        Err = C - CMatrixFixedNumeric<double, 3, 2>(C_ok);

        EXPECT_NEAR(0, fabs(Err.sum()), 1e-5) << "A:   " << A << "B:   " << B
                                                                                  << "A*B: " << C << endl;
}

TEST(Matrices, SerializeCMatrixD)
{
        CMatrixDouble A(3, 2, dat_A);
        CMatrixFixedNumeric<double, 3, 2> fA;

        CMatrixD As = A;

        CMemoryStream membuf;
        membuf << As;
        membuf.Seek(0);
        membuf >> fA;

        EXPECT_NEAR(0, fabs((CMatrixDouble(fA) - A).sum()), 1e-9);

        try
        {
                // Now, if we try to de-serialize into the wrong type, we should get an
                // exception:
                membuf.Seek(0);
                CMatrixFixedNumeric<double, 2, 2> fB;
                membuf >> fB;  // Wrong size!

                GTEST_FAIL() << "Exception not launched when it was expected!";
        }
        catch (...)
        {  // OK, exception occurred, as expected
        }
}

TEST(Matrices, EigenVal2x2dyn)
{
        const double dat_C1[] = {14.6271, 5.8133, 5.8133, 16.8805};
        CMatrixDouble C1(2, 2, dat_C1);

        Eigen::MatrixXd C1_V, C1_D;
        C1.eigenVectors(C1_V, C1_D);

        CMatrixDouble C1_RR = C1_V * C1_D * C1_V.transpose();
        EXPECT_NEAR((C1_RR - C1).array().abs().sum(), 0, 1e-4);
}

TEST(Matrices, EigenVal3x3dyn)
{
        const double dat_C1[] = {8, 6, 1, 6, 9, 4, 1, 4, 10};
        CMatrixDouble C1(3, 3, dat_C1);

        Eigen::MatrixXd C1_V, C1_D;
        C1.eigenVectors(C1_V, C1_D);

        CMatrixDouble C1_RR = C1_V * C1_D * C1_V.transpose();
        EXPECT_NEAR((C1_RR - C1).array().abs().sum(), 0, 1e-4);
}

TEST(Matrices, EigenVal2x2fix)
{
        const double dat_C1[] = {14.6271, 5.8133, 5.8133, 16.8805};
        CMatrixDouble22 C1(dat_C1);

        Eigen::Matrix2d C1_V, C1_D;
        C1.eigenVectors(C1_V, C1_D);

        CMatrixDouble22 C1_RR = C1_V * C1_D * (~C1_V);
        EXPECT_NEAR((C1_RR - C1).array().abs().sum(), 0, 1e-4);
}

TEST(Matrices, EigenVal3x3fix)
{
        const double dat_C1[] = {8, 6, 1, 6, 9, 4, 1, 4, 10};
        CMatrixDouble33 C1(dat_C1);

        CMatrixDouble33 C1_V, C1_D;
        C1.eigenVectors(C1_V, C1_D);

        CMatrixDouble33 C1_RR = C1_V * C1_D * (~C1_V);
        EXPECT_NEAR((C1_RR - C1).array().abs().sum(), 0, 1e-4);
}

#if 0  // JL: Disabled since it fails in some PPA build servers. Reported to
           // Eigen list for possible fixes...

// Compare the two ways of computing matrix eigenvectors: generic & for symmetric matrices:
TEST(Matrices,EigenVal4x4_sym_vs_generic)
{
        const double   dat_C1[] = {
                13.737245,10.248641,-5.839599,11.108320,
                10.248641,14.966139,-5.259922,11.662222,
                -5.839599,-5.259922,9.608822,-4.342505,
                11.108320,11.662222,-4.342505,12.121940 };
        const CMatrixDouble44  C1(dat_C1);

        CMatrixDouble44 eigvecs_sym, eigvecs_gen, eigvals_symM, eigvals_genM;
        CVectorDouble   eigvals_sym, eigvals_gen;

        C1.eigenVectorsVec(eigvecs_gen,eigvals_gen);
        C1.eigenVectorsSymmetricVec(eigvecs_sym,eigvals_sym);

        eigvals_symM.setZero();eigvals_symM.diagonal() = eigvals_sym;
        eigvals_genM.setZero();eigvals_genM.diagonal() = eigvals_gen;

        EXPECT_NEAR( (C1-eigvecs_gen*eigvals_genM*(~eigvecs_gen)).array().abs().sum(),0,1e-5)
                << endl << endl
                << "eigvecs_gen*eigvals_gen*(~eigvecs_gen):\n" << eigvecs_gen*eigvals_genM*(~eigvecs_gen) << endl
                << "C1:\n" << C1 << endl
                << "eigvals_sym:\n" <<  eigvals_sym << endl
                << "eigvals_gen:\n" << eigvals_gen << endl
                << "eigvals_symM:\n" <<  eigvals_symM << endl
                << "eigvals_genM:\n" << eigvals_genM << endl
                << "eigvecs_gen:\n" << eigvecs_gen << endl
                << "eigvecs_sym:\n" << eigvecs_sym << endl<< endl;

        EXPECT_NEAR( (C1-eigvecs_sym*eigvals_symM*(~eigvecs_sym)).array().abs().sum(),0,1e-5)
                << endl << endl
                << "eigvecs_sym*eigvals_sym*(~eigvecs_sym):\n" << eigvecs_sym*eigvals_symM*(~eigvecs_sym) << endl
                << "C1:\n" << C1 << endl;

        EXPECT_NEAR( (eigvals_gen-eigvals_sym).array().abs().sum(),0,1e-5)
                << endl << endl
                << "eigvals_gen:\n" << eigvals_gen<< endl
                << "eigvals_sym:\n" << eigvals_sym << endl;
}
#endif