matrix_ops1_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 |
   +------------------------------------------------------------------------+ */

// 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/serialization/CArchive.h>
#include <mrpt/io/CMemoryStream.h>
#include <mrpt/random.h>
#include <gtest/gtest.h>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::random;
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;

    mrpt::io::CMemoryStream membuf;
    auto arch = mrpt::serialization::archiveFrom(membuf);
    arch << As;
    membuf.Seek(0);
    arch >> 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;
        arch >> 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.transpose();
    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.transpose();
    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